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 }
});
}
