1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
43 #include "_cvmodelest.h"
45 template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )
48 for( i = j = 0; i < count; i++ )
58 class CvHomographyEstimator : public CvModelEstimator2
61 CvHomographyEstimator( int modelPoints );
63 virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
64 virtual bool refine( const CvMat* m1, const CvMat* m2,
65 CvMat* model, int maxIters );
67 virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
68 const CvMat* model, CvMat* error );
72 CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)
73 : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)
75 assert( _modelPoints == 4 || _modelPoints == 5 );
76 checkPartialSubsets = false;
79 int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
81 int i, count = m1->rows*m1->cols;
82 const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
83 const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
85 double LtL[9][9], W[9][9], V[9][9];
86 CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
87 CvMat _W = cvMat( 9, 9, CV_64F, W );
88 CvMat _V = cvMat( 9, 9, CV_64F, V );
89 CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );
90 CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );
91 CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};
93 for( i = 0; i < count; i++ )
95 cm.x += m[i].x; cm.y += m[i].y;
96 cM.x += M[i].x; cM.y += M[i].y;
99 cm.x /= count; cm.y /= count;
100 cM.x /= count; cM.y /= count;
102 for( i = 0; i < count; i++ )
104 sm.x += fabs(m[i].x - cm.x);
105 sm.y += fabs(m[i].y - cm.y);
106 sM.x += fabs(M[i].x - cM.x);
107 sM.y += fabs(M[i].y - cM.y);
110 sm.x = count/sm.x; sm.y = count/sm.y;
111 sM.x = count/sM.x; sM.y = count/sM.y;
113 double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
114 double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
115 CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );
116 CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
119 for( i = 0; i < count; i++ )
121 double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
122 double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
123 double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
124 double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
126 for( j = 0; j < 9; j++ )
127 for( k = j; k < 9; k++ )
128 LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
130 cvCompleteSymm( &_LtL );
132 //cvSVD( &_LtL, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
133 cvEigenVV( &_LtL, &_V, &_W );
134 cvMatMul( &_invHnorm, &_H0, &_Htemp );
135 cvMatMul( &_Htemp, &_Hnorm2, &_H0 );
136 cvConvertScale( &_H0, H, 1./_H0.data.db[8] );
142 void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
143 const CvMat* model, CvMat* _err )
145 int i, count = m1->rows*m1->cols;
146 const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
147 const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
148 const double* H = model->data.db;
149 float* err = _err->data.fl;
151 for( i = 0; i < count; i++ )
153 double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
154 double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;
155 double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;
156 err[i] = (float)(dx*dx + dy*dy);
160 bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )
162 CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
163 int i, j, k, count = m1->rows*m1->cols;
164 const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
165 const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
166 CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );
167 cvCopy( &modelPart, solver.param );
171 const CvMat* _param = 0;
172 CvMat *_JtJ = 0, *_JtErr = 0;
173 double* _errNorm = 0;
175 if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))
178 for( i = 0; i < count; i++ )
180 const double* h = _param->data.db;
181 double Mx = M[i].x, My = M[i].y;
182 double ww = 1./(h[6]*Mx + h[7]*My + 1.);
183 double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;
184 double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;
185 double err[] = { _xi - m[i].x, _yi - m[i].y };
190 { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
191 { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
194 for( j = 0; j < 8; j++ )
196 for( k = j; k < 8; k++ )
197 _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];
198 _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];
202 *_errNorm += err[0]*err[0] + err[1]*err[1];
206 cvCopy( solver.param, &modelPart );
212 cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
213 CvMat* __H, int method, double ransacReprojThreshold,
216 const double confidence = 0.995;
217 const int maxIters = 2000;
219 CvMat *m = 0, *M = 0, *tempMask = 0;
221 CV_FUNCNAME( "cvFindHomography" );
226 CvMat _H = cvMat( 3, 3, CV_64FC1, H );
229 CV_ASSERT( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
231 count = MAX(imagePoints->cols, imagePoints->rows);
232 CV_ASSERT( count >= 4 );
234 m = cvCreateMat( 1, count, CV_64FC2 );
235 cvConvertPointsHomogeneous( imagePoints, m );
237 M = cvCreateMat( 1, count, CV_64FC2 );
238 cvConvertPointsHomogeneous( objectPoints, M );
242 CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
243 (mask->rows == 1 || mask->cols == 1) &&
244 mask->rows*mask->cols == count );
248 tempMask = cvCreateMat( 1, count, CV_8U );
250 cvSet( tempMask, cvScalarAll(1.) );
253 CvHomographyEstimator estimator( MIN(count, 5) );
256 if( method == CV_LMEDS )
257 result = estimator.runLMeDS( M, m, &_H, tempMask, confidence, maxIters );
258 else if( method == CV_RANSAC )
259 result = estimator.runRANSAC( M, m, &_H, tempMask, ransacReprojThreshold, confidence, maxIters);
261 result = estimator.runKernel( M, m, &_H ) > 0;
263 if( result && count > 4 )
265 icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
266 count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
267 M->cols = m->cols = count;
268 estimator.refine( M, m, &_H, 10 );
273 cvConvert( &_H, __H );
279 if( tempMask != mask )
280 cvReleaseMat( &tempMask );
286 /* Evaluation of Fundamental Matrix from point correspondences.
287 The original code has been written by Valery Mosyagin */
289 /* The algorithms (except for RANSAC) and the notation have been taken from
290 Zhengyou Zhang's research report
291 "Determining the Epipolar Geometry and its Uncertainty: A Review"
292 that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
294 /************************************** 7-point algorithm *******************************/
295 class CvFMEstimator : public CvModelEstimator2
298 CvFMEstimator( int _modelPoints );
300 virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
301 virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model );
302 virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model );
304 virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
305 const CvMat* model, CvMat* error );
308 CvFMEstimator::CvFMEstimator( int _modelPoints )
309 : CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 )
311 assert( _modelPoints == 7 || _modelPoints == 8 );
315 int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
317 return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model );
320 int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
322 double a[7*9], w[7], v[9*9], c[4], r[3];
325 CvMat A = cvMat( 7, 9, CV_64F, a );
326 CvMat V = cvMat( 9, 9, CV_64F, v );
327 CvMat W = cvMat( 7, 1, CV_64F, w );
328 CvMat coeffs = cvMat( 1, 4, CV_64F, c );
329 CvMat roots = cvMat( 1, 3, CV_64F, r );
330 const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
331 const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
332 double* fmatrix = _fmatrix->data.db;
335 // form a linear system: i-th row of A(=a) represents
336 // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0
337 for( i = 0; i < 7; i++ )
339 double x0 = m1[i].x, y0 = m1[i].y;
340 double x1 = m2[i].x, y1 = m2[i].y;
353 // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so
354 // the solution is linear subspace of dimensionality 2.
355 // => use the last two singular vectors as a basis of the space
356 // (according to SVD properties)
357 cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
361 // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
362 // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
363 // so f ~ lambda*f1 + (1 - lambda)*f2.
364 // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
365 // it will be a cubic equation.
366 // find c - polynomial coefficients.
367 for( i = 0; i < 9; i++ )
370 t0 = f2[4]*f2[8] - f2[5]*f2[7];
371 t1 = f2[3]*f2[8] - f2[5]*f2[6];
372 t2 = f2[3]*f2[7] - f2[4]*f2[6];
374 c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;
376 c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
377 f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
378 f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
379 f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
380 f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
381 f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
382 f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);
384 t0 = f1[4]*f1[8] - f1[5]*f1[7];
385 t1 = f1[3]*f1[8] - f1[5]*f1[6];
386 t2 = f1[3]*f1[7] - f1[4]*f1[6];
388 c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -
389 f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
390 f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
391 f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
392 f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
393 f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
394 f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);
396 c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;
398 // solve the cubic equation; there can be 1 to 3 roots ...
399 n = cvSolveCubic( &coeffs, &roots );
404 for( k = 0; k < n; k++, fmatrix += 9 )
406 // for each root form the fundamental matrix
407 double lambda = r[k], mu = 1.;
408 double s = f1[8]*r[k] + f2[8];
410 // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1
411 if( fabs(s) > DBL_EPSILON )
420 for( i = 0; i < 8; i++ )
421 fmatrix[i] = f1[i]*lambda + f2[i]*mu;
428 int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
430 double a[9*9], w[9], v[9*9];
431 CvMat W = cvMat( 1, 9, CV_64F, w );
432 CvMat V = cvMat( 9, 9, CV_64F, v );
433 CvMat A = cvMat( 9, 9, CV_64F, a );
436 CvPoint2D64f m0c = {0,0}, m1c = {0,0};
437 double t, scale0 = 0, scale1 = 0;
439 const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
440 const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
441 double* fmatrix = _fmatrix->data.db;
442 int i, j, k, count = _m1->cols*_m1->rows;
444 // compute centers and average distances for each of the two point sets
445 for( i = 0; i < count; i++ )
447 double x = m1[i].x, y = m1[i].y;
448 m0c.x += x; m0c.y += y;
450 x = m2[i].x, y = m2[i].y;
451 m1c.x += x; m1c.y += y;
454 // calculate the normalizing transformations for each of the point sets:
455 // after the transformation each set will have the mass center at the coordinate origin
456 // and the average distance from the origin will be ~sqrt(2).
458 m0c.x *= t; m0c.y *= t;
459 m1c.x *= t; m1c.y *= t;
461 for( i = 0; i < count; i++ )
463 double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y;
464 scale0 += sqrt(x*x + y*y);
466 x = fabs(m2[i].x - m1c.x), y = fabs(m2[i].y - m1c.y);
467 scale1 += sqrt(x*x + y*y);
473 if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
476 scale0 = sqrt(2.)/scale0;
477 scale1 = sqrt(2.)/scale1;
481 // form a linear system Ax=0: for each selected pair of points m1 & m2,
482 // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0
483 // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0.
484 for( i = 0; i < count; i++ )
486 double x0 = (m1[i].x - m0c.x)*scale0;
487 double y0 = (m1[i].y - m0c.y)*scale0;
488 double x1 = (m2[i].x - m1c.x)*scale1;
489 double y1 = (m2[i].y - m1c.y)*scale1;
490 double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 };
491 for( j = 0; j < 9; j++ )
492 for( k = 0; k < 9; k++ )
493 a[j*9+k] += r[j]*r[k];
496 cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
498 for( i = 0; i < 8; i++ )
500 if( fabs(w[i]) < DBL_EPSILON )
507 F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
509 // make F0 singular (of rank 2) by decomposing it with SVD,
510 // zeroing the last diagonal element of W and then composing the matrices back.
512 // use v as a temporary storage for different 3x3 matrices
519 cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
522 // F0 <- U*diag([W(1), W(2), 0])*V'
523 cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );
524 cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );
526 // apply the transformation that is inverse
527 // to what we used to normalize the point coordinates
529 double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };
530 double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
537 cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );
538 F0.data.db = fmatrix;
539 cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );
542 if( fabs(F0.data.db[8]) > FLT_EPSILON )
543 cvScale( &F0, &F0, 1./F0.data.db[8] );
550 void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2,
551 const CvMat* model, CvMat* _err )
553 int i, count = _m1->rows*_m1->cols;
554 const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
555 const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
556 const double* F = model->data.db;
557 float* err = _err->data.fl;
559 for( i = 0; i < count; i++ )
561 double a, b, c, d1, d2, s1, s2;
563 a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];
564 b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];
565 c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];
568 d2 = m2[i].x*a + m2[i].y*b + c;
570 a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];
571 b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];
572 c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];
575 d1 = m1[i].x*a + m1[i].y*b + c;
577 err[i] = (float)std::max(d1*d1*s1, d2*d2*s2);
583 cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
584 CvMat* fmatrix, int method,
585 double param1, double param2, CvMat* mask )
588 CvMat *m1 = 0, *m2 = 0, *tempMask = 0;
590 CV_FUNCNAME( "cvFindFundamentalMat" );
595 CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );
598 CV_ASSERT( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );
599 CV_ASSERT( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&
600 (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) );
602 count = MAX(points1->cols, points1->rows);
606 m1 = cvCreateMat( 1, count, CV_64FC2 );
607 cvConvertPointsHomogeneous( points1, m1 );
609 m2 = cvCreateMat( 1, count, CV_64FC2 );
610 cvConvertPointsHomogeneous( points2, m2 );
614 CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
615 (mask->rows == 1 || mask->cols == 1) &&
616 mask->rows*mask->cols == count );
617 tempMask = cvCreateMatHeader(1, count, CV_8U);
618 cvSetData(tempMask, mask->data.ptr, 0);
621 tempMask = cvCreateMat( 1, count, CV_8U );
623 cvSet( tempMask, cvScalarAll(1.) );
626 CvFMEstimator estimator( MIN(count, (method & 3) == CV_FM_7POINT ? 7 : 8) );
628 result = estimator.run7Point(m1, m2, &_F9x3);
629 else if( count == 8 || method == CV_FM_8POINT )
630 result = estimator.run8Point(m1, m2, &_F3x3);
635 if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
638 if( (method & ~3) == CV_RANSAC )
639 result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 );
641 result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );
644 /*icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );
645 count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count );
646 assert( count >= 8 );
647 m1->cols = m2->cols = count;
648 estimator.run8Point(m1, m2, &_F3x3);*/
653 cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );
659 if( tempMask != mask )
660 cvReleaseMat( &tempMask );
667 cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
668 const CvMat* fmatrix, CvMat* lines )
670 CV_FUNCNAME( "cvComputeCorrespondEpilines" );
674 int abc_stride, abc_plane_stride, abc_elem_size;
675 int plane_stride, stride, elem_size;
676 int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
678 const uchar *xp, *yp, *zp;
680 CvMat F = cvMat( 3, 3, CV_64F, f );
682 if( !CV_IS_MAT(points) )
683 CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
685 depth = CV_MAT_DEPTH(points->type);
686 cn = CV_MAT_CN(points->type);
687 if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) )
688 CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
690 if( points->rows > points->cols )
692 dims = cn*points->cols;
693 count = points->rows;
697 if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) )
698 CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
699 dims = cn * points->rows;
700 count = points->cols;
703 if( dims != 2 && dims != 3 )
704 CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
706 if( !CV_IS_MAT(fmatrix) )
707 CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
709 if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
710 CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
712 if( fmatrix->cols != 3 || fmatrix->rows != 3 )
713 CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" );
715 if( !CV_IS_MAT(lines) )
716 CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
718 abc_depth = CV_MAT_DEPTH(lines->type);
719 abc_cn = CV_MAT_CN(lines->type);
720 if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) )
721 CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
723 if( lines->rows > lines->cols )
725 abc_dims = abc_cn*lines->cols;
726 abc_count = lines->rows;
730 if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) )
731 CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
732 abc_dims = abc_cn * lines->rows;
733 abc_count = lines->cols;
737 CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
739 if( abc_count != count )
740 CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
742 elem_size = CV_ELEM_SIZE(depth);
743 abc_elem_size = CV_ELEM_SIZE(abc_depth);
745 if( points->rows == dims )
747 plane_stride = points->step;
752 plane_stride = elem_size;
753 stride = points->rows == 1 ? dims*elem_size : points->step;
756 if( lines->rows == 3 )
758 abc_plane_stride = lines->step;
759 abc_stride = abc_elem_size;
763 abc_plane_stride = abc_elem_size;
764 abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
767 CV_CALL( cvConvert( fmatrix, &F ));
768 if( pointImageID == 2 )
769 cvTranspose( &F, &F );
771 xp = points->data.ptr;
772 yp = xp + plane_stride;
773 zp = dims == 3 ? yp + plane_stride : 0;
775 ap = lines->data.ptr;
776 bp = ap + abc_plane_stride;
777 cp = bp + abc_plane_stride;
779 for( i = 0; i < count; i++ )
784 if( depth == CV_32F )
786 x = *(float*)xp; y = *(float*)yp;
788 z = *(float*)zp, zp += stride;
792 x = *(double*)xp; y = *(double*)yp;
794 z = *(double*)zp, zp += stride;
797 xp += stride; yp += stride;
799 a = f[0]*x + f[1]*y + f[2]*z;
800 b = f[3]*x + f[4]*y + f[5]*z;
801 c = f[6]*x + f[7]*y + f[8]*z;
803 nu = nu ? 1./sqrt(nu) : 1.;
804 a *= nu; b *= nu; c *= nu;
806 if( abc_depth == CV_32F )
808 *(float*)ap = (float)a;
809 *(float*)bp = (float)b;
810 *(float*)cp = (float)c;
829 cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
834 CV_FUNCNAME( "cvConvertPointsHomogeneous" );
838 int i, s_count, s_dims, d_count, d_dims;
839 CvMat _src, _dst, _ones;
842 if( !CV_IS_MAT(src) )
843 CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
844 "The input parameter is not a valid matrix" );
846 if( !CV_IS_MAT(dst) )
847 CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
848 "The output parameter is not a valid matrix" );
850 if( src == dst || src->data.ptr == dst->data.ptr )
852 if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
853 CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
857 if( src->rows > src->cols )
859 if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
860 CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
862 s_dims = CV_MAT_CN(src->type)*src->cols;
867 if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
868 CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
870 s_dims = CV_MAT_CN(src->type)*src->rows;
874 if( src->rows == 1 || src->cols == 1 )
875 src = cvReshape( src, &_src, 1, s_count );
877 if( dst->rows > dst->cols )
879 if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
880 CV_ERROR( CV_StsBadSize,
881 "Either the number of channels or columns or rows in the input matrix must be =1" );
883 d_dims = CV_MAT_CN(dst->type)*dst->cols;
888 if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
889 CV_ERROR( CV_StsBadSize,
890 "Either the number of channels or columns or rows in the output matrix must be =1" );
892 d_dims = CV_MAT_CN(dst->type)*dst->rows;
896 if( dst->rows == 1 || dst->cols == 1 )
897 dst = cvReshape( dst, &_dst, 1, d_count );
899 if( s_count != d_count )
900 CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
902 if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
903 CV_ERROR( CV_StsUnsupportedFormat,
904 "Both matrices must be floating-point (single or double precision)" );
906 if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
907 CV_ERROR( CV_StsOutOfRange,
908 "Both input and output point dimensionality must be 2, 3 or 4" );
910 if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
911 CV_ERROR( CV_StsUnmatchedSizes,
912 "The dimensionalities of input and output point sets differ too much" );
914 if( s_dims == d_dims - 1 )
916 if( d_count == dst->rows )
918 ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
919 dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
923 ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
924 dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
928 if( s_dims <= d_dims )
930 if( src->rows == dst->rows && src->cols == dst->cols )
932 if( CV_ARE_TYPES_EQ( src, dst ) )
935 cvConvert( src, dst );
939 if( !CV_ARE_TYPES_EQ( src, dst ))
941 CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
942 cvConvert( src, temp );
945 cvTranspose( src, dst );
949 cvSet( ones, cvRealScalar(1.) );
953 int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
955 if( !CV_ARE_TYPES_EQ( src, dst ))
957 CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
958 cvConvert( src, temp );
962 elem_size = CV_ELEM_SIZE(src->type);
964 if( s_count == src->cols )
965 s_plane_stride = src->step / elem_size, s_stride = 1;
967 s_stride = src->step / elem_size, s_plane_stride = 1;
969 if( d_count == dst->cols )
970 d_plane_stride = dst->step / elem_size, d_stride = 1;
972 d_stride = dst->step / elem_size, d_plane_stride = 1;
974 CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));
976 if( CV_MAT_DEPTH(dst->type) == CV_32F )
978 const float* xs = src->data.fl;
979 const float* ys = xs + s_plane_stride;
981 const float* ws = xs + (s_dims - 1)*s_plane_stride;
983 float* iw = denom->data.fl;
985 float* xd = dst->data.fl;
986 float* yd = xd + d_plane_stride;
991 zs = ys + s_plane_stride;
992 zd = yd + d_plane_stride;
995 for( i = 0; i < d_count; i++, ws += s_stride )
998 iw[i] = fabs((double)t) > FLT_EPSILON ? t : 1.f;
1001 cvDiv( 0, denom, denom );
1004 for( i = 0; i < d_count; i++ )
1007 float x = *xs * w, y = *ys * w, z = *zs * w;
1008 xs += s_stride; ys += s_stride; zs += s_stride;
1009 *xd = x; *yd = y; *zd = z;
1010 xd += d_stride; yd += d_stride; zd += d_stride;
1013 for( i = 0; i < d_count; i++ )
1016 float x = *xs * w, y = *ys * w;
1017 xs += s_stride; ys += s_stride;
1019 xd += d_stride; yd += d_stride;
1024 const double* xs = src->data.db;
1025 const double* ys = xs + s_plane_stride;
1026 const double* zs = 0;
1027 const double* ws = xs + (s_dims - 1)*s_plane_stride;
1029 double* iw = denom->data.db;
1031 double* xd = dst->data.db;
1032 double* yd = xd + d_plane_stride;
1037 zs = ys + s_plane_stride;
1038 zd = yd + d_plane_stride;
1041 for( i = 0; i < d_count; i++, ws += s_stride )
1044 iw[i] = fabs(t) > DBL_EPSILON ? t : 1.;
1047 cvDiv( 0, denom, denom );
1050 for( i = 0; i < d_count; i++ )
1053 double x = *xs * w, y = *ys * w, z = *zs * w;
1054 xs += s_stride; ys += s_stride; zs += s_stride;
1055 *xd = x; *yd = y; *zd = z;
1056 xd += d_stride; yd += d_stride; zd += d_stride;
1059 for( i = 0; i < d_count; i++ )
1062 double x = *xs * w, y = *ys * w;
1063 xs += s_stride; ys += s_stride;
1065 xd += d_stride; yd += d_stride;
1072 cvReleaseMat( &denom );
1073 cvReleaseMat( &temp );
1079 static Mat _findHomography( const Mat& points1, const Mat& points2,
1080 int method, double ransacReprojThreshold,
1081 vector<uchar>* mask )
1083 CV_Assert(points1.isContinuous() && points2.isContinuous() &&
1084 points1.type() == points2.type() &&
1085 ((points1.rows == 1 && points1.channels() == 2) ||
1086 points1.cols*points1.channels() == 2) &&
1087 ((points2.rows == 1 && points2.channels() == 2) ||
1088 points2.cols*points2.channels() == 2));
1090 Mat H(3, 3, CV_64F);
1091 CvMat _pt1 = Mat(points1), _pt2 = Mat(points2);
1092 CvMat _H = H, _mask, *pmask = 0;
1095 mask->resize(points1.cols*points1.rows*points1.channels()/2);
1096 pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));
1098 bool ok = cvFindHomography( &_pt1, &_pt2, &_H, method, ransacReprojThreshold, pmask ) > 0;
1104 static Mat _findFundamentalMat( const Mat& points1, const Mat& points2,
1105 int method, double param1, double param2,
1106 vector<uchar>* mask )
1108 CV_Assert(points1.isContinuous() && points2.isContinuous() &&
1109 points1.type() == points2.type() &&
1110 ((points1.rows == 1 && points1.channels() == 2) ||
1111 points1.cols*points1.channels() == 2) &&
1112 ((points2.rows == 1 && points2.channels() == 2) ||
1113 points2.cols*points2.channels() == 2));
1115 Mat F(3, 3, CV_64F);
1116 CvMat _pt1 = Mat(points1), _pt2 = Mat(points2);
1117 CvMat _F = F, _mask, *pmask = 0;
1120 mask->resize(points1.cols*points1.rows*points1.channels()/2);
1121 pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));
1123 int n = cvFindFundamentalMat( &_pt1, &_pt2, &_F, method, param1, param2, pmask );
1132 cv::Mat cv::findHomography( const Mat& srcPoints, const Mat& dstPoints,
1133 vector<uchar>& mask, int method,
1134 double ransacReprojThreshold )
1136 return _findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, &mask);
1139 cv::Mat cv::findHomography( const Mat& srcPoints, const Mat& dstPoints,
1140 int method, double ransacReprojThreshold )
1142 return _findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, 0);
1146 cv::Mat cv::findFundamentalMat( const Mat& points1, const Mat& points2,
1147 vector<uchar>& mask, int method, double param1, double param2 )
1149 return _findFundamentalMat( points1, points2, method, param1, param2, &mask );
1152 cv::Mat cv::findFundamentalMat( const Mat& points1, const Mat& points2,
1153 int method, double param1, double param2 )
1155 return _findFundamentalMat( points1, points2, method, param1, param2, 0 );
1158 void cv::computeCorrespondEpilines( const Mat& points, int whichImage,
1159 const Mat& F, vector<Vec3f>& lines )
1161 CV_Assert(points.isContinuous() &&
1162 (points.depth() == CV_32S || points.depth() == CV_32F) &&
1163 ((points.rows == 1 && points.channels() == 2) ||
1164 points.cols*points.channels() == 2));
1166 lines.resize(points.cols*points.rows*points.channels()/2);
1167 CvMat _points = points, _lines = Mat(lines), _F = F;
1168 cvComputeCorrespondEpilines(&_points, whichImage, &_F, &_lines);
1171 void cv::convertPointsHomogeneous( const Mat& src, vector<Point3f>& dst )
1173 CV_Assert(src.isContinuous() &&
1174 (src.depth() == CV_32S || src.depth() == CV_32F) &&
1175 ((src.rows == 1 && src.channels() == 2) ||
1176 src.cols*src.channels() == 2));
1178 dst.resize(src.cols*src.rows*src.channels()/2);
1179 CvMat _src = src, _dst = Mat(dst);
1180 cvConvertPointsHomogeneous(&_src, &_dst);
1183 void cv::convertPointsHomogeneous( const Mat& src, vector<Point2f>& dst )
1185 CV_Assert(src.isContinuous() &&
1186 (src.depth() == CV_32S || src.depth() == CV_32F) &&
1187 ((src.rows == 1 && src.channels() == 3) ||
1188 src.cols*src.channels() == 3));
1190 dst.resize(src.cols*src.rows*src.channels()/3);
1191 CvMat _src = Mat(src), _dst = Mat(dst);
1192 cvConvertPointsHomogeneous(&_src, &_dst);