From 0688bb61ed33d66d6e0a736b75824eafa40d4f78 Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Thu, 9 Jul 2015 15:02:25 +0200 Subject: [PATCH] simplify 8point algorithm using Matx classes --- modules/calib3d/src/fundam.cpp | 75 +++++++++++++--------------------- 1 file changed, 28 insertions(+), 47 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 230182e8c..5d7e706a4 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/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(); const Point2f* m2 = _m2.ptr(); - double* fmatrix = _fmatrix.ptr(); 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 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 r( x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1 ); + A += r*r.t(); } + Vec W; + Matx 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(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(2,2)) > FLT_EPSILON ) - F0 *= 1./F0.at(2,2); + if( fabs(F0(2,2)) > FLT_EPSILON ) + F0 *= 1./F0(2,2); + + Mat(F0).copyTo(_fmatrix); return 1; }