///// <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++;
}
