///// <summary> ///// Computes the augmented rotation matrix for an augmented 2D point around the origin ///// </summary> ///// <param name="theta">The clockwise rotation angle in radians</param> ///// <returns>The augmented (3x3) 2D rotation matrix</returns> //public static Matrix<double> RotationMatrix(double theta) //{ // return DenseMatrix.OfArray(new double[,] // { // {Math.Cos(theta), Math.Sin(theta), 0 }, // {-1*Math.Sin(theta), Math.Cos(theta), 0 }, // {0, 0, 1 } // }); //} ///// <summary> ///// Computes the augmented translation matrix for an augmented 2D point ///// </summary> ///// <param name="dx">Translation amount in x direction</param> ///// <param name="dy">Translation amount in y direction</param> ///// <returns>The augmented (3x3) 2D translation matrix</returns> //public static Matrix<double> TranslationMatrix(double dx, double dy) //{ // return DenseMatrix.OfArray(new double[,] // { // {1, 0, dx }, // {0, 1, dy }, // {0, 0, 1 } // }); //} ///// <summary> ///// Augments a 2D coordinate point ///// </summary> ///// <param name="px">The x-coordinate</param> ///// <param name="py">The y-coordinate</param> ///// <returns>A 3 element vector comprising the point</returns> //public static Vector<double> Augment2DCoordinate(double px, double py) //{ // return DenseVector.OfArray(new double[] { px, py, 1 }); //} ///// <summary> ///// Composes (multiplies) a list of transformation matrices in order ///// NOTE: List should be ordered in order of application which needs to be reversed for the composition! ///// </summary> ///// <param name="matrices">List of transformations to compose</param> ///// <returns>The composed matrix</returns> //public static Matrix<double> Compose(List<Matrix<double>> matrices) //{ // Matrix<double> m = DenseMatrix.CreateIdentity(3); // for(int i = matrices.Count-1; i>=0; i--) // m *= matrices[i]; // return m; //} ///// <summary> ///// Applies transformation to given point ///// </summary> ///// <param name="p"></param> ///// <param name="T"></param> ///// <returns></returns> //public static IppiPoint TransformPoint(IppiPoint p, Matrix<double> T) //{ // Vector<double> result = Augment2DCoordinate(p.x, p.y) * T; // return new IppiPoint((int)result[0], (int)result[1]); //} ///// <summary> ///// Applies transformation to given point ///// </summary> ///// <param name="p"></param> ///// <param name="T"></param> ///// <returns></returns> //public static IppiPoint_32f TransformPoint(IppiPoint_32f p, Matrix<double> T) //{ // Vector<double> result = T * Augment2DCoordinate(p.x, p.y); // return new IppiPoint_32f((float)result[0], (float)result[1]); //} /// <summary> /// Method for creating image background /// </summary> /// <param name="frameNumber">The current camera frame number</param> /// <param name="camImage">The camera image</param> /// <param name="poi">A detected centroid (all null)</param> protected void BuildBackground(int frameNumber, Image8 camImage, out IppiPoint?poi) { _laser.LaserPower = 0; poi = null; if (frameNumber == 0 || _bgModel == null) { _bgModel = new DynamicBackgroundModel(camImage, 1.0f / Properties.Settings.Default.FrameRate); _fgModel = new DynamicBackgroundModel(camImage, 5.0f / Properties.Settings.Default.FrameRate); //Create image intermediates _calc = new Image8(camImage.Size); _foreground = new Image8(camImage.Size); _strel3x3 = Morphology.Generate3x3Mask(camImage.Size); //Initialize buffer for label markers int bufferSize = 0; cv.ippiLabelMarkersGetBufferSize_8u_C1R(camImage.Size, &bufferSize); if (_markerBuffer != null) { Marshal.FreeHGlobal((IntPtr)_markerBuffer); _markerBuffer = null; } _markerBuffer = (byte *)Marshal.AllocHGlobal(bufferSize); } else { _bgModel.UpdateBackground(camImage); } if (frameNumber >= Properties.Settings.Default.FrameRate * 5) { _experimentPhase = ExperimentPhases.ThreePoint; _threePointFrame = 0; _threePointPoints = new CalibrationPoints(); } }
/// <summary> /// Runs the construction of the interpolation lookup table /// </summary> /// <param name="frameNumber">The current camera frame number</param> /// <param name="camImage">The camera image</param> /// <param name="poi">The detected beam centroid</param> protected void InterpTableCalibration(int frameNumber, Image8 camImage, out IppiPoint?poi) { poi = null; if (_interpParams.RequestDealt) { System.Diagnostics.Debug.Assert(!_interpParams.Walking); //we have dealt the last request so lets get the next point or leave calibration //if table is complete if (!_interpParams.LookupTable.AddNext(new PointVoltagePair(_interpParams.CurrentPoint, _interpParams.CurrentVolts.x, _interpParams.CurrentVolts.y), out _interpParams.NextRequest)) { //lookup table complete. Precompute and save to file _interpParams.LookupTable.Precompute(); _experimentPhase = ExperimentPhases.Done; var calibFile = File.CreateText("main.calib"); _interpParams.LookupTable.SaveToFile(calibFile); calibFile.Close(); System.Diagnostics.Debug.WriteLine("Calibration updated"); return; } //we got a new point to deal with - prepare to immediately trigger the next condition and reset our foreground _fgModel.Dispose(); _fgModel = new DynamicBackgroundModel(camImage, 5.0f / Properties.Settings.Default.FrameRate); _interpParams.RequestDealt = false; _interpParams.StepFrame = 0; } if (_interpParams.StepFrame == 0 && !_interpParams.RequestDealt) { //we have a request to deal with - try to target the requested coordinates and initiate walk _interpParams.CurrentVolts = GetMirrorVolts(_interpParams.NextRequest); _interpParams.CurrentVolts.x += _interpParams.XError * InterpolationParams.VoltsPerErrorPixel; _interpParams.CurrentVolts.y += _interpParams.YError * InterpolationParams.VoltsPerErrorPixel; _scanner.Hit(_interpParams.CurrentVolts); _interpParams.Walking = true; _interpParams.StepFrame++; } else if (_interpParams.StepFrame > Properties.Settings.Default.FrameRate / 100 && _interpParams.StepFrame < Properties.Settings.Default.FrameRate / 5 && _interpParams.Walking) { //give scanner 10ms to reach the target and then collect the foreground until 200ms _fgModel.UpdateBackground(camImage); _interpParams.StepFrame++; } else if (_interpParams.StepFrame > Properties.Settings.Default.FrameRate / 5 && _interpParams.Walking) { //Analyse coordinates. If they match the requested point, add the voltages //to the lookup table. Otherwise compute the error and try again IppiPoint actual = FindBeamLocation(_fgModel.Background, _bgModel.Background, _calc, _foreground); poi = new IppiPoint(actual.x, actual.y); //test for error if ((actual.x - _interpParams.NextRequest.x) == 0 && (actual.y - _interpParams.NextRequest.y) == 0) { //Looks good, add point and on next iteration we continue with next request _interpParams.RequestDealt = true; _interpParams.RetryCount = 0; _interpParams.CurrentPoint = _interpParams.NextRequest; _interpParams.StepFrame = 0; } else { int errorx, errory; //update x and y errors if (_interpParams.NextRequest.x > actual.x) { errorx = _interpParams.NextRequest.x - actual.x; if (errorx < 5) { _interpParams.XError += errorx; } else { _interpParams.XError += 5; } } else { errorx = _interpParams.NextRequest.x - actual.x; if (errorx > -5) { _interpParams.XError += errorx; } else { _interpParams.XError -= 5; } } if (_interpParams.NextRequest.y > actual.y) { errory = _interpParams.NextRequest.y - actual.y; if (errory < 5) { _interpParams.YError += errory; } else { _interpParams.YError += 5; } } else { errory = _interpParams.NextRequest.y - actual.y; if (errory > -5) { _interpParams.YError += errory; } else { _interpParams.YError -= 5; } } //too large errors may indicate that the point actually moved off the screen //in that case we try to recover by returning to the original position and going //from there if (Math.Abs(errorx) > 100 || Math.Abs(errory) > 100) { _interpParams.XError = 0; _interpParams.YError = 0; System.Diagnostics.Debug.WriteLine("Error > 100. Trying to recover."); } _interpParams.RetryCount++; if (_interpParams.RetryCount > 250) { //We can't reach the point. Add it to the table nonetheless and continue (may be occluded point) //At this point the errors are likely meaningless so reset them //also we add the point as if the trigonometry lookup table is correct _interpParams.CurrentVolts = GetMirrorVolts(_interpParams.NextRequest); _interpParams.XError = 0; _interpParams.YError = 0; _interpParams.CurrentPoint = _interpParams.NextRequest; if (!_interpParams.LookupTable.ForceAddNext(new PointVoltagePair(_interpParams.CurrentPoint, _interpParams.CurrentVolts.x, _interpParams.CurrentVolts.y), out _interpParams.NextRequest)) { //lookup table complete - Precompute interpolations and save to file _interpParams.LookupTable.Precompute(); var calibFile = File.CreateText("main.calib"); _interpParams.LookupTable.SaveToFile(calibFile); calibFile.Close(); System.Diagnostics.Debug.WriteLine("Calibration updated"); return; } System.Diagnostics.Debug.WriteLine("Force add point to lookup table"); _interpParams.RetryCount = 0; } } //Not walking and reset step frame to restart where we left off and create new foreground _interpParams.Walking = false; _interpParams.StepFrame = 0; _fgModel.Dispose(); _fgModel = new DynamicBackgroundModel(camImage, 5.0f / Properties.Settings.Default.FrameRate); }//else if analyse walk else { _interpParams.StepFrame++;//Waiting for mirror to reach final position } }
/// <summary> /// Runs the initial three point calibration to find approximate camera height and angle/reflection /// </summary> /// <param name="frameNumber">The current camera frame number</param> /// <param name="camImage">The camera image</param> /// <param name="poi">The detected beam centroid</param> protected void ThreePointCalibration(int frameNumber, Image8 camImage, out IppiPoint?poi) { poi = null; //Points not set in _threePointPoints are set to -1,-1. Use this to track which point we handle right now if (_threePointPoints.Origin.x == -1) { //Work on origin var p = MoveAndDetect(_threePointFrame, camImage, new IppiPoint_32f(0.0f, 0.0f)); if (p.x != -1) { //found the point _threePointFrame = -1; //will be incremented to 0 below! _threePointPoints.Origin = new IppiPoint(p.x, p.y); _p0 = new IppiPoint(p.x, p.y); //set our origin coordinates for later calculations poi = p; //reset foreground in preparation for x movement _fgModel.Dispose(); _fgModel = new DynamicBackgroundModel(camImage, 5.0f / Properties.Settings.Default.FrameRate); } } else if (_threePointPoints.XMove.x == -1) { //Work on x-displacement var p = MoveAndDetect(_threePointFrame, camImage, new IppiPoint_32f(Properties.Settings.Default.MirrorXVAln, 0.0f)); if (p.x != -1) { //found the point _threePointFrame = -1;//will be incremented to 0 below! _threePointPoints.XMove = new IppiPoint(p.x, p.y); poi = p; //reset foreground in preparation for y movement _fgModel.Dispose(); _fgModel = new DynamicBackgroundModel(camImage, 5.0f / Properties.Settings.Default.FrameRate); } } else if (_threePointPoints.YMove.x == -1) { //Work on y-displacement var p = MoveAndDetect(_threePointFrame, camImage, new IppiPoint_32f(0.0f, Properties.Settings.Default.MirrorYVAln)); if (p.x != -1) { //found the point _threePointFrame = -1;//will be incremented to 0 below! _threePointPoints.YMove = new IppiPoint(p.x, p.y); poi = p; //reset foreground in preparation of interpolation table building _fgModel.Dispose(); _fgModel = new DynamicBackgroundModel(camImage, 20.0f / Properties.Settings.Default.FrameRate); } } else { //Use results to calculate calibration //Height double camera_height_x = CalculateHeight(_threePointPoints.XMove, MirrorAngle(Properties.Settings.Default.MirrorXVAln)); System.Diagnostics.Debug.WriteLine("Height according to x-mirror is: {0}", camera_height_x); double camera_height_y = CalculateHeight(_threePointPoints.YMove, MirrorAngle(Properties.Settings.Default.MirrorYVAln)); System.Diagnostics.Debug.WriteLine("Height according to y-mirror is: {0}", camera_height_y); _camera_height = (camera_height_x + camera_height_y) / 2.0; //Theta and reflection double theta_x = CalculateCameraTheta(_threePointPoints.XMove, false); System.Diagnostics.Debug.WriteLine("Theta according to x-mirror is: {0}", theta_x); double theta_y = CalculateCameraTheta(_threePointPoints.YMove, true); System.Diagnostics.Debug.WriteLine("Theta according to y-mirror is: {0}", theta_y); _isYReflected = CheckReflection(theta_x, theta_y, out _camera_theta); System.Diagnostics.Debug.WriteLine("Y reflection: {0}", _isYReflected); System.Diagnostics.Debug.WriteLine("Final camera theta is: {0}", _camera_theta); _experimentPhase = ExperimentPhases.InterpTable; _interpParams = new InterpolationParams(); //At this point have 10 pixel borders around camera image in the interpolation ROI _interpParams.LookupTable = new BLIScanLookupTable(new IppiROI(10, 10, camImage.Width - 20, camImage.Height - 20), 4); } _threePointFrame++; }