generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #4183 from paroj:8point

Browse Source
This commit is contained in:
Vadim Pisarevsky 2015-07-22 11:01:23 +00:00
commit 6d3bc7c82d
1 changed files with 28 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
modules/calib3d/src/fundam.cpp
View File

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