Laser Pointer Tracking
LaserPointerTracking.zipThe C# code below defines several data structures thar make up the core of a laser pointer tracking application I developed for the .NET framework. The video stream is handled by the DirectShow library and the (not included) code to create and render the video stream graph is straight forward. I developed this application to be used with my thesis. You can read more about it if you are curious how it applies.
Feel free to explore the files on the page, or you can download the original C# code in a zip file. The code provided is incomplete. Its purpose is to give a general idea of the code's architecture as well as how the application works.
LaserDotTracking - The LaserDotTracking class handles processing of video frames to identify laser dot positions. It works with the calibration classes to control what pixels are identified as laser dots as well as the PixelList and PixelClump classes to make these points meaningful.
(show)/// LaserDotTracking class... public class LaserDotTracking { /* * CONSTANTS */ /// <summary> /// modifier for the red channel's contribution to brightness /// </summary> public const ushort RED_MODIFIER = 4; /// <summary> /// modifier for the green channel's contribution to brightness /// </summary> public const ushort GREEN_MODIFIER = 2; /// <summary> /// modifier for the blue channel's contribution to brightness /// </summary> public const ushort BLUE_MODIFIER = 2; //red * 4 + green * 2 + blue * 2 public const int MAX_POSSIBLE_BRIGHTNESS = 255 * (RED_MODIFIER + GREEN_MODIFIER + BLUE_MODIFIER); /* * CLASS PROPERTIES */ /// <summary> /// the object to store video calibration data /// </summary> private CalibrationData calibrationData; /// <summary> /// the raster size of the video feed /// </summary> private Size sourceSize; /// <summary> /// the number of lasers identified in the last frame /// </summary> private int numLasersOnScreen; /// <summary> /// the list of laser dots being tracked /// </summary> private List<LaserDot> lasers; /// <summary> /// bright pixel clump list /// </summary> private PixelList brightPixelList; /// <summary> /// the video frame used for thresholding /// </summary> private UnsafeBitmap uThresholdFrame; /* * CONSTRUCTOR */ /// <summary> /// constructor for the tracking object /// </summary> /// <param name="newData">the calibration object data</param> /// <param name="newLasers">the array of laser dots</param> public LaserDotTracking(CalibrationData newData, List<LaserDot> newLasers) { calibrationData = newData; lasers = newLasers; } /// <summary> /// unloads the laser dot tracker object /// </summary> public void unload() { uThresholdFrame.Dispose(); lasers.Clear(); lasers = null; brightPixelList = null; calibrationData = null; } //encapsulators omitted... /* * FRAME PROCESSING */ /// <summary> /// process a new frame of the input video /// </summary> public void ProcessFrame(Bitmap tmpImage1) { //Get width and height of source video if (brightPixelList == null || sourceSize.Width != tmpImage1.Width || sourceSize.Height != tmpImage1.Height) { sourceSize.Width = tmpImage1.Width; sourceSize.Height = tmpImage1.Height; brightPixelList = new PixelList(this, calibrationData, sourceSize.Width, sourceSize.Height); } //clear pixel list and prepare to process new frame brightPixelList.beginFrame(); //copy image content to bitmap in unmanaged memory UnsafeBitmap uBitmap = new UnsafeBitmap(tmpImage1); //lock memory uBitmap.LockBitmap(); if (!uThresholdFrame.Locked) this.uThresholdFrame.LockBitmap(); //integer variables avoid byte overflow int dRed; int dGreen; int dBlue; //for each scan line for (int y = 0; y < sourceSize.Height; y++) { //for each pixel in the scan line for (int x = 0; x < sourceSize.Width; x++) { PixelData thresholdPixel = this.uThresholdFrame.GetPixel(x, y); PixelData newPixel = uBitmap.GetPixel(x, y); //subtract brightnessThreshold pixel color from current pixel color dRed = (newPixel.red - thresholdPixel.red); dGreen = (newPixel.green - thresholdPixel.green); dBlue = (newPixel.blue - thresholdPixel.blue); //enforce zero minimum for adjusted color if (dRed < 0) dRed = 0; if (dGreen < 0) dGreen = 0; if (dBlue < 0) dBlue = 0; //pixel brightness function int brightness = (RED_MODIFIER * dRed) + (GREEN_MODIFIER * dGreen) + (BLUE_MODIFIER * dBlue); //if pixel is bright enough to be considered // a laser dot add it to the pixel list if (brightness > calibrationData.Threshold) brightPixelList.addPixel(new Point(x, y), (ushort)(brightness)); } } //identify laser pixel clumps brightPixelList.finishFrame(); //unlock bitmap uBitmap.UnlockBitmap(); //dispose frame uBitmap.Bitmap.Dispose(); tmpImage1.Dispose(); //process clumps and match them with laser dots from the previous frame foreach(LaserDot laser in lasers) if(laser.onScreen) laser.setClumpDistances( ref brightPixelList.PixelClumps ); // iteratively approach a best solution // for laser dot to pixel clump matching Boolean changeMade = true; int iteration = 0; while (changeMade && iteration < 10) { changeMade = false; foreach (LaserDot laser in lasers) { if (laser.claimBestClump()) changeMade = true; } iteration++; } //update position from best pixel clump foreach (LaserDot laser in lasers) laser.updateFromClaim(); //check for new lasers that were not visible in the previous frame foreach(LaserDot laser in lasers) laser.checkForNewLaser( ref brightPixelList.PixelClumps ); //total number of lasers dots in the current frame numLasersOnScreen = 0; for (int i = 0; i < lasers.Count; i++) { if (lasers[i].onScreen) numLasersOnScreen++; } } }
CalibrationData - This class stores the information regarding the calibration of a video capture source for tracking laser pointer dots or "laser dots". This class performs the following functions:
- Crops the video stream to help optimize tracking
- Corrects tracked positions to account for lens distortion
- Stores a reference image to compare video frames against for brightness thresholding.
//CalibrationData class... public class CalibrationData { /* * CONSTANTS */ /// <summary> /// the default pixel brightness threshold /// </summary> public const int DEFAULT_THRESHOLD = 500; /// <summary> /// the padding to add to the pixel brightness threshold calculation /// </summary> public const int THRESHOLD_PADDING = 20; /// <summary> /// the number of K vals needed to describe the camera lens /// </summary> public const int NUM_K_VALS = 2; /* * CLASS PROPERTIES */ /// <summary> /// the camera device name /// </summary> protected string cameraDeviceName; /// <summary> /// the capture device object /// </summary> protected CaptureDevice cameraDevice; /// <summary> /// the calibration form object /// </summary> protected CalibrationForm calibrationForm; /// <summary> /// the video monitor on the calibration form /// </summary> protected PictureBox videoMonitor; /// <summary> /// the last captured frame from the video /// for display to the video monitor /// </summary> protected Bitmap displayFrame; /// <summary> /// the full uncropped resolution of the video feed /// </summary> protected WinDraw.Size fullResolution; /// <summary> /// cropping rectangle /// </summary> protected WinDraw.Rectangle cropRectangle; //lens rectification //K lens parameters protected bool KValuesFound; protected DoubleVector Kvector; //normalization matrices protected Matrix3 tNormLens; protected Matrix3 tNormLensInv; /// <summary> /// point correction matrix to adjust for cropping /// </summary> protected Matrix3 tPointCorrection; /// <summary> /// the laser dot tracker object /// </summary> protected LaserDotTracking laserTracker; /// <summary> /// the brightness threshold level, above /// which pixels are considered to be a laser dot /// </summary> protected int brightnessThreshold; /// <summary> /// the number of lasers being tracked /// </summary> protected int numLasers; /// <summary> /// the list of laser dot objects /// </summary> protected List<LaserDot> lasers; //user point manipulation data omitted... /// <summary> /// constructor for a new video calibration object /// </summary> /// <param name="newCaptureDeviceName">the name of the capture device</param> /// <param name="videoResolution">the resolution of the video feed</param> /// <param name="newMonitor">the form object for displaying the video feed</param> /// <param name="newCaptureDevice">the new capture device object</param> /// <param name="newParentForm">the calibration form object</param> public CalibrationData(string newCaptureDeviceName, WinDraw.Size videoResolution, PictureBox newMonitor, CaptureDevice newCaptureDevice, CalibrationForm newCalibrationForm) { //video feed calibrationForm = newCalibrationForm; videoMonitor = newMonitor; fullResolution = videoResolution; cameraDeviceName = newCaptureDeviceName; cameraDevice = newCaptureDevice; if (cameraDevice != null) cameraDevice.NewFrame += new CameraEventHandler(ProcessFrame); //lens correction KValuesFound = false; Kvector = new DoubleVector(NUM_K_VALS); updateTnorm(); //default threshold brightnessThreshold = DEFAULT_THRESHOLD; //no cropping cropRectangle = new WinDraw.Rectangle(0, 0, fullResolution.Width, fullResolution.Height); tPointCorrection = Matrix3.Identity; //laser dots this.NumLasers = DEFAULT_NUM_LASERS; //create laserTracker laserTracker = new LaserDotTracking(this, lasers); } //encapsulators omitted /* * VIDEO PROCESSING */ /// <summary> /// should be called every CalibrationForm.UPDATES_PER_SECOND /// by the calibration form object /// </summary> public virtual void update() { //update display if (displayFrame != null && videoMonitor != null) { try { if (videoMonitor.Image != this.displayFrame) { if (videoMonitor.Image != null) videoMonitor.Image.Dispose(); drawOnDisplayFrame(); videoMonitor.Image = this.displayFrame; } } catch (InvalidOperationException exc) { Log(exc.Tostring()); } catch (ArgumentException exc) { Log(exc.Tostring()); } } } /// <summary> /// draw laser positions and crop box onto frame /// </summary> private void drawOnDisplayFrame() { try { Graphics dc = Graphics.FromImage(displayFrame); //draw cropping rectangle dc.DrawRectangle(new Pen(Color.White), cropRectangle); //add circle to image where laser positions were located if (laserTracker.NumVisibleLasers > 0) { Pen p = new Pen(Color.Red, 1); foreach (LaserDot laser in lasers) if (laser.onScreen) dc.DrawEllipse(p, (float)laser.NextPosition.X - 5, (float)laser.NextPosition.Y - 5, 10, 10); } dc.Dispose(); } catch (OutOfMemoryException OoM) { Log(OoM.Tostring()); GC.Collect(); //force garbage collection } catch (AccessViolationException exc) { Log(exc.Tostring()); return; } } /// <summary> /// event listener to handle a new video frame from the camera /// </summary> protected virtual void ProcessFrame(object sender, CameraEventArgs e) { if (videoMonitor.Image != displayFrame) displayFrame.Dispose(); //frame for display to the video monitor displayFrame = (Bitmap)e.Bitmap.Clone(); //process frame to identify new laser dot positions laserTracker.ProcessFrame((Bitmap)e.Bitmap.Clone(cropRectangle, e.Bitmap.PixelFormat)); //rectify positions from video space to NDC application space rectifyLaserPositions(); } /// <summary> /// stops video feed processsing /// </summary> public void stopAll() { if (cameraDevice != null) { cameraDevice.NewFrame -= new CameraEventHandler(ProcessFrame); cameraDevice.Stop(); } laserTracker.unload(); } /// <summary> /// rectififies the identified laser dot positions /// from video raster space to NDC application space /// </summary> public virtual void rectifyLaserPositions() { foreach(LaserDot laser in lasers) { if (laser.onScreen) { //lens correction... if (KValuesFound) laser.AdjustedPosition = tNormLensInv.transform( radialDistort( tNormLens.transform( laser.NextPosition), Kvector)); else laser.AdjustedPosition = laser.NextPosition; } } } /* * AUTO CALLIBRATION */ public virtual void AutoUpdateCorners() { //overriden by planar calibration } /* * CROPPING */ /// <summary> /// crop window to corners of screen area /// (overriden by planar calibration) /// </summary> public virtual void AutoCrop() { //crop the thresholding image if (laserTracker.ThresholdImage != null) laserTracker.ThresholdImage = laserTracker.ThresholdImage.Clone( cropRectangle, laserTracker.ThresholdImage.PixelFormat); //update callibration form if (calibrationForm != null) calibrationForm.updateCrop(cropRectangle.Top, fullResolution.Width - cropRectangle.Right + 1, fullResolution.Height - cropRectangle.Bottom + 1, cropRectangle.Left); } /// <summary> /// gets / sets the current video stream cropping rectangle /// </summary> public WinDraw.Rectangle CroppingRectangle { get { return cropRectangle; } set { cropRectangle = value; updateCrop(); } } /// <summary> /// updates the cropped display /// </summary> public void updateCrop() { //videoMonitor.Location = new Point(cropRectangle.X, cropRectangle.Y); if (laserTracker != null) laserTracker.Reset(); //update the matrix which corrects video positions to adjust for cropping tPointCorrection = Matrix3.Identity; tPointCorrection.translate(cropRectangle.X, cropRectangle.Y); } /// <summary> /// sets the last captured frame to be the brightnessThreshold image /// </summary> public void updateThresholdImage() { if (this.displayFrame != null) { //save threshold image for testing purposes displayFrame.Save("./" + CalibrationData.THRESHOLD_IMAGE_NAME, WinImage.ImageFormat.Jpeg); //cropped brightnessThreshold image laserTracker.ThresholdImage = displayFrame.Clone(cropRectangle, displayFrame.PixelFormat); } } /// <summary> /// uses the brightnessThreshold frame ///(an image of the screen at 50% brightness) /// and the max brightness frame /// (an image with the screen at 100% brightness) /// to derive a reasonable brightness brightnessThreshold, /// above which pixels are considered to /// be a laser pointer dot /// </summary> public void updateThresholdLimit() { if (this.laserTracker == null || t his.laserTracker.ThresholdImage == null) return; UnsafeBitmap thresholdBitmap = new UnsafeBitmap((Bitmap)laserTracker.ThresholdImage); UnsafeBitmap maxBrightBitmap = new UnsafeBitmap((Bitmap)displayFrame.Clone( this.cropRectangle, this.laserTracker.ThresholdImage.PixelFormat)); maxBrightBitmap.Bitmap.Save("./" + CalibrationData.MAX_THRESHOLD_IMAGE_NAME, WinImage.ImageFormat.Jpeg); thresholdBitmap.LockBitmap(); maxBrightBitmap.LockBitmap(); if (thresholdBitmap.Bitmap.Width != maxBrightBitmap.Bitmap.Width || thresholdBitmap.Bitmap.Height != maxBrightBitmap.Bitmap.Height) { throw new Exception("raster image size mismatch"); } // find the maximum difference between the max // brightness image and the thresholding image int maxBrightness = 0; for (int i = 0; i < maxBrightBitmap.Bitmap.Height; i++) { for (int j = 0; j < maxBrightBitmap.Bitmap.Width; j++) { PixelData brightPixel = maxBrightBitmap.GetPixel(j, i); PixelData thresPixel = thresholdBitmap.GetPixel(j, i); int ired = brightPixel.red - thresPixel.red; int igreen = brightPixel.green - thresPixel.green; int iblue = brightPixel.blue - thresPixel.blue; if(ired < 0) thresPixel.red = 0; else thresPixel.red = (byte)ired; if(igreen < 0) thresPixel.green = 0; else thresPixel.green = (byte)igreen; if(iblue < 0) thresPixel.blue = 0; else thresPixel.blue = (byte)iblue; int curThreshold = (thresPixel.red * LaserDotTracking.RED_MODIFIER) + (thresPixel.green * LaserDotTracking.GREEN_MODIFIER) + (thresPixel.blue * LaserDotTracking.BLUE_MODIFIER); if (curThreshold > maxBrightness) maxBrightness = curThreshold; } } //set the brightnessThreshold padding based //on the maximum brightness difference this.Threshold = maxBrightness + THRESHOLD_PADDING; calibrationForm.setThreshold(this.Threshold); } //user grid point manipulation omitted... /* * LENS RECTIFICATION */ /// <summary> /// updates the normalization matrix based on the video feed's width and height /// </summary> private void updateTnorm() { //normalization matrix places origin at the center of the video tNormLensInv = Matrix3.Identity; //translation tNormLensInv[0, 2] = (float)fullResolution.Width / 2; tNormLensInv[1, 2] = (float)fullResolution.Height / 2; //scaling tNormLensInv[0, 0] = (float)(fullResolution.Width + fullResolution.Height); tNormLensInv[1, 1] = (float)(fullResolution.Width + fullResolution.Height); tNormLensInv[2, 2] = 1.0f; tNormLens = tNormLensInv.returnInverse(); } /// <summary> /// get the square of the distance from a test point to a line defined /// by a point on the line and a direction and the line length squared (for performance) /// </summary> public double distanceSquaredToLine(Vector2d lineStart, Vector2d lineDir, double lengthSqr, Vector2d testPoint) { // vector from line's start to test point Vector2d w = testPoint - lineStart; // find perpendicular projection of hit point onto line segment double r = (w * lineDir) / lengthSqr; // closest point is at parameter r distance from start return Vector2d.DistanceSquared(testPoint, lineStart + (r * lineDir)); } /// <summary> /// get the distance from a test point to a line defined /// by a point and a line direction and the line length squared (for performance) /// </summary> public double distanceToLine(Vector2d lineStart, Vector2d lineDir, double lengthSqr, Vector2d testPoint) { return Math.Sqrt(distanceSquaredToLine(lineStart, lineDir, lengthSqr, testPoint)); } /// <summary> /// get the radially distorted point position from a current point and a K matrix /// this method assumes the point has been normalized to a central origin /// </summary> public Vector2d radialDistort(Vector2d curP, DoubleVector Kvals) { double r = curP.Length(); double Lr = 1; for (int i = 0; i < Kvals.Length; i++) Lr += Math.Pow(r, i + 1) * Kvals[i]; return Lr * curP; } /// <summary> /// determines the error /// </summary> /// <param name="x">the K vector that defines the lens distortion</param> /// <param name="count">the iteration count</param> /// <param name="maxIter">the total number of iterations</param> /// <returns>the residual error</returns> private double determineError(DoubleVector Kvals, int count, int maxIter) { double sum = 0; Vector2d correctLine = Vector2d.Zero; Vector2d lineStart = Vector2d.Zero; Vector2d lineEnd = Vector2d.Zero; double lengthSqr = 0; //lines defined by grid rows for (int i = 0; i < numGridRows; i++) { //rectified start and end points of grid row lineStart = radialDistort(gridPoints[i, 0], Kvals); lineEnd = radialDistort(gridPoints[i, numGridCols - 1], Kvals); //corrected line length and vector correctLine = lineEnd - lineStart; lengthSqr = correctLine.LengthSquared(); //use all points in row except far left and right for (int j = 1; j < numGridCols-1; j++) //distance from staright line to test point sum += distanceSquaredToLine(lineStart, correctLine, lengthSqr, radialDistort(gridPoints[i, j], Kvals)); } //lines defined by grid columns for (int j = 0; j < numGridCols; j++) { //rectified start and end points of grid column lineStart = radialDistort(gridPoints[0, j], Kvals); lineEnd = radialDistort(gridPoints[numGridRows - 1, j], Kvals); //corrected line length and vector correctLine = lineEnd - lineStart; lengthSqr = correctLine.LengthSquared(); //all points in column except top and bottom for (int i = 1; i < numGridRows - 1; i++) //distance from staright line to test point sum += distanceSquaredToLine(lineStart, correctLine, lengthSqr, radialDistort(gridPoints[i, j], Kvals)); } return sum; } /// <summary> /// determine appropriate K values to adjust for lens distortion /// </summary> public void LensDistortionCorrection() { gridPoints = new Vector2d[numGridRows, numGridCols]; for (int i = 0; i < numGridRows; i++) for (int j = 0; j < numGridCols; j++) gridPoints[i, j] = tNormLens * new Vector2d(gridCircles[i, j].Location); Kvector = new DoubleVector(NUM_K_VALS); //test error when fixing barrel distortion FloatMatrix.setAllValues(ref Kvector, 0.1); double barrelDistortionError = determineError(Kvector, 0, 200); //test error when fixing pin cushion distortion FloatMatrix.setAllValues(ref Kvector, -0.1); double pinCushionDistortionError = determineError(Kvector, 0, 200); //use more accurate model if(barrelDistortionError < pinCushionDistortionError) FloatMatrix.setAllValues(ref Kvector, 0.1); double minimizedError = 0; string outmsg = ""; //two methods for iteratively solving for lens distortion parameters //SimpleIterative uses a method of my own design, LMFsolve uses the Levenberg-Marquardt method Kvector = FloatMatrix.SimpleIterative(Kvector, new LFMResiduals(determineError), 1E-12, ref outmsg, ref minimizedError, 50, 10); //Kvector = FloatMatrix.LMFsolve(Kvector, new LFMResiduals(determineError), ref outmsg, //ref numIterations, ref minimizedError, 50, 1e-12, 1e-12, 10); Log(outmsg); int numPoints = numGridRows * numGridCols; double initialError = determineError(new DoubleVector(Kvector.Length), 0, 1); double residualError = determineError(this.Kvector, 0, 1); Log("initial error: " + initialError + " residual error: " + residualError); KValuesFound = true; } }
PlaneCalibration - The plane callibration class handles the transformation that map points from the raster space of the video feed to NDC coordinates. It accomplishes this transformation by analying an image from the video feed to identify and store the corner locations of the screen in the image. Then a homography is found that maps these points to NDC corners.
(show)//PlaneCalibration class... public class PlaneCalibration : CallibrationData { /* * CONSTANTS */ public const int GREEN_CORNER_THRESHOLD = 10; public const int BOUNDS_PADDING = 10; /* * CLASS PROPERTIES */ private bool HomFound; private Matrix3 HomMatrix; private List<PictureBox> hCircles; private Vector2 cornerTL; private Vector2 cornerTR; private Vector2 cornerBL; private Vector2 cornerBR; private WinDraw.Rectangle bounds; /* * CONSTRUCTOR */ /// <summary> /// constructor for a new plane callibration object /// </summary> /// <param name="newCaptureDeviceName">the name of the capture device</param> /// <param name="videoResolution">the resolution of the video feed</param> /// <param name="newMonitor">the form object for displaying the video feed</param> /// <param name="newCaptureDevice">the new capture device object</param> /// <param name="newParentForm">the calibration form object</param> public PlaneCalibration(String newCaptureDeviceName, WinDraw.Size videoResolution, PictureBox newMonitor, CaptureDevice newCaptureDevice, CalibrationForm newCalibrationForm) : base( newName, videoResolution, newMonitor, newDevice, newParentForm) { HomFound = false; HomMatrix = Matrix3.Identity; hCircles = new List<PictureBox>(); } //manual point manipualtion omitted /* * AUTO CALIBRATION */ /// <summary> /// Automatically identifies the corners of the screen /// in the video feed image (using the most recent frame). /// The application displays an image with green circles in the four /// corners of the screen so that only a few pixels need to be considered. /// </summary> public override void AutoUpdateCorners() { //verify non null values if (displayFrame == null || videoMonitor == null) throw new Exception("null component, cannot proceed with auto calibration"); //get most recent frame as a bitmap in unmanaged memory Bitmap cornerRefBitmap = (Bitmap)displayFrame.Clone(); UnsafeBitmap cornerRef = new UnsafeBitmap(cornerRefBitmap); cornerRef.LockBitmap(); //brightest pixels List<Vector2d> brightPoints = new List<Vector2d>(); // identify all points bright enough to // be considered part of the screen image for (int y = 0; y < cornerRef.Bitmap.Height; y++) { for (int x = 0; x < cornerRef.Bitmap.Width; x++) { //for each pixel in image checkt to see if pixel is bright enough //to be a corner area pixel if (cornerRef.GetPixel(x, y).green > GREEN_CORNER_THRESHOLD) brightPoints.Add(new Vector2d(x, y)); } } if (brightPoints.Count < 4) throw new Exception("Not enough points found."); //find the hull of the bright pixels double minX = brightPoints[0].X; double maxX = brightPoints[0].X; double minY = brightPoints[0].Y; double maxY = brightPoints[0].Y; for (int i = 1; i < brightPoints.Count; i++) { if (brightPoints[i].X < minX) minX = brightPoints[i].X; if (brightPoints[i].Y < minY) minY = brightPoints[i].Y; if (brightPoints[i].X > maxX) maxX = brightPoints[i].X; if (brightPoints[i].Y > maxY) maxY = brightPoints[i].Y; } //boudning box of the corner points bounds = new System.Drawing.Rectangle((int)minX, (int)minY, (int)(maxX - minX), (int)(maxY - minY)); //points on the corners of the bounding box Vector2d boundsTL = new Vector2d(minX, minY); Vector2d boundsTR = new Vector2d(maxX, minY); Vector2d boundsBL = new Vector2d(minX, maxY); Vector2d boundsBR = new Vector2d(maxX, maxY); //direction of the bounding box edges Vector2d topDir = boundsTR - boundsTL; Vector2d bottomDir = boundsBR - boundsBL; Vector2d leftDir = boundsTL - boundsBL; Vector2d rightDir = boundsTR - boundsBR; //screen corners Vector2d TL = Vector2d.Zero; Vector2d TR = Vector2d.Zero; Vector2d BL = Vector2d.Zero; Vector2d BR = Vector2d.Zero; //edge lengths double topLength = Vector2d.DistanceSquared(boundsTL, boundsTR); double bottomLength = Vector2d.DistanceSquared(boundsBL, boundsBR); double rightLength = Vector2d.DistanceSquared(boundsTR, boundsBR); double leftLength = Vector2d.DistanceSquared(boundsTL, boundsBL); double curDist = 0; double TLMinDist = 0; double TRMinDist = 0; double BLMinDist = 0; double BRMinDist = 0; // find the positions that minimizes the distance between // two edges of the bounding box to find the screen corner for (int i = 0; i < brightPoints.Count; i++) { //top and left curDist = distanceToLine(boundsTL, topDir, topLength, brightPoints[i]) + distanceToLine(boundsBL, leftDir, leftLength, brightPoints[i]); if (curDist < TLMinDist || i == 0) { TL = brightPoints[i]; TLMinDist = curDist; } //top and right curDist = distanceToLine(boundsTL, topDir, topLength, brightPoints[i]) + distanceToLine(boundsBR, rightDir, rightLength, brightPoints[i]); if (curDist < TRMinDist || i == 0) { TR = brightPoints[i]; TRMinDist = curDist; } //bottom and left curDist = distanceToLine(boundsBL, bottomDir, bottomLength, brightPoints[i]) + distanceToLine(boundsBL, leftDir, leftLength, brightPoints[i]); if (curDist < BLMinDist || i == 0) { BL = brightPoints[i]; BLMinDist = curDist; } //bottom and right curDist = distanceToLine(boundsBL, bottomDir, bottomLength, brightPoints[i]) + distanceToLine(boundsBR, rightDir, rightLength, brightPoints[i]); if (curDist < BRMinDist || i == 0) { BR = brightPoints[i]; BRMinDist = curDist; } } //convert to XNA vectors cornerTL = TL.ToXNAVector(); cornerTR = TR.ToXNAVector(); cornerBL = BL.ToXNAVector(); cornerBR = BR.ToXNAVector(); //output image with identified corners circled for testing purposes cornerRef.UnlockBitmap(); Graphics dc = Graphics.FromImage(cornerRefBitmap); Pen p = new Pen(Color.Red, 1); dc.DrawEllipse(p, (float)cornerTL.X - 2, (float)cornerTL.Y - 2, 4, 4); dc.DrawEllipse(p, (float)cornerTR.X - 2, (float)cornerTR.Y - 2, 4, 4); dc.DrawEllipse(p, (float)cornerBL.X - 2, (float)cornerBL.Y - 2, 4, 4); dc.DrawEllipse(p, (float)cornerBR.X - 2, (float)cornerBR.Y - 2, 4, 4); dc.Dispose(); cornerRefBitmap.Save(curGame.ApplicationDirectory + CallibrationData.CORNER_REF_IMAGE, WinImage.ImageFormat.Jpeg); //update corner point positions (hCircles list) //in video monitor on calibration form updateCorners(); } /// <summary> /// crop video feed to the corners of the screen image area /// </summary> public override void AutoCrop() { base.autoCrop(); if (bounds != null && bounds.Width > 0 && bounds.Height > 0) { //add padding to bounding box bounds.X -= BOUNDS_PADDING; bounds.Y -= BOUNDS_PADDING; float twicePadding = BOUNDS_PADDING * 2; bounds.Width += twicePadding; bounds.Height += twicePadding; if (bounds.X < 0) bounds.X = 0; if (bounds.Y < 0) bounds.Y = 0; if (bounds.Right > fullResolution.Width - 1) bounds.Width = fullResolution.Width - bounds.X - 1; if (bounds.Bottom > fullResolution.Height - 1) bounds.Height = fullResolution.Height - bounds.Y - 1; //update crop... this.cropRectangle = bounds; } } /* * HOMOGRAPHIC MATRIX CALCULATION */ /// <summary> /// put point positions into a list in the correct /// order for homography calculation(TL, TR, BL, BR) /// </summary> private void getCornerPoints() { if (hCircles.Count >= 4) { List<Vector2d> screenCorners = new List<Vector2d>(); for (int i = 0; i < 4; i++) { //correct for video scaling and cropping screenCorners.Add(new Vector2d(hCircles[i].Location)); if (mKfound) screenCorners[i] = tNormLensInv.transform(radialDistort( tNormLens.transform(screenCorners[i]), Kvector)); } //identify each of 4 corners as the top left, top right, bottom left, and bottom right positions double minX = fullResolution.Width; double minX2 = fullResolution.Width; double minY = fullResolution.Height; double minY2 = fullResolution.Height; double minXIndex = 0; double minX2Index = 0; double minYIndex = 0; double minY2Index = 0; //find smallest two X coordinates for (int i = 0; i < 4; i++) { if (screenCorners[i].X < minX) { minX2 = minX; minX2Index = minXIndex; minX = screenCorners[i].X; minXIndex = i; } else if (screenCorners[i].X < minX2) { minX2 = screenCorners[i].X; minX2Index = i; } } //find smallest two Y coordinates for (int i = 0; i < 4; i++) { if (screenCorners[i].Y < minY) { minY2 = minY; minY2Index = minYIndex; minY = screenCorners[i].Y; minYIndex = i; } else if (screenCorners[i].Y < minY2) { minY2 = screenCorners[i].Y; minY2Index = i; } } //find identified screen corners Vector2d TL = Vector2d.Zero; Vector2d TR = Vector2d.Zero; Vector2d BL = Vector2d.Zero; Vector2d BR = Vector2d.Zero; for (int i = 0; i < 4; i++) { if ((i == minXIndex || i == minX2Index) && i == minYIndex || i == minY2Index)) TL = screenCorners[i]; else if ((!(i == minXIndex || i == minX2Index)) && (i == minYIndex || i == minY2Index)) TR = screenCorners[i]; else if ((i == minXIndex || i == minX2Index) && !(i == minYIndex || i == minY2Index)) BL = screenCorners[i]; else BR = screenCorners[i]; } screenCorners[0] = TL; screenCorners[1] = TR; screenCorners[2] = BL; screenCorners[3] = BR; return screenCorners; } return null; } /// <summary> /// updates homographic matrix based on corner circle positions /// </summary> private void updateHomography() { HomFound = false; if (hCircles != null && hCircles.Count >= 4) { List<Vector2d> screenCorners = getCornerPoints(); //normalized device coordinate corners NDCPoints = new List<Vector2d>(); NDCPoints.Add(new Vector2d(-1, -1)); NDCPoints.Add(new Vector2d(1, -1)); NDCPoints.Add(new Vector2d(-1, 1)); NDCPoints.Add(new Vector2d(1, 1)); //find mapping from raster corners in video space to NDC corners HomMatrix = Matrix3.Homography(screenCorners, NDCPoints); HomFound = true; } } /* * LASER POSITION RECTIFICATION */ /// <summary> /// rectify point positions from video space to NDC application space /// </summary> public override void rectifyLaserPositions() { base.rectifyLaserPositions(); if (HomFound) { foreach (LaserDot laser in lasers) if (laser.onScreen) laser.AdjustedPosition = HomMatrix.transform(laser.AdjustedPosition); } } }
PixelList - This class manages a list of pixel coordinates that have an intensity bright enough to be a laser dot. Once a frame has finished processing by the tracker class the identified coordinates are grouped together into PixelClumps.
(show)//PixelList class public class PixelList { /* * CONSTANTS */ /// <summary> /// the minimum number of pixels in a clump to be considered part of a laser dot /// </summary> public const int NUM_PIXELS_NEEDED_FOR_LASER = 3; /* * CLASS PROPERTIES */ /// <summary> /// the list of bright pixel clumps /// </summary> public List<PixelClump> PixelClumps; /// <summary> /// the maximum number of clumps /// </summary> protected int maxNumClumps; //possible pixel attributes /// <summary> /// the list of bright pixel positions identified by the tracker /// </summary> protected List<Point> brightPixels; /// <summary> /// the raster size of the video stream /// </summary> protected Size sourceSize; /// <summary> /// the matrix of values flags indicating which pixels /// have been identified as part of a pixel clump /// </summary> protected Boolean[,] identified; /// <summary> /// the matrix of laser dot weights (brightness) for each pixel /// </summary> protected ushort[,] pixelWeights; /// <summary> /// the video calibration data object /// </summary> protected CalibrationData calibrationData; /// <summary> /// the laser dot tracking object /// </summary> protected LaserDotTracking curTracker; /* * CONSTRUCTOR */ /// <summary> /// constructor for a new pixel list manager object /// </summary> /// <param name="newTracker">the laser dot tracking object</param> /// <param name="newData">the calibration data object</param> /// <param name="sourceWidth">the raster width of the video feed</param> /// <param name="sourceHeight">the raster height of the video feed</param> public PixelList(LaserDotTracking newTracker, CalibrationData newData, int sourceWidth, int sourceHeight) { curTracker = newTracker; calibrationData = newData; maxNumClumps = calibrationData.NumLasers * 2; sourceSize = new Size(sourceWidth, height); pixelWeights = new ushort[sourceSize.sourceHeight, sourceSize.Height]; identified = new Boolean[sourceSize.Width, sourceSize.Height]; brightPixels = new List<Point>(150); PixelClumps = new List<PixelClump>(); } /// <summary> /// called when a new frame begins processing /// </summary> public void beginFrame() { brightPixels.Clear(); for (int i = 0; i < sourceSize.Height; i++) for (int j = 0; j < sourceSize.Width; j++) { //set all pixels to no laser weight pixelWeights[j, i] = 0; identified[j, i] = false; } } /// <summary> /// adds a new entry to the list of pixels which are intense /// enough to be considered a laser pointer /// </summary> /// <param name="pos">the pixel's position</param> /// <param name="weight">the pixel's weight (brightness)</param> public void addPixel(Point pos, ushort weight) { pixelWeights[pos.X, pos.Y] = weight; brightPixels.Add(pos); } /* * PROCESS FINISHED FRAME */ /// <summary> /// called when a frame has finished processing /// by the laser dot tracking object. Processes /// the identified bright pixels to group them /// into pixel clumps /// </summary> public virtual void finishFrame() { PixelClumps.Clear(); for (int i = 0; i < brightPixels.Count; i++) { //if the current bright pixel has not already been identified as part of another pixel clump... if (!identified[brightPixels[i].X, brightPixels[i].Y]) { //create a new pixel clump PixelClump curClump = new PixelClump(false); //gets all the pixels that make up this clump recursiveGetPixelClump(brightPixels[i].X, brightPixels[i].Y,ref curClump); if (curClump.NumPixels == 0) { curClump.AveragePosition.X = 0; curClump.AveragePosition.Y = 0; curClump.AverageWeight = 0; } else { //find the average position of this clump curClump.AveragePosition.X = curClump.PositionSum.X / curClump.NumPixels; curClump.AveragePosition.Y = curClump.PositionSum.Y / curClump.NumPixels; curClump.AveragePosition += calibrationData.CropOffset; curClump.AverageWeight = curClump.TotalWeight / curClump.NumPixels; } //if the clump is large enough add it to the list of pixel clumps that represent laser dots if (curClump.NumPixels >= PixelList.NUM_PIXELS_NEEDED_FOR_LASER) PixelClumps.Add(curClump); //limit the number of pixel clumps //to prevent a stack overflow or OoM if(PixelClumps.Count > maxNumClumps) break; } } } /// <summary> /// recursively identifies all pixels in this /// bright clump by flood filling algorithm /// </summary> /// <param name="X">the current X location</param> /// <param name="Y">the current Y location</param> /// <param name="clump">the current clump of pixels</param> private void recursiveGetPixelClump(int X, int Y, ref PixelClump clump) { if (X >= 0 && X < sourceSize.Width && Y >= 0 && Y < sourceSize.Height && !identified[X, Y] && pixelWeights[X, Y] > 0) { //if in bounds and not yet a part of a pixel clump and pixel weight is large enough identified[X, Y] = true; clump.TotalWeight += pixelWeights[X, Y]; clump.PositionSum.X += X; clump.PositionSum.Y += Y; clump.NumPixels++; // limit number of pixels which can make up a clump // to prevent a stack overflow if (clump.NumPixels > 400) return; //add neighbors recursiveGetPixelClump(X + 1, Y - 1, ref clump); //right, top recursiveGetPixelClump(X + 1, Y, ref clump); //right recursiveGetPixelClump(X + 1, Y + 1, ref clump); //right, botom recursiveGetPixelClump(X, Y - 1, ref clump); //top recursiveGetPixelClump(X, Y + 1, ref clump); //bottom recursiveGetPixelClump(X - 1, Y - 1, ref clump); //left, top recursiveGetPixelClump(X - 1, Y, ref clump); //left recursiveGetPixelClump(X - 1, Y + 1, ref clump); //left, botom } } }
PixelClump - A simple struct that stores information describing a clump of bright pixels. This clump is then matched with a laser dot being tracked from the previous frame or is identified as a new laser dot.
(show)//PixelClump struct... public struct PixelClump { /* * STRUCT PROPERTIES */ /// <summary> /// clump position from average pixel position /// </summary> public Vector2d AveragePosition; /// <summary> /// the average pixel position in this clump /// </summary> public Point PositionSum; /// <summary> /// the sum of all pixel weights /// </summary> public int TotalWeight; /// <summary> /// the average pixel weight /// </summary> public int AverageWeight; /// <summary> /// the number of pixels in this clump /// </summary> public int NumPixels; /// <summary> /// indicates if this clump has been claimed by a laser dot /// </summary> public Boolean Claimed; /// <summary> /// the laser dot that claims this laser dot /// </summary> public LaserDot LaserDot; /// <summary> /// the distance of this laser dot from the expected position /// </summary> public double DistanceFromExpected; /* * CONSTRUCTOR */ /// <summary> /// the constructor for a new pixel clump /// </summary> public PixelClump(bool newClaimed) { PositionSum = new Point(0, 0); TotalWeight = 0; NumPixels = 0; AveragePosition = Vector2d.Zero; AverageWeight = 0; Claimed = newClaimed; LaserDot = null; DistanceFromExpected = 0; } /* * CLAIM / UNCLAIM */ /// <summary> /// claims the laser dot as belonging to the specified laser dot /// </summary> /// <param name="dot">the laser dot claiming this pixel clump</param> /// <param name="distance">the distance from the expected laser dot position to this clump's position</param> public void claim(LaserDot dot, double distance) { Claimed = true; LaserDot = dot; DistanceFromExpected = distance; } /// <summary> /// sets this clump to be unclaimed by any laser dots /// </summary> public void unclaim() { LaserDot.unclaim(); Claimed = false; LaserDot = null; DistanceFromExpected = 0; } }