/// <summary>
///
/// </summary>
/// <param name="m1"></param>
/// <param name="m2"></param>
/// <param name="model"></param>
/// <returns></returns>
public override unsafe int RunKernel(CvMat m1, CvMat m2, CvMat model)
{
int count = m1.Rows * m1.Cols;
CvPoint2D64f *M = (CvPoint2D64f *)m1.DataByte;
CvPoint2D64f *m = (CvPoint2D64f *)m2.DataByte;
double *LtL = stackalloc double[9 * 9],
W = stackalloc double[9 * 9],
V = stackalloc double[9 * 9];
CvMat _LtL = new CvMat(9, 9, MatrixType.F64C1, new IntPtr(LtL));
CvMat matW = new CvMat(9, 9, MatrixType.F64C1, new IntPtr(W));
CvMat matV = new CvMat(9, 9, MatrixType.F64C1, new IntPtr(V));
CvMat _H0 = new CvMat(3, 3, MatrixType.F64C1, new IntPtr(V + (9 * 8)));
CvMat _Htemp = new CvMat(3, 3, MatrixType.F64C1, new IntPtr(V + (9 * 7)));
CvPoint2D64f cM = new CvPoint2D64f(0, 0),
cm = new CvPoint2D64f(0, 0),
sM = new CvPoint2D64f(0, 0),
sm = new CvPoint2D64f(0, 0);
for (int i = 0; i < count; i++)
{
cm.X += m[i].X; cm.Y += m[i].Y;
cM.X += M[i].X; cM.Y += M[i].Y;
}
cm.X /= count; cm.Y /= count;
cM.X /= count; cM.Y /= count;
for (int i = 0; i < count; i++)
{
sm.X += Math.Abs(m[i].X - cm.X);
sm.Y += Math.Abs(m[i].Y - cm.Y);
sM.X += Math.Abs(M[i].X - cM.X);
sM.Y += Math.Abs(M[i].Y - cM.Y);
}
if (Math.Abs(sm.X) < double.Epsilon || Math.Abs(sm.Y) < double.Epsilon ||
Math.Abs(sM.X) < double.Epsilon || Math.Abs(sM.Y) < double.Epsilon)
{
return(0);
}
sm.X = count / sm.X; sm.Y = count / sm.Y;
sM.X = count / sM.X; sM.Y = count / sM.Y;
double[] invHnorm = new double[9] {
1.0 / sm.X, 0, cm.X, 0, 1.0 / sm.Y, cm.Y, 0, 0, 1
};
double[] Hnorm2 = new double[9] {
sM.X, 0, -cM.X * sM.X, 0, sM.Y, -cM.Y * sM.Y, 0, 0, 1
};
CvMat _invHnorm = new CvMat(3, 3, MatrixType.F64C1, invHnorm);
CvMat _Hnorm2 = new CvMat(3, 3, MatrixType.F64C1, Hnorm2);
Cv.Zero(_LtL);
for (int i = 0; i < count; i++)
{
double x = (m[i].X - cm.X) * sm.X, y = (m[i].Y - cm.Y) * sm.Y;
double X = (M[i].X - cM.X) * sM.X, Y = (M[i].Y - cM.Y) * sM.Y;
double[] Lx = { X, Y, 1, 0, 0, 0, -x * X, -x * Y, -x };
double[] Ly = { 0, 0, 0, X, Y, 1, -y * X, -y * Y, -y };
int j, k;
for (j = 0; j < 9; j++)
{
for (k = j; k < 9; k++)
{
LtL[j * 9 + k] += Lx[j] * Lx[k] + Ly[j] * Ly[k];
}
}
}
Cv.CompleteSymm(_LtL);
//cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
Cv.EigenVV(_LtL, matV, matW);
Cv.MatMul(_invHnorm, _H0, _Htemp);
Cv.MatMul(_Htemp, _Hnorm2, _H0);
Cv.ConvertScale(_H0, model, 1.0 / _H0.DataDouble[8]);
return(1);
}
/// <summary>
/// 指定した点と直線の距離を返す
/// </summary>
/// <param name="point"></param>
#else
/// <summary>
/// Returns the distance between this line and the specified point
/// </summary>
/// <param name="point"></param>
#endif
public double Distance(CvPoint2D64f point)
{
return(Distance(point.X, point.Y));
}
