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