public TrackerMagnitude(WebCamTracks tracks)
{
//Remove all tracks except the largest, v.simple fitering
//Thought about adding an interface for all trackers, but can't be assed
if (tracks.TrackedPoints.Count > 0)
{
int maxCount = 0;
int maxIndex = -1;
for (int i = 0; i < tracks.TrackedPoints.Count; i++)
{
if (tracks.TrackedPoints[i].Points.Count > maxCount)
{
maxCount = tracks.TrackedPoints[i].Points.Count;
maxIndex = i;
}
}
WebCamTrack maxCluster = tracks.TrackedPoints[maxIndex];
FilteredTracks.TrackedPoints.Add(maxCluster);
}
}
private void drawRays(RegisteredCamera thisCamera, int i)
{
//for each tracked point project from the camera centre to the point:
float aspect = GraphicsDevice.Viewport.AspectRatio;
effect.World = Matrix.Identity * Matrix.CreateTranslation(viewTranslationVector);
//effect.World = effect.World * Matrix.CreateTranslation(thisCamera.PositionVector);
//effect.World = effect.World * Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity);
//effect.World = effect.World * Matrix.CreateScale(globalScaling);
//effect.View = Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity) * Matrix.CreateLookAt(new Vector3(0, 0, -5 * globalScaling),
// Vector3.Zero, Vector3.Up);
effect.View = ViewMatrix;
effect.Projection = Matrix.CreatePerspectiveFieldOfView(1, aspect, 0.01f, 100);
//for (int i = 0; i < thisCamera.TrackedPoints.Count; i++)
//{
//int i = (int)rotAngleX;
//if (i < thisCamera.TrackedPoints.Count)
//{
//TrackedImagePoint thisPoint = thisCamera.TrackedPoints[i];
int testy = 0;
//if (WebCamPermute != null)
//{
//int thisCamIndex = WebCamPermute[i];
int thisCamIndex = i;
if (thisCamIndex < WebCamsEye.Count)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints != null)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints != null)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints.Count > 0)
{
//if (WebCamPermute.Count > 0)
// {
try
{
for (int r = 0; r < WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints.Count; r++)
{
WebCamTrack thisTrack = WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints[r];
if (thisTrack.Points.Count > 0)
{
Vector3 WordCoordinates = thisCamera.ImageToWorld(thisTrack.Points.Select(x => x.X).Average(), thisTrack.Points.Select(x => x.Y).Average(), -100, transposeRot, negateRot, invertRot, invertZ, rotAngleX, rotAngleY, rotAngleZ);
//Vector3 WordCoordinates = thisCamera.ImageToWorld(thisTrack.Points[0].X, thisTrack.Points[0].Y, -100, transposeRot, negateRot, invertRot, invertZ, rotAngleX, rotAngleY, rotAngleZ);
Vector3 startPoint = new Vector3(thisCamera.PositionVector.X, thisCamera.PositionVector.Y, thisCamera.PositionVector.Z);
Vector3 endPoint = new Vector3(WordCoordinates.X, (float)WordCoordinates.Y, (float)WordCoordinates.Z);
//trackInCamera.Add(thisCamIndex);
IntersectionLines.Add(new Line3D {
LineStart = new Point3D(startPoint.X, startPoint.Y, startPoint.Z), LineEnd = new Point3D(endPoint.X, endPoint.Y, endPoint.Z)
});
effect.CurrentTechnique.Passes[0].Apply();
var vertices = new[] { new VertexPositionColor(startPoint, Microsoft.Xna.Framework.Color.Red), new VertexPositionColor(endPoint, Microsoft.Xna.Framework.Color.Red) };
effect.GraphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, vertices, 0, 1);
}
else
{
//trackInCamera[thisCamIndex] = false;
}
}
}
catch (Exception test)
{
testy = 1;
}
return;
//}
}
else
{
//trackInCamera[thisCamIndex] = false;
}
}
}
//}
//IntersectionLines[thisCamIndex].Displ
}
}
void Cam_NewFrame(object sender, NewFrameEventArgs eventArgs)
{
Bitmap img = (Bitmap)eventArgs.Frame.Clone();
//Graphics thisGraphics = Graphics.FromImage(img);
//Pen thisPen = new Pen(Color.Red);
//Remove first frame from all subsequent ones:
//Update the appropriate coordinates/images
//Bitmap background = new Bitmap(img.Width, img.Height);
//for (int it = 0; it < videoDevices.Count; it++)
//{
// var inneri = it;
// if (videoDevices[inneri] != null)
// {
// if (((VideoCaptureDevice)sender).Source.Equals(videoDevices[inneri].MonikerString))
// {
// if (firstFrame[inneri])
// {
// firstFrame[inneri] = false;
// initImages[inneri] = img;
// return;
// }
// background = initImages[inneri];
// }
// }
//}
//BitmapData backgroundData = background.LockBits(new Rectangle(0, 0, background.Width, background.Height), ImageLockMode.ReadWrite, background.PixelFormat);
BitmapData bmd = img.LockBits(new Rectangle(0, 0, img.Width, img.Height), ImageLockMode.ReadWrite, img.PixelFormat);
List <int> thresholdxs = new List <int>();
List <int> thresholdys = new List <int>();
unsafe
{
int PixelSize = 3;
for (int y = 0; y < bmd.Height; y++)
{
byte *row = (byte *)bmd.Scan0 + (y * bmd.Stride);
//byte* backgroundRow = (byte*)backgroundData.Scan0 + (y * backgroundData.Stride);
for (int x = 0; x < bmd.Width; x++)
{
if (thresholdxs.Count < 10000)
{
if (row[x * PixelSize] > thresMag && row[(x * PixelSize) + 1] > thresMag && row[(x * PixelSize) + 2] > thresMag)
{
thresholdxs.Add(x);
thresholdys.Add(y);
}
}
}
}
}
img.UnlockBits(bmd);
img.Dispose();
//background.UnlockBits(backgroundData);
WebCamTracks Clusters = new WebCamTracks();
int numClusters = 0;
//Remove points which are all alone
while (thresholdxs.Count > 0)
{
WebCamTrack newCluster = new WebCamTrack();
newCluster.Points.Add(new Point2D {
X = thresholdxs[0], Y = thresholdys[0], Magnitude = 0
});
int innercount = 1;
while (innercount < thresholdxs.Count)
{
double xdeltasq = Math.Pow(thresholdxs[0] - thresholdxs[innercount], 2);
double ydeltasq = Math.Pow(thresholdys[0] - thresholdys[innercount], 2);
double distance = Math.Sqrt(xdeltasq + ydeltasq);
if (distance < 25)
{
newCluster.Points.Add(new Point2D {
X = thresholdxs[innercount], Y = thresholdys[innercount], Magnitude = 0
});
thresholdxs.RemoveAt(innercount);
thresholdys.RemoveAt(innercount);
}
else
{
innercount++;
}
}
thresholdxs.RemoveAt(0);
thresholdys.RemoveAt(0);
//if(newCluster.points
Clusters.TrackedPoints.Add(newCluster);
}
//for (int i = 0; i < indexesToRemove.Count; i++)
//{
// thresholdxs.RemoveAt(indexesToRemove[indexesToRemove.Count - i - 1]);
// thresholdys.RemoveAt(indexesToRemove[indexesToRemove.Count - i - 1]);
//}
////Remove naughty clusters
//List<int> indexsToRemove = new List<int>();
//for (int p = 0; p < clustersToRemove.Count; p++)
//{
// for (int q = 0; q < Clusters.Count; q++)
// {
// float clustcentroidx = (float)Clusters[q].points.Select(x => x.x).Sum() / (float)Clusters[q].points.Count;
// float clustcentroidy = (float)Clusters[q].points.Select(x => x.y).Sum() / (float)Clusters[q].points.Count;
// double xdeltasq = Math.Pow(clustersToRemove[p].x - clustcentroidx, 2);
// double ydeltasq = Math.Pow(clustersToRemove[p].y - clustcentroidy, 2);
// double distance = Math.Sqrt(xdeltasq + ydeltasq);
// if (distance < 50)
// {
// indexsToRemove.Add(q);
// }
// }
//}
//indexsToRemove = indexsToRemove.GroupBy(x => x).Select(y => y.First()).ToList();
//indexsToRemove.Sort();
//for (int p = 0; p < indexsToRemove.Count; p++)
//{
// Clusters.RemoveAt(indexsToRemove[indexsToRemove.Count - p - 1]);
//}
//find the largest cluster:
double lcentroidx = -1;
double lcentroidy = -1;
//Apply the filter:
TrackerMagnitude newTracker = new TrackerMagnitude(Clusters);
//if (newTracker.FilteredTracks.TrackedPoints.Count > 0)
//{
if (Clusters.TrackedPoints.Count > 0)
{
int maxCount = 0;
int maxIndex = -1;
FilteredTrackedPoints = Clusters;
//FilteredTrackedPoints = newTracker.FilteredTracks;
//for (int i = 0; i < Clusters.TrackedPoints.Count; i++)
//{
// if (Clusters.TrackedPoints[i].Points.Count > maxCount)
// {
// maxCount = Clusters.TrackedPoints[i].Points.Count;
// maxIndex = i;
// }
//}
//WebCamTrack maxCluster = Clusters.TrackedPoints[maxIndex];
//if (maxCluster.Points.Count > 1)
//{
// lcentroidx = (float)maxCluster.Points.Select(x => x.X).Sum() / (float)maxCluster.Points.Count;
// lcentroidy = (float)maxCluster.Points.Select(x => x.Y).Sum() / (float)maxCluster.Points.Count;
//}
//if (lcentroidx != -1 && lcentroidy != -1)
//{
// //float centroidx = (float)lcentroidx;
// //float centroidy = (float)lcentroidy;
// //for (int i = 0; i < videoDevices.Count; i++)
// //{
// // if (videoDevices[i] != null)
// // {
// // if (((VideoCaptureDevice)sender).Source.Equals(videoDevices[i].MonikerString))
// // {
// // spinningTriangleControl.cameraConfig.Cameras[i].TrackedPoints.Clear();
// // spinningTriangleControl.cameraConfig.Cameras[i].TrackedPoints.Add(new TrackedImagePoint(640 - centroidx, centroidy));
// // }
// // }
// //}
//}
//else
//{
// //for (int i = 0; i < videoDevices.Count; i++)
// //{
// // if (videoDevices[i] != null)
// // {
// // if (((VideoCaptureDevice)sender).Source.Equals(videoDevices[i].MonikerString))
// // {
// // spinningTriangleControl.cameraConfig.Cameras[i].TrackedPoints.Clear();
// // //spinningTriangleControl.cameraConfig.Cameras[i].TrackedPoints.Add(new TrackedImagePoint(centroidx, centroidy));
// // }
// // }
// //}
//}
}
else
{
FilteredTrackedPoints.TrackedPoints.Clear();
}
////Update the appropriate coordinates/images
//for (int it = 0; it < videoDevices.Count; it++)
//{
// var inneri = it;
// if (videoDevices[inneri] != null)
// {
// if (((VideoCaptureDevice)sender).Source.Equals(videoDevices[inneri].MonikerString))
// {
// counters[inneri]++;
// this.BeginInvoke(
// (MethodInvoker)delegate()
// {
// ((TextBox)(this.Controls.Find("txtbox" + inneri, true).First())).Text = lcentroidx.ToString() + ", " + lcentroidy.ToString();
// ((PictureBox)(this.Controls.Find("pictureBox" + inneri, true).First())).Image = img;
// //pictureBox1.Image = img;
// });
// }
// }
//}
//thisGraphics.Dispose();
//pictureBox1.Image = img;
}
// capture thread
void Capture(object obj)
{
_threadRunning = true;
//Random rng = new Random();
CLEyeCameraStart(_camera);
while (_threadRunning)
{
if (CLEyeCameraGetFrame(_camera, _ptrBmpPixels, 500))
{
numFrames++;
Bitmap bmpIn = new Bitmap(640, 480, 640, PixelFormat.Format8bppIndexed, _ptrBmpPixels);
ColorPalette GrayPalette = bmpIn.Palette;
for (int i = 0; i < GrayPalette.Entries.Length; i++)
{
GrayPalette.Entries[i] = Color.FromArgb(i, i, i);
}
bmpIn.Palette = GrayPalette;
Bitmap converted = new Bitmap(bmpIn.Width, bmpIn.Height, PixelFormat.Format24bppRgb);
using (Graphics g = Graphics.FromImage(converted))
{
// Prevent DPI conversion
g.PageUnit = GraphicsUnit.Pixel;
// Draw the image
g.DrawImageUnscaled(bmpIn, 0, 0);
}
List <int> thresholdxs = new List <int>();
List <int> thresholdys = new List <int>();
BitmapData bmd = ((Bitmap)converted).LockBits(new Rectangle(0, 0, converted.Width, converted.Height), ImageLockMode.ReadWrite, converted.PixelFormat);
if (EnableBackGroundSubtract)
{
//BitmapData bmdClone;
if (GrabBackground)
{
bClone = (Bitmap)converted.Clone();
//bmdClone = ((Bitmap)bClone).LockBits(new Rectangle(0, 0, bClone.Width, bClone.Height), ImageLockMode.ReadWrite, bClone.PixelFormat);
GrabBackground = false;
}
BitmapData bmdClone = ((Bitmap)bClone).LockBits(new Rectangle(0, 0, bClone.Width, bClone.Height), ImageLockMode.ReadWrite, bClone.PixelFormat);
unsafe
{
int PixelSize = 3;
for (int y = 0; y < bmd.Height; y++)
{
byte *row = (byte *)bmd.Scan0 + (y * bmd.Stride);
byte *rowClone = (byte *)bmdClone.Scan0 + (y * bmdClone.Stride);
//byte* backgroundRow = (byte*)backgroundData.Scan0 + (y * backgroundData.Stride);
for (int x = 0; x < bmd.Width; x++)
{
//If backgroundsubtraction is enabled:
if (EnableBackGroundSubtract)
{
row[x * PixelSize] = (byte)((int)row[x * PixelSize] - (int)rowClone[x * PixelSize]);
//row[x * PixelSize] = (byte)((int)rowClone[x * PixelSize] - (int)row[x * PixelSize]);
if (row[x * PixelSize] <= 0)
{
row[x * PixelSize] = 1;
}
row[(x * PixelSize) + 1] = (byte)((int)row[(x * PixelSize) + 1] - (int)rowClone[(x * PixelSize) + 1]);
//row[(x * PixelSize) + 1] = (byte)((int)rowClone[(x * PixelSize) + 1] - (int)row[(x * PixelSize) + 1]);
if (row[(x * PixelSize) + 1] <= 0)
{
row[(x * PixelSize) + 1] = 1;
}
row[(x * PixelSize) + 2] = (byte)((int)row[(x * PixelSize) + 2] - (int)rowClone[(x * PixelSize) + 2]);
//row[(x * PixelSize) + 2] = (byte)((int)rowClone[(x * PixelSize) + 2] - (int)row[(x * PixelSize) + 2]);
if (row[(x * PixelSize) + 2] <= 0)
{
row[(x * PixelSize) + 2] = 1;
}
}
if (thresholdxs.Count < 10000)
{
if (row[x * PixelSize] > thresMag)// && row[(x * PixelSize) + 1] > thresMag && row[(x * PixelSize) + 2] > thresMag)
{
//row[x * PixelSize] = 150;
//row[(x * PixelSize) + 1] = 0;
//row[(x * PixelSize) + 2] = 255;
thresholdxs.Add(640 - x);
thresholdys.Add(y);
}
}
}
}
}
bClone.UnlockBits(bmdClone);
}
else
{
unsafe
{
int PixelSize = 3;
for (int y = 0; y < bmd.Height; y++)
{
byte *row = (byte *)bmd.Scan0 + (y * bmd.Stride);
//byte* backgroundRow = (byte*)backgroundData.Scan0 + (y * backgroundData.Stride);
for (int x = 0; x < bmd.Width; x++)
{
if (thresholdxs.Count < 10000)
{
if (row[x * PixelSize] > thresMag)// && row[(x * PixelSize) + 1] > thresMag && row[(x * PixelSize) + 2] > thresMag)
{
//row[x * PixelSize] = 150;
//row[(x * PixelSize) + 1] = 0;
//row[(x * PixelSize) + 2] = 255;
if (IsCalibrateMode)
{
thresholdxs.Add(x);
thresholdys.Add(y);
}
else
{
thresholdxs.Add(640 - x);
thresholdys.Add(y);
}
}
}
}
}
}
}
converted.UnlockBits(bmd);
if (multiPoint)
{
WebCamTracks Clusters = new WebCamTracks();
int numClusters = 0;
//Remove points which are all alone
while (thresholdxs.Count > 0)
{
WebCamTrack newCluster = new WebCamTrack();
newCluster.Points.Add(new Point2D {
X = thresholdxs[0], Y = thresholdys[0], Magnitude = 0
});
int innercount = 1;
while (innercount < thresholdxs.Count)
{
double xdeltasq = Math.Pow(thresholdxs[0] - thresholdxs[innercount], 2);
double ydeltasq = Math.Pow(thresholdys[0] - thresholdys[innercount], 2);
double distance = Math.Sqrt(xdeltasq + ydeltasq);
if (distance < ClusterDistance)
{
newCluster.Points.Add(new Point2D {
X = thresholdxs[innercount], Y = thresholdys[innercount], Magnitude = 0
});
thresholdxs.RemoveAt(innercount);
thresholdys.RemoveAt(innercount);
}
else
{
innercount++;
}
}
thresholdxs.RemoveAt(0);
thresholdys.RemoveAt(0);
Clusters.TrackedPoints.Add(newCluster);
}
Graphics thisGraphics = Graphics.FromImage(converted);
Pen thisPen = new Pen(Color.Red);
for (int i = 0; i < Clusters.TrackedPoints.Count; i++)
{
thisGraphics.DrawRectangle(thisPen, ((float)Clusters.TrackedPoints[i].GetCentroid().X - (float)8), (float)(float)Clusters.TrackedPoints[i].GetCentroid().Y - 8, 16, 16);
}
FilteredTrackedPoints = Clusters;
}
else
{
thisGraphics = Graphics.FromImage(converted);
float centroidx;
float centroidy;
if (thresholdxs.Count > 0)
{
if (ExposureCalibrationComplete)
{
centroidx = (float)thresholdxs.Average();
centroidy = (float)thresholdys.Average();
WebCamTrack thisTrack = new WebCamTrack();
thisTrack.Points.Add(new Point2D(centroidx, centroidy, 0));
thisTrack.CameraID = CameraID;
FilteredTrackedPoints.TrackedPoints.Clear();
FilteredTrackedPoints.TrackedPoints.Add(thisTrack);
CumulativeTrackedPoints.Points.Add(new Point2D(centroidx, centroidy, 0, DateTime.Now));
if (_form.Visible == true)
{
thisGraphics.DrawRectangle(thisPen, ((float)(float)centroidx - (float)8), (float)centroidy - 8, 16, 16);
}
}
else
{
if (!ExposureCalibrationComplete)
{
if (Exposure - 10 > 0)
{
Exposure = Exposure - 10;
}
else
{
ExposureCalibrationComplete = true;
}
}
}
}
else
{
FilteredTrackedPoints.TrackedPoints.Clear();
ExposureCalibrationSuccessFrames++;
if (ExposureCalibrationSuccessFrames >= 10)
{
ExposureCalibrationComplete = true;
ExposureCalibrationSuccessFrames = 0;
}
}
}
try
{
_form.pictureBox1.Image = converted;
}
catch (Exception ex)
{
}
_form.pictureBox1.Invalidate();
}
}
CLEyeCameraStop(_camera);
CLEyeDestroyCamera(_camera);
_exitEvent.Set();
}