simplify 8point algorithm using Matx classes

modules/calib3d/src/fundam.cpp

@@ -547,45 +547,32 @@ static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
 
 static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
 {
-    double a[9*9], w[9], v[9*9];
-    Mat W( 9, 1, CV_64F, w );
-    Mat V( 9, 9, CV_64F, v );
-    Mat A( 9, 9, CV_64F, a );
-    Mat U, F0, TF;
-
     Point2d m1c(0,0), m2c(0,0);
     double t, scale1 = 0, scale2 = 0;
 
     const Point2f* m1 = _m1.ptr<Point2f>();
     const Point2f* m2 = _m2.ptr<Point2f>();
-    double* fmatrix = _fmatrix.ptr<double>();
     CV_Assert( (_m1.cols == 1 || _m1.rows == 1) && _m1.size() == _m2.size());
-    int i, j, k, count = _m1.checkVector(2);
+    int i, count = _m1.checkVector(2);
 
     // compute centers and average distances for each of the two point sets
     for( i = 0; i < count; i++ )
     {
-        double x = m1[i].x, y = m1[i].y;
-        m1c.x += x; m1c.y += y;
-
-        x = m2[i].x, y = m2[i].y;
-        m2c.x += x; m2c.y += y;
+        m1c += Point2d(m1[i]);
+        m2c += Point2d(m2[i]);
     }
 
     // calculate the normalizing transformations for each of the point sets:
     // after the transformation each set will have the mass center at the coordinate origin
     // and the average distance from the origin will be ~sqrt(2).
     t = 1./count;
-    m1c.x *= t; m1c.y *= t;
-    m2c.x *= t; m2c.y *= t;
+    m1c *= t;
+    m2c *= t;
 
     for( i = 0; i < count; i++ )
     {
-        double x = m1[i].x - m1c.x, y = m1[i].y - m1c.y;
-        scale1 += std::sqrt(x*x + y*y);
-
-        x = m2[i].x - m2c.x, y = m2[i].y - m2c.y;
-        scale2 += std::sqrt(x*x + y*y);
+        scale1 += norm(Point2d(m1[i].x - m1c.x, m1[i].y - m1c.y));
+        scale2 += norm(Point2d(m2[i].x - m2c.x, m2[i].y - m2c.y));
     }
 
     scale1 *= t;
@@ -597,7 +584,7 @@ static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
     scale1 = std::sqrt(2.)/scale1;
     scale2 = std::sqrt(2.)/scale2;
 
-    A.setTo(Scalar::all(0));
+    Matx<double, 9, 9> A;
 
     // form a linear system Ax=0: for each selected pair of points m1 & m2,
     // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0
@@ -608,56 +595,50 @@ static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix )
         double y1 = (m1[i].y - m1c.y)*scale1;
         double x2 = (m2[i].x - m2c.x)*scale2;
         double y2 = (m2[i].y - m2c.y)*scale2;
-        double r[9] = { x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1 };
-        for( j = 0; j < 9; j++ )
-            for( k = 0; k < 9; k++ )
-                a[j*9+k] += r[j]*r[k];
+        Vec<double, 9> r( x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1 );
+        A += r*r.t();
     }
 
+    Vec<double, 9> W;
+    Matx<double, 9, 9> V;
+
     eigen(A, W, V);
 
     for( i = 0; i < 9; i++ )
     {
-        if( fabs(w[i]) < DBL_EPSILON )
+        if( fabs(W[i]) < DBL_EPSILON )
             break;
     }
 
     if( i < 8 )
         return 0;
 
-    F0 = Mat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
+    Matx33d F0( V.val + 9*8 ); // take the last column of v as a solution of Af = 0
 
     // make F0 singular (of rank 2) by decomposing it with SVD,
     // zeroing the last diagonal element of W and then composing the matrices back.
 
-    // use v as a temporary storage for different 3x3 matrices
-    W = U = V = TF = F0;
-    W = Mat(3, 1, CV_64F, v);
-    U = Mat(3, 3, CV_64F, v + 9);
-    V = Mat(3, 3, CV_64F, v + 18);
-    TF = Mat(3, 3, CV_64F, v + 27);
+    Vec3d w;
+    Matx33d U;
+    Matx33d Vt;
 
-    SVDecomp( F0, W, U, V, SVD::MODIFY_A );
-    W.at<double>(2) = 0.;
+    SVD::compute( F0, w, U, Vt);
+    w[2] = 0.;
 
-    // F0 <- U*diag([W(1), W(2), 0])*V'
-    gemm( U, Mat::diag(W), 1., 0, 0., TF, 0 );
-    gemm( TF, V, 1., 0, 0., F0, 0/*CV_GEMM_B_T*/ );
+    F0 = U*Matx33d::diag(w)*Vt;
 
     // apply the transformation that is inverse
     // to what we used to normalize the point coordinates
-    double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
-    double tt2[] = { scale2, 0, -scale2*m2c.x, 0, scale2, -scale2*m2c.y, 0, 0, 1 };
-    Mat T1(3, 3, CV_64F, tt1), T2(3, 3, CV_64F, tt2);
+    Matx33d T1( scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 );
+    Matx33d T2( scale2, 0, -scale2*m2c.x, 0, scale2, -scale2*m2c.y, 0, 0, 1 );
 
-    // F0 <- T2'*F0*T1
-    gemm( T2, F0, 1., 0, 0., TF, GEMM_1_T );
-    F0 = Mat(3, 3, CV_64F, fmatrix);
-    gemm( TF, T1, 1., 0, 0., F0, 0 );
+    F0 = T2.t()*F0*T1;
 
     // make F(3,3) = 1
-    if( fabs(F0.at<double>(2,2)) > FLT_EPSILON )
-        F0 *= 1./F0.at<double>(2,2);
+    if( fabs(F0(2,2)) > FLT_EPSILON )
+        F0 *= 1./F0(2,2);
+
+    Mat(F0).copyTo(_fmatrix);
 
     return 1;
 }