///// <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();
}
}
public void AddCalibrationPoint(Point point)
{
if (CalibrationPoints == null)
{
CalibrationPoints = new List <Point>();
}
CalibrationPoints.Add(point);
foreach (VideoForm videoForm in VideoForms)
{
videoForm.TrackingCamera.RecordCalibrationPoint();
}
}
public void Calibrate()
{
//CalibrationPoints.Add(PointUV.Create(-100, -100));
//CalibrationPoints.Add(PointUV.Create(-100, 100));
//CalibrationPoints.Add(PointUV.Create(100, -100));
//CalibrationPoints.Add(PointUV.Create(100, 100));
//CalibrationPoints.Add(PointUV.Create(-80, -80));
//CalibrationPoints.Add(PointUV.Create(-80, 80));
//CalibrationPoints.Add(PointUV.Create(80, -80));
//CalibrationPoints.Add(PointUV.Create(80, 80));
double h = (double)imageSize.Height;
CalibrationPoints.Add(PointUV.Create(0, 0));
CalibrationPoints.Add(PointUV.Create(0, h));
CalibrationPoints.Add(PointUV.Create(h, 0));
CalibrationPoints.Add(PointUV.Create(h, h));
h *= 2;
CalibrationPoints.Add(PointUV.Create(0, 0));
CalibrationPoints.Add(PointUV.Create(0, h));
CalibrationPoints.Add(PointUV.Create(h, 0));
CalibrationPoints.Add(PointUV.Create(h, h));
NMatrix calibrationMatrixA = new NMatrix(2 * CalibrationPoints.Count, 11);
NMatrix calibrationMatrixB = new NMatrix(2 * CalibrationPoints.Count, 1);
for (int i = 0; i < CalibrationPoints.Count; i++)
{
double x = controlForm.CalibrationPoints[i].X;
double y = controlForm.CalibrationPoints[i].Y;
double z = controlForm.CalibrationPoints[i].Z;
double u = CalibrationPoints[i].U;
double v = CalibrationPoints[i].V;
calibrationMatrixA[2 * i, 0] = x;
calibrationMatrixA[2 * i, 1] = y;
calibrationMatrixA[2 * i, 2] = z;
calibrationMatrixA[2 * i, 3] = 1;
calibrationMatrixA[2 * i, 8] = -u * x;
calibrationMatrixA[2 * i, 9] = -u * y;
calibrationMatrixA[2 * i, 10] = -u * z;
calibrationMatrixA[2 * i + 1, 4] = x;
calibrationMatrixA[2 * i + 1, 5] = y;
calibrationMatrixA[2 * i + 1, 6] = z;
calibrationMatrixA[2 * i + 1, 7] = 1;
calibrationMatrixA[2 * i + 1, 8] = -v * x;
calibrationMatrixA[2 * i + 1, 9] = -v * y;
calibrationMatrixA[2 * i + 1, 10] = -v * z;
calibrationMatrixB[2 * i, 0] = u;
calibrationMatrixB[2 * i + 1, 0] = v;
}
NMatrix calibrationMatrixAT = NMatrix.Transpose(calibrationMatrixA);
NMatrix homographyData = NMatrix.Inverse(calibrationMatrixAT * calibrationMatrixA) * calibrationMatrixAT * calibrationMatrixB;
Homography = new NMatrix(new double[, ] {
{ homographyData[0, 0], homographyData[1, 0], homographyData[2, 0], homographyData[3, 0] },
{ homographyData[4, 0], homographyData[5, 0], homographyData[6, 0], homographyData[7, 0] },
{ homographyData[8, 0], homographyData[9, 0], homographyData[10, 0], 1 }
});
}
public void RecordCalibrationPoint()
{
CalibrationPoints.Add(Position);
}
