-/*M///////////////////////////////////////////////////////////////////////////////////////\r
-//\r
-// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
-//\r
-// By downloading, copying, installing or using the software you agree to this license.\r
-// If you do not agree to this license, do not download, install,\r
-// copy or use the software.\r
-//\r
-//\r
-// Intel License Agreement\r
-// For Open Source Computer Vision Library\r
-//\r
-// Copyright (C) 2000, Intel Corporation, all rights reserved.\r
-// Third party copyrights are property of their respective owners.\r
-//\r
-// Redistribution and use in source and binary forms, with or without modification,\r
-// are permitted provided that the following conditions are met:\r
-//\r
-// * Redistribution's of source code must retain the above copyright notice,\r
-// this list of conditions and the following disclaimer.\r
-//\r
-// * Redistribution's in binary form must reproduce the above copyright notice,\r
-// this list of conditions and the following disclaimer in the documentation\r
-// and/or other materials provided with the distribution.\r
-//\r
-// * The name of Intel Corporation may not be used to endorse or promote products\r
-// derived from this software without specific prior written permission.\r
-//\r
-// This software is provided by the copyright holders and contributors "as is" and\r
-// any express or implied warranties, including, but not limited to, the implied\r
-// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
-// In no event shall the Intel Corporation or contributors be liable for any direct,\r
-// indirect, incidental, special, exemplary, or consequential damages\r
-// (including, but not limited to, procurement of substitute goods or services;\r
-// loss of use, data, or profits; or business interruption) however caused\r
-// and on any theory of liability, whether in contract, strict liability,\r
-// or tort (including negligence or otherwise) arising in any way out of\r
-// the use of this software, even if advised of the possibility of such damage.\r
-//\r
-//M*/\r
-\r
-#include "_cv.h"\r
-#include "_cvmodelest.h"\r
-\r
-template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )\r
-{\r
- int i, j;\r
- for( i = j = 0; i < count; i++ )\r
- if( mask[i*mstep] )\r
- {\r
- if( i > j )\r
- ptr[j] = ptr[i];\r
- j++;\r
- }\r
- return j;\r
-}\r
-\r
-class CvHomographyEstimator : public CvModelEstimator2\r
-{\r
-public:\r
- CvHomographyEstimator( int modelPoints );\r
-\r
- virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );\r
- virtual bool refine( const CvMat* m1, const CvMat* m2,\r
- CvMat* model, int maxIters );\r
-protected:\r
- virtual void computeReprojError( const CvMat* m1, const CvMat* m2,\r
- const CvMat* model, CvMat* error );\r
-};\r
-\r
-\r
-CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)\r
- : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)\r
-{\r
- assert( _modelPoints == 4 || _modelPoints == 5 );\r
-}\r
-\r
-int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )\r
-{\r
- int i, count = m1->rows*m1->cols;\r
- const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;\r
- const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;\r
-\r
- double LtL[9][9], W[9][9], V[9][9];\r
- CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );\r
- CvMat _W = cvMat( 9, 9, CV_64F, W );\r
- CvMat _V = cvMat( 9, 9, CV_64F, V );\r
- CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );\r
- CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );\r
- CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};\r
-\r
- for( i = 0; i < count; i++ )\r
- {\r
- cm.x += m[i].x; cm.y += m[i].y;\r
- cM.x += M[i].x; cM.y += M[i].y;\r
- }\r
-\r
- cm.x /= count; cm.y /= count;\r
- cM.x /= count; cM.y /= count;\r
-\r
- for( i = 0; i < count; i++ )\r
- {\r
- sm.x += fabs(m[i].x - cm.x);\r
- sm.y += fabs(m[i].y - cm.y);\r
- sM.x += fabs(M[i].x - cM.x);\r
- sM.y += fabs(M[i].y - cM.y);\r
- }\r
-\r
- sm.x = count/sm.x; sm.y = count/sm.y;\r
- sM.x = count/sM.x; sM.y = count/sM.y;\r
-\r
- double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };\r
- double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };\r
- CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );\r
- CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );\r
-\r
- cvZero( &_LtL );\r
- for( i = 0; i < count; i++ )\r
- {\r
- double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;\r
- double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;\r
- double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };\r
- double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };\r
- int j, k;\r
- for( j = 0; j < 9; j++ )\r
- for( k = j; k < 9; k++ )\r
- LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];\r
- }\r
- cvCompleteSymm( &_LtL );\r
-\r
- cvSVD( &_LtL, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );\r
- cvMatMul( &_invHnorm, &_H0, &_Htemp );\r
- cvMatMul( &_Htemp, &_Hnorm2, &_H0 );\r
- cvConvertScale( &_H0, H, 1./_H0.data.db[8] );\r
-\r
- return 1;\r
-}\r
-\r
-\r
-void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,\r
- const CvMat* model, CvMat* _err )\r
-{\r
- int i, count = m1->rows*m1->cols;\r
- const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;\r
- const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;\r
- const double* H = model->data.db;\r
- float* err = _err->data.fl;\r
-\r
- for( i = 0; i < count; i++ )\r
- {\r
- double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);\r
- double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;\r
- double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;\r
- err[i] = (float)(dx*dx + dy*dy);\r
- }\r
-}\r
-\r
-bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )\r
-{\r
- CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));\r
- int i, j, k, count = m1->rows*m1->cols;\r
- const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;\r
- const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;\r
- CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );\r
- cvCopy( &modelPart, solver.param );\r
-\r
- for(;;)\r
- {\r
- const CvMat* _param = 0;\r
- CvMat *_JtJ = 0, *_JtErr = 0;\r
- double* _errNorm = 0;\r
-\r
- if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))\r
- break;\r
-\r
- for( i = 0; i < count; i++ )\r
- {\r
- const double* h = _param->data.db;\r
- double Mx = M[i].x, My = M[i].y;\r
- double ww = 1./(h[6]*Mx + h[7]*My + 1.);\r
- double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;\r
- double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;\r
- double err[] = { _xi - m[i].x, _yi - m[i].y };\r
- if( _JtJ || _JtErr )\r
- {\r
- double J[][8] =\r
- {\r
- { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },\r
- { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }\r
- };\r
-\r
- for( j = 0; j < 8; j++ )\r
- {\r
- for( k = j; k < 8; k++ )\r
- _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];\r
- _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];\r
- }\r
- }\r
- if( _errNorm )\r
- *_errNorm += err[0]*err[0] + err[1]*err[1];\r
- }\r
- }\r
-\r
- cvCopy( solver.param, &modelPart );\r
- return true;\r
-}\r
-\r
-\r
-CV_IMPL int\r
-cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,\r
- CvMat* __H, int method, double ransacReprojThreshold,\r
- CvMat* mask )\r
-{\r
- const double confidence = 0.99;\r
- bool result = false;\r
- CvMat *m = 0, *M = 0, *tempMask = 0;\r
-\r
- CV_FUNCNAME( "cvFindHomography" );\r
-\r
- __BEGIN__;\r
-\r
- double H[9];\r
- CvMat _H = cvMat( 3, 3, CV_64FC1, H );\r
- int count;\r
-\r
- CV_ASSERT( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );\r
-\r
- count = MAX(imagePoints->cols, imagePoints->rows);\r
- CV_ASSERT( count >= 4 );\r
-\r
- m = cvCreateMat( 1, count, CV_64FC2 );\r
- cvConvertPointsHomogeneous( imagePoints, m );\r
-\r
- M = cvCreateMat( 1, count, CV_64FC2 );\r
- cvConvertPointsHomogeneous( objectPoints, M );\r
-\r
- if( mask )\r
- {\r
- CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&\r
- (mask->rows == 1 || mask->cols == 1) &&\r
- mask->rows*mask->cols == count );\r
- tempMask = mask;\r
- }\r
- else if( count > 4 )\r
- tempMask = cvCreateMat( 1, count, CV_8U );\r
- if( tempMask )\r
- cvSet( tempMask, cvScalarAll(1.) );\r
-\r
- {\r
- CvHomographyEstimator estimator( MIN(count, 5) );\r
- if( count == 4 )\r
- method = 0;\r
- if( method == CV_LMEDS )\r
- result = estimator.runLMeDS( M, m, &_H, tempMask, confidence );\r
- else if( method == CV_RANSAC )\r
- result = estimator.runRANSAC( M, m, &_H, tempMask, ransacReprojThreshold, confidence );\r
- else\r
- result = estimator.runKernel( M, m, &_H ) > 0;\r
-\r
- if( result && count > 4 )\r
- {\r
- icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );\r
- count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );\r
- M->cols = m->cols = count;\r
- estimator.refine( M, m, &_H, 10 );\r
- }\r
- }\r
-\r
- if( result )\r
- cvConvert( &_H, __H );\r
-\r
- __END__;\r
-\r
- cvReleaseMat( &m );\r
- cvReleaseMat( &M );\r
- if( tempMask != mask )\r
- cvReleaseMat( &tempMask );\r
-\r
- return (int)result;\r
-}\r
-\r
-\r
-/* Evaluation of Fundamental Matrix from point correspondences.\r
- The original code has been written by Valery Mosyagin */\r
-\r
-/* The algorithms (except for RANSAC) and the notation have been taken from\r
- Zhengyou Zhang's research report\r
- "Determining the Epipolar Geometry and its Uncertainty: A Review"\r
- that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */\r
-\r
-/************************************** 7-point algorithm *******************************/\r
-class CvFMEstimator : public CvModelEstimator2\r
-{\r
-public:\r
- CvFMEstimator( int _modelPoints );\r
-\r
- virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );\r
- virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model );\r
- virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model );\r
-protected:\r
- virtual void computeReprojError( const CvMat* m1, const CvMat* m2,\r
- const CvMat* model, CvMat* error );\r
-};\r
-\r
-CvFMEstimator::CvFMEstimator( int _modelPoints )\r
-: CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 )\r
-{\r
- assert( _modelPoints == 7 || _modelPoints == 8 );\r
-}\r
-\r
-\r
-int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )\r
-{\r
- return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model );\r
-}\r
-\r
-int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )\r
-{\r
- double a[7*9], w[7], v[9*9], c[4], r[3];\r
- double* f1, *f2;\r
- double t0, t1, t2;\r
- CvMat A = cvMat( 7, 9, CV_64F, a );\r
- CvMat V = cvMat( 9, 9, CV_64F, v );\r
- CvMat W = cvMat( 7, 1, CV_64F, w );\r
- CvMat coeffs = cvMat( 1, 4, CV_64F, c );\r
- CvMat roots = cvMat( 1, 3, CV_64F, r );\r
- const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;\r
- const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;\r
- double* fmatrix = _fmatrix->data.db;\r
- int i, k, n;\r
-\r
- // form a linear system: i-th row of A(=a) represents\r
- // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0\r
- for( i = 0; i < 7; i++ )\r
- {\r
- double x0 = m1[i].x, y0 = m1[i].y;\r
- double x1 = m2[i].x, y1 = m2[i].y;\r
-\r
- a[i*9+0] = x1*x0;\r
- a[i*9+1] = x1*y0;\r
- a[i*9+2] = x1;\r
- a[i*9+3] = y1*x0;\r
- a[i*9+4] = y1*y0;\r
- a[i*9+5] = y1;\r
- a[i*9+6] = x0;\r
- a[i*9+7] = y0;\r
- a[i*9+8] = 1;\r
- }\r
-\r
- // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so\r
- // the solution is linear subspace of dimensionality 2.\r
- // => use the last two singular vectors as a basis of the space\r
- // (according to SVD properties)\r
- cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );\r
- f1 = v + 7*9;\r
- f2 = v + 8*9;\r
-\r
- // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.\r
- // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),\r
- // so f ~ lambda*f1 + (1 - lambda)*f2.\r
- // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.\r
- // it will be a cubic equation.\r
- // find c - polynomial coefficients.\r
- for( i = 0; i < 9; i++ )\r
- f1[i] -= f2[i];\r
-\r
- t0 = f2[4]*f2[8] - f2[5]*f2[7];\r
- t1 = f2[3]*f2[8] - f2[5]*f2[6];\r
- t2 = f2[3]*f2[7] - f2[4]*f2[6];\r
-\r
- c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;\r
-\r
- c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -\r
- f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +\r
- f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -\r
- f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +\r
- f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -\r
- f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +\r
- f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);\r
-\r
- t0 = f1[4]*f1[8] - f1[5]*f1[7];\r
- t1 = f1[3]*f1[8] - f1[5]*f1[6];\r
- t2 = f1[3]*f1[7] - f1[4]*f1[6];\r
-\r
- c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -\r
- f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +\r
- f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -\r
- f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +\r
- f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -\r
- f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +\r
- f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);\r
-\r
- c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;\r
-\r
- // solve the cubic equation; there can be 1 to 3 roots ...\r
- n = cvSolveCubic( &coeffs, &roots );\r
-\r
- if( n < 1 || n > 3 )\r
- return n;\r
-\r
- for( k = 0; k < n; k++, fmatrix += 9 )\r
- {\r
- // for each root form the fundamental matrix\r
- double lambda = r[k], mu = 1.;\r
- double s = f1[8]*r[k] + f2[8];\r
-\r
- // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1\r
- if( fabs(s) > DBL_EPSILON )\r
- {\r
- mu = 1./s;\r
- lambda *= mu;\r
- fmatrix[8] = 1.;\r
- }\r
- else\r
- fmatrix[8] = 0.;\r
-\r
- for( i = 0; i < 8; i++ )\r
- fmatrix[i] = f1[i]*lambda + f2[i]*mu;\r
- }\r
-\r
- return n;\r
-}\r
-\r
-\r
-int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )\r
-{\r
- double a[9*9], w[9], v[9*9];\r
- CvMat W = cvMat( 1, 9, CV_64F, w );\r
- CvMat V = cvMat( 9, 9, CV_64F, v );\r
- CvMat A = cvMat( 9, 9, CV_64F, a );\r
- CvMat U, F0, TF;\r
-\r
- CvPoint2D64f m0c = {0,0}, m1c = {0,0};\r
- double t, scale0 = 0, scale1 = 0;\r
-\r
- const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;\r
- const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;\r
- double* fmatrix = _fmatrix->data.db;\r
- int i, j, k, count = _m1->cols*_m1->rows;\r
-\r
- // compute centers and average distances for each of the two point sets\r
- for( i = 0; i < count; i++ )\r
- {\r
- double x = m1[i].x, y = m1[i].y;\r
- m0c.x += x; m0c.y += y;\r
-\r
- x = m2[i].x, y = m2[i].y;\r
- m1c.x += x; m1c.y += y;\r
- }\r
-\r
- // calculate the normalizing transformations for each of the point sets:\r
- // after the transformation each set will have the mass center at the coordinate origin\r
- // and the average distance from the origin will be ~sqrt(2).\r
- t = 1./count;\r
- m0c.x *= t; m0c.y *= t;\r
- m1c.x *= t; m1c.y *= t;\r
-\r
- for( i = 0; i < count; i++ )\r
- {\r
- double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y;\r
- scale0 += sqrt(x*x + y*y);\r
-\r
- x = fabs(m2[i].x - m1c.x), y = fabs(m2[i].y - m1c.y);\r
- scale1 += sqrt(x*x + y*y);\r
- }\r
-\r
- scale0 *= t;\r
- scale1 *= t;\r
-\r
- if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )\r
- return 0;\r
-\r
- scale0 = sqrt(2.)/scale0;\r
- scale1 = sqrt(2.)/scale1;\r
- \r
- cvZero( &A );\r
-\r
- // form a linear system Ax=0: for each selected pair of points m1 & m2,\r
- // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0\r
- // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0. \r
- for( i = 0; i < count; i++ )\r
- {\r
- double x0 = (m1[i].x - m0c.x)*scale0;\r
- double y0 = (m1[i].y - m0c.y)*scale0;\r
- double x1 = (m2[i].x - m1c.x)*scale1;\r
- double y1 = (m2[i].y - m1c.y)*scale1;\r
- double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 };\r
- for( j = 0; j < 9; j++ )\r
- for( k = 0; k < 9; k++ )\r
- a[j*9+k] += r[j]*r[k];\r
- }\r
-\r
- cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );\r
-\r
- for( i = 0; i < 8; i++ )\r
- {\r
- if( fabs(w[i]) < DBL_EPSILON )\r
- break;\r
- }\r
-\r
- if( i < 7 )\r
- return 0;\r
-\r
- F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0\r
-\r
- // make F0 singular (of rank 2) by decomposing it with SVD,\r
- // zeroing the last diagonal element of W and then composing the matrices back.\r
-\r
- // use v as a temporary storage for different 3x3 matrices\r
- W = U = V = TF = F0;\r
- W.data.db = v;\r
- U.data.db = v + 9;\r
- V.data.db = v + 18;\r
- TF.data.db = v + 27;\r
-\r
- cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );\r
- W.data.db[8] = 0.;\r
-\r
- // F0 <- U*diag([W(1), W(2), 0])*V'\r
- cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );\r
- cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );\r
-\r
- // apply the transformation that is inverse\r
- // to what we used to normalize the point coordinates\r
- {\r
- double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };\r
- double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };\r
- CvMat T0, T1;\r
- T0 = T1 = F0;\r
- T0.data.db = tt0;\r
- T1.data.db = tt1;\r
-\r
- // F0 <- T1'*F0*T0\r
- cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );\r
- F0.data.db = fmatrix;\r
- cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );\r
-\r
- // make F(3,3) = 1\r
- if( fabs(F0.data.db[8]) > FLT_EPSILON )\r
- cvScale( &F0, &F0, 1./F0.data.db[8] );\r
- }\r
-\r
- return 1;\r
-}\r
-\r
-\r
-void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2,\r
- const CvMat* model, CvMat* _err )\r
-{\r
- int i, count = _m1->rows*_m1->cols;\r
- const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;\r
- const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;\r
- const double* F = model->data.db;\r
- float* err = _err->data.fl;\r
- \r
- for( i = 0; i < count; i++ )\r
- {\r
- double a, b, c, d1, d2, s1, s2;\r
-\r
- a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];\r
- b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];\r
- c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];\r
-\r
- s2 = 1./(a*a + b*b);\r
- d2 = m2[i].x*a + m2[i].y*b + c;\r
-\r
- a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];\r
- b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];\r
- c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];\r
-\r
- s1 = 1./(a*a + b*b);\r
- d1 = m1[i].x*a + m1[i].y*b + c;\r
-\r
- err[i] = (float)std::max(d1*d1*s1, d2*d2*s2);\r
- }\r
-}\r
-\r
-\r
-CV_IMPL int\r
-cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,\r
- CvMat* fmatrix, int method,\r
- double param1, double param2, CvMat* mask )\r
-{\r
- int result = 0;\r
- CvMat *m1 = 0, *m2 = 0, *tempMask = 0;\r
-\r
- CV_FUNCNAME( "cvFindFundamentalMat" );\r
-\r
- __BEGIN__;\r
-\r
- double F[3*9];\r
- CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );\r
- int count;\r
-\r
- CV_ASSERT( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );\r
- CV_ASSERT( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&\r
- (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) );\r
-\r
- count = MAX(points1->cols, points1->rows);\r
- if( count < 7 )\r
- EXIT;\r
-\r
- m1 = cvCreateMat( 1, count, CV_64FC2 );\r
- cvConvertPointsHomogeneous( points1, m1 );\r
-\r
- m2 = cvCreateMat( 1, count, CV_64FC2 );\r
- cvConvertPointsHomogeneous( points2, m2 );\r
-\r
- if( mask )\r
- {\r
- CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&\r
- (mask->rows == 1 || mask->cols == 1) &&\r
- mask->rows*mask->cols == count );\r
- tempMask = cvCreateMatHeader(1, count, CV_8U);\r
- cvSetData(tempMask, mask->data.ptr, 0);\r
- }\r
- else if( count > 8 )\r
- tempMask = cvCreateMat( 1, count, CV_8U );\r
- if( tempMask )\r
- cvSet( tempMask, cvScalarAll(1.) );\r
-\r
- {\r
- CvFMEstimator estimator( MIN(count, (method & 3) == CV_FM_7POINT ? 7 : 8) );\r
- if( count == 7 )\r
- result = estimator.run7Point(m1, m2, &_F9x3);\r
- else if( count == 8 || method == CV_FM_8POINT )\r
- result = estimator.run8Point(m1, m2, &_F3x3);\r
- else if( count > 8 )\r
- {\r
- if( param1 <= 0 )\r
- param1 = 3;\r
- if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )\r
- param2 = 0.99;\r
- \r
- if( (method & ~3) == CV_RANSAC )\r
- result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 );\r
- else\r
- result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );\r
- if( result <= 0 )\r
- EXIT;\r
- /*icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );\r
- count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count );\r
- assert( count >= 8 );\r
- m1->cols = m2->cols = count;\r
- estimator.run8Point(m1, m2, &_F3x3);*/\r
- }\r
- }\r
-\r
- if( result )\r
- cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );\r
-\r
- __END__;\r
-\r
- cvReleaseMat( &m1 );\r
- cvReleaseMat( &m2 );\r
- if( tempMask != mask )\r
- cvReleaseMat( &tempMask );\r
-\r
- return result;\r
-}\r
-\r
-\r
-CV_IMPL void\r
-cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,\r
- const CvMat* fmatrix, CvMat* lines )\r
-{\r
- CV_FUNCNAME( "cvComputeCorrespondEpilines" );\r
-\r
- __BEGIN__;\r
-\r
- int abc_stride, abc_plane_stride, abc_elem_size;\r
- int plane_stride, stride, elem_size;\r
- int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;\r
- uchar *ap, *bp, *cp;\r
- const uchar *xp, *yp, *zp;\r
- double f[9];\r
- CvMat F = cvMat( 3, 3, CV_64F, f );\r
-\r
- if( !CV_IS_MAT(points) )\r
- CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );\r
-\r
- depth = CV_MAT_DEPTH(points->type);\r
- cn = CV_MAT_CN(points->type);\r
- if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) )\r
- CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );\r
-\r
- if( points->rows > points->cols )\r
- {\r
- dims = cn*points->cols;\r
- count = points->rows;\r
- }\r
- else\r
- {\r
- if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) )\r
- CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );\r
- dims = cn * points->rows;\r
- count = points->cols;\r
- }\r
-\r
- if( dims != 2 && dims != 3 )\r
- CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );\r
-\r
- if( !CV_IS_MAT(fmatrix) )\r
- CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );\r
-\r
- if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )\r
- CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );\r
-\r
- if( fmatrix->cols != 3 || fmatrix->rows != 3 )\r
- CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" );\r
-\r
- if( !CV_IS_MAT(lines) )\r
- CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );\r
-\r
- abc_depth = CV_MAT_DEPTH(lines->type);\r
- abc_cn = CV_MAT_CN(lines->type);\r
- if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) )\r
- CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );\r
-\r
- if( lines->rows > lines->cols )\r
- {\r
- abc_dims = abc_cn*lines->cols;\r
- abc_count = lines->rows;\r
- }\r
- else\r
- {\r
- if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) )\r
- CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );\r
- abc_dims = abc_cn * lines->rows;\r
- abc_count = lines->cols;\r
- }\r
-\r
- if( abc_dims != 3 )\r
- CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );\r
-\r
- if( abc_count != count )\r
- CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );\r
-\r
- elem_size = CV_ELEM_SIZE(depth);\r
- abc_elem_size = CV_ELEM_SIZE(abc_depth);\r
-\r
- if( points->rows == dims )\r
- {\r
- plane_stride = points->step;\r
- stride = elem_size;\r
- }\r
- else\r
- {\r
- plane_stride = elem_size;\r
- stride = points->rows == 1 ? dims*elem_size : points->step;\r
- }\r
-\r
- if( lines->rows == 3 )\r
- {\r
- abc_plane_stride = lines->step;\r
- abc_stride = abc_elem_size;\r
- }\r
- else\r
- {\r
- abc_plane_stride = abc_elem_size;\r
- abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;\r
- }\r
-\r
- CV_CALL( cvConvert( fmatrix, &F ));\r
- if( pointImageID == 2 )\r
- cvTranspose( &F, &F );\r
-\r
- xp = points->data.ptr;\r
- yp = xp + plane_stride;\r
- zp = dims == 3 ? yp + plane_stride : 0;\r
-\r
- ap = lines->data.ptr;\r
- bp = ap + abc_plane_stride;\r
- cp = bp + abc_plane_stride;\r
-\r
- for( i = 0; i < count; i++ )\r
- {\r
- double x, y, z = 1.;\r
- double a, b, c, nu;\r
-\r
- if( depth == CV_32F )\r
- {\r
- x = *(float*)xp; y = *(float*)yp;\r
- if( zp )\r
- z = *(float*)zp, zp += stride;\r
- }\r
- else\r
- {\r
- x = *(double*)xp; y = *(double*)yp;\r
- if( zp )\r
- z = *(double*)zp, zp += stride;\r
- }\r
-\r
- xp += stride; yp += stride;\r
-\r
- a = f[0]*x + f[1]*y + f[2]*z;\r
- b = f[3]*x + f[4]*y + f[5]*z;\r
- c = f[6]*x + f[7]*y + f[8]*z;\r
- nu = a*a + b*b;\r
- nu = nu ? 1./sqrt(nu) : 1.;\r
- a *= nu; b *= nu; c *= nu;\r
-\r
- if( abc_depth == CV_32F )\r
- {\r
- *(float*)ap = (float)a;\r
- *(float*)bp = (float)b;\r
- *(float*)cp = (float)c;\r
- }\r
- else\r
- {\r
- *(double*)ap = a;\r
- *(double*)bp = b;\r
- *(double*)cp = c;\r
- }\r
-\r
- ap += abc_stride;\r
- bp += abc_stride;\r
- cp += abc_stride;\r
- }\r
-\r
- __END__;\r
-}\r
-\r
-\r
-CV_IMPL void\r
-cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )\r
-{\r
- CvMat* temp = 0;\r
- CvMat* denom = 0;\r
-\r
- CV_FUNCNAME( "cvConvertPointsHomogeneous" );\r
-\r
- __BEGIN__;\r
-\r
- int i, s_count, s_dims, d_count, d_dims;\r
- CvMat _src, _dst, _ones;\r
- CvMat* ones = 0;\r
-\r
- if( !CV_IS_MAT(src) )\r
- CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,\r
- "The input parameter is not a valid matrix" );\r
-\r
- if( !CV_IS_MAT(dst) )\r
- CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,\r
- "The output parameter is not a valid matrix" );\r
-\r
- if( src == dst || src->data.ptr == dst->data.ptr )\r
- {\r
- if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )\r
- CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );\r
- EXIT;\r
- }\r
-\r
- if( src->rows > src->cols )\r
- {\r
- if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )\r
- CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );\r
-\r
- s_dims = CV_MAT_CN(src->type)*src->cols;\r
- s_count = src->rows;\r
- }\r
- else\r
- {\r
- if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )\r
- CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );\r
-\r
- s_dims = CV_MAT_CN(src->type)*src->rows;\r
- s_count = src->cols;\r
- }\r
-\r
- if( src->rows == 1 || src->cols == 1 )\r
- src = cvReshape( src, &_src, 1, s_count );\r
-\r
- if( dst->rows > dst->cols )\r
- {\r
- if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )\r
- CV_ERROR( CV_StsBadSize,\r
- "Either the number of channels or columns or rows in the input matrix must be =1" );\r
-\r
- d_dims = CV_MAT_CN(dst->type)*dst->cols;\r
- d_count = dst->rows;\r
- }\r
- else\r
- {\r
- if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )\r
- CV_ERROR( CV_StsBadSize,\r
- "Either the number of channels or columns or rows in the output matrix must be =1" );\r
-\r
- d_dims = CV_MAT_CN(dst->type)*dst->rows;\r
- d_count = dst->cols;\r
- }\r
-\r
- if( dst->rows == 1 || dst->cols == 1 )\r
- dst = cvReshape( dst, &_dst, 1, d_count );\r
-\r
- if( s_count != d_count )\r
- CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );\r
-\r
- if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )\r
- CV_ERROR( CV_StsUnsupportedFormat,\r
- "Both matrices must be floating-point (single or double precision)" );\r
-\r
- if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )\r
- CV_ERROR( CV_StsOutOfRange,\r
- "Both input and output point dimensionality must be 2, 3 or 4" );\r
-\r
- if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )\r
- CV_ERROR( CV_StsUnmatchedSizes,\r
- "The dimensionalities of input and output point sets differ too much" );\r
-\r
- if( s_dims == d_dims - 1 )\r
- {\r
- if( d_count == dst->rows )\r
- {\r
- ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));\r
- dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));\r
- }\r
- else\r
- {\r
- ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));\r
- dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));\r
- }\r
- }\r
-\r
- if( s_dims <= d_dims )\r
- {\r
- if( src->rows == dst->rows && src->cols == dst->cols )\r
- {\r
- if( CV_ARE_TYPES_EQ( src, dst ) )\r
- cvCopy( src, dst );\r
- else\r
- cvConvert( src, dst );\r
- }\r
- else\r
- {\r
- if( !CV_ARE_TYPES_EQ( src, dst ))\r
- {\r
- CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));\r
- cvConvert( src, temp );\r
- src = temp;\r
- }\r
- cvTranspose( src, dst );\r
- }\r
-\r
- if( ones )\r
- cvSet( ones, cvRealScalar(1.) );\r
- }\r
- else\r
- {\r
- int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;\r
-\r
- if( !CV_ARE_TYPES_EQ( src, dst ))\r
- {\r
- CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));\r
- cvConvert( src, temp );\r
- src = temp;\r
- }\r
-\r
- elem_size = CV_ELEM_SIZE(src->type);\r
-\r
- if( s_count == src->cols )\r
- s_plane_stride = src->step / elem_size, s_stride = 1;\r
- else\r
- s_stride = src->step / elem_size, s_plane_stride = 1;\r
-\r
- if( d_count == dst->cols )\r
- d_plane_stride = dst->step / elem_size, d_stride = 1;\r
- else\r
- d_stride = dst->step / elem_size, d_plane_stride = 1;\r
-\r
- CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));\r
-\r
- if( CV_MAT_DEPTH(dst->type) == CV_32F )\r
- {\r
- const float* xs = src->data.fl;\r
- const float* ys = xs + s_plane_stride;\r
- const float* zs = 0;\r
- const float* ws = xs + (s_dims - 1)*s_plane_stride;\r
-\r
- float* iw = denom->data.fl;\r
-\r
- float* xd = dst->data.fl;\r
- float* yd = xd + d_plane_stride;\r
- float* zd = 0;\r
-\r
- if( d_dims == 3 )\r
- {\r
- zs = ys + s_plane_stride;\r
- zd = yd + d_plane_stride;\r
- }\r
-\r
- for( i = 0; i < d_count; i++, ws += s_stride )\r
- {\r
- float t = *ws;\r
- iw[i] = fabs((double)t) > FLT_EPSILON ? t : 1.f;\r
- }\r
-\r
- cvDiv( 0, denom, denom );\r
-\r
- if( d_dims == 3 )\r
- for( i = 0; i < d_count; i++ )\r
- {\r
- float w = iw[i];\r
- float x = *xs * w, y = *ys * w, z = *zs * w;\r
- xs += s_stride; ys += s_stride; zs += s_stride;\r
- *xd = x; *yd = y; *zd = z;\r
- xd += d_stride; yd += d_stride; zd += d_stride;\r
- }\r
- else\r
- for( i = 0; i < d_count; i++ )\r
- {\r
- float w = iw[i];\r
- float x = *xs * w, y = *ys * w;\r
- xs += s_stride; ys += s_stride;\r
- *xd = x; *yd = y;\r
- xd += d_stride; yd += d_stride;\r
- }\r
- }\r
- else\r
- {\r
- const double* xs = src->data.db;\r
- const double* ys = xs + s_plane_stride;\r
- const double* zs = 0;\r
- const double* ws = xs + (s_dims - 1)*s_plane_stride;\r
-\r
- double* iw = denom->data.db;\r
-\r
- double* xd = dst->data.db;\r
- double* yd = xd + d_plane_stride;\r
- double* zd = 0;\r
-\r
- if( d_dims == 3 )\r
- {\r
- zs = ys + s_plane_stride;\r
- zd = yd + d_plane_stride;\r
- }\r
-\r
- for( i = 0; i < d_count; i++, ws += s_stride )\r
- {\r
- double t = *ws;\r
- iw[i] = fabs(t) > DBL_EPSILON ? t : 1.;\r
- }\r
-\r
- cvDiv( 0, denom, denom );\r
-\r
- if( d_dims == 3 )\r
- for( i = 0; i < d_count; i++ )\r
- {\r
- double w = iw[i];\r
- double x = *xs * w, y = *ys * w, z = *zs * w;\r
- xs += s_stride; ys += s_stride; zs += s_stride;\r
- *xd = x; *yd = y; *zd = z;\r
- xd += d_stride; yd += d_stride; zd += d_stride;\r
- }\r
- else\r
- for( i = 0; i < d_count; i++ )\r
- {\r
- double w = iw[i];\r
- double x = *xs * w, y = *ys * w;\r
- xs += s_stride; ys += s_stride;\r
- *xd = x; *yd = y;\r
- xd += d_stride; yd += d_stride;\r
- }\r
- }\r
- }\r
-\r
- __END__;\r
-\r
- cvReleaseMat( &denom );\r
- cvReleaseMat( &temp );\r
-}\r
-\r
-namespace cv\r
-{\r
-\r
-static Mat _findHomography( const Vector<Point2f>& srcPoints,\r
- const Vector<Point2f>& dstPoints,\r
- int method, double ransacReprojThreshold,\r
- Vector<bool>* mask )\r
-{\r
- Mat H(3, 3, CV_64F);\r
- CvMat _pt1 = srcPoints, _pt2 = dstPoints;\r
- CvMat _H = H, _mask, *pmask = 0;\r
- if( mask )\r
- {\r
- mask->resize(srcPoints.size());\r
- pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));\r
- }\r
- bool ok = cvFindHomography( &_pt1, &_pt2, &_H, method, ransacReprojThreshold, pmask ) > 0;\r
- if( !ok )\r
- H = Scalar(0);\r
- return H;\r
-}\r
-\r
-Mat findHomography( const Vector<Point2f>& srcPoints,\r
- const Vector<Point2f>& dstPoints,\r
- Vector<bool>& mask, int method,\r
- double ransacReprojThreshold )\r
-{\r
- return _findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, &mask);\r
-}\r
-\r
-Mat findHomography( const Vector<Point2f>& srcPoints,\r
- const Vector<Point2f>& dstPoints,\r
- int method, double ransacReprojThreshold )\r
-{\r
- return _findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, 0);\r
-}\r
-\r
-static Mat _findFundamentalMat( const Vector<Point2f>& points1,\r
- const Vector<Point2f>& points2,\r
- int method, double param1, double param2,\r
- Vector<bool>* mask )\r
-{\r
- Mat F(3, 3, CV_64F);\r
- CvMat _pt1 = points1, _pt2 = points2;\r
- CvMat _F = F, _mask, *pmask = 0;\r
- if( mask )\r
- {\r
- mask->resize(points1.size());\r
- pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));\r
- }\r
- int n = cvFindFundamentalMat( &_pt1, &_pt2, &_F, method, param1, param2, pmask );\r
- if( n <= 0 )\r
- F = Scalar(0);\r
- return F;\r
-}\r
-\r
-Mat findFundamentalMat( const Vector<Point2f>& points1,\r
- const Vector<Point2f>& points2,\r
- Vector<bool>& mask,\r
- int method, double param1, double param2 )\r
-{\r
- return _findFundamentalMat( points1, points2, method, param1, param2, &mask );\r
-}\r
-\r
-Mat findFundamentalMat( const Vector<Point2f>& points1,\r
- const Vector<Point2f>& points2,\r
- int method, double param1, double param2 )\r
-{\r
- return _findFundamentalMat( points1, points2, method, param1, param2, 0 );\r
-}\r
-\r
-void computeCorrespondEpilines( const Vector<Point2f>& points,\r
- int whichImage, const Mat& F,\r
- Vector<Vec3f>& lines )\r
-{\r
- lines.resize(points.size());\r
- CvMat _points = points, _lines = lines, _F = F;\r
- cvComputeCorrespondEpilines(&_points, whichImage, &_F, &_lines);\r
-}\r
-\r
-void convertPointsHomogeneous( const Vector<Point2f>& src,\r
- Vector<Point3f>& dst )\r
-{\r
- dst.resize(src.size());\r
- CvMat _src = src, _dst = dst;\r
- cvConvertPointsHomogeneous(&_src, &_dst);\r
-}\r
-\r
-void convertPointsHomogeneous( const Vector<Point3f>& src,\r
- Vector<Point2f>& dst )\r
-{\r
- dst.resize(src.size());\r
- CvMat _src = src, _dst = dst;\r
- cvConvertPointsHomogeneous(&_src, &_dst);\r
-}\r
-\r
-}\r
-\r
-/* End of file. */\r
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// Intel License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of Intel Corporation may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "_cv.h"
+#include "_cvmodelest.h"
+
+using namespace cv;
+
+template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )
+{
+ int i, j;
+ for( i = j = 0; i < count; i++ )
+ if( mask[i*mstep] )
+ {
+ if( i > j )
+ ptr[j] = ptr[i];
+ j++;
+ }
+ return j;
+}
+
+class CvHomographyEstimator : public CvModelEstimator2
+{
+public:
+ CvHomographyEstimator( int modelPoints );
+
+ virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
+ virtual bool refine( const CvMat* m1, const CvMat* m2,
+ CvMat* model, int maxIters );
+protected:
+ virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
+ const CvMat* model, CvMat* error );
+};
+
+
+CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)
+ : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)
+{
+ assert( _modelPoints == 4 || _modelPoints == 5 );
+ checkPartialSubsets = false;
+}
+
+int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
+{
+ int i, count = m1->rows*m1->cols;
+ const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
+ const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
+
+ double LtL[9][9], W[9][9], V[9][9];
+ CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
+ CvMat matW = cvMat( 9, 9, CV_64F, W );
+ CvMat matV = cvMat( 9, 9, CV_64F, V );
+ CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );
+ CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );
+ CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};
+
+ for( 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( i = 0; i < count; i++ )
+ {
+ sm.x += fabs(m[i].x - cm.x);
+ sm.y += fabs(m[i].y - cm.y);
+ sM.x += fabs(M[i].x - cM.x);
+ sM.y += fabs(M[i].y - cM.y);
+ }
+
+ if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
+ fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_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[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
+ double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
+ CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );
+ CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
+
+ cvZero( &_LtL );
+ for( 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][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
+ }
+ cvCompleteSymm( &_LtL );
+
+ //cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
+ cvEigenVV( &_LtL, &matV, &matW );
+ cvMatMul( &_invHnorm, &_H0, &_Htemp );
+ cvMatMul( &_Htemp, &_Hnorm2, &_H0 );
+ cvConvertScale( &_H0, H, 1./_H0.data.db[8] );
+
+ return 1;
+}
+
+
+void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
+ const CvMat* model, CvMat* _err )
+{
+ int i, count = m1->rows*m1->cols;
+ const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
+ const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
+ const double* H = model->data.db;
+ float* err = _err->data.fl;
+
+ for( i = 0; i < count; i++ )
+ {
+ double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
+ double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;
+ double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;
+ err[i] = (float)(dx*dx + dy*dy);
+ }
+}
+
+bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )
+{
+ CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
+ int i, j, k, count = m1->rows*m1->cols;
+ const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
+ const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
+ CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );
+ cvCopy( &modelPart, solver.param );
+
+ for(;;)
+ {
+ const CvMat* _param = 0;
+ CvMat *_JtJ = 0, *_JtErr = 0;
+ double* _errNorm = 0;
+
+ if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))
+ break;
+
+ for( i = 0; i < count; i++ )
+ {
+ const double* h = _param->data.db;
+ double Mx = M[i].x, My = M[i].y;
+ double ww = 1./(h[6]*Mx + h[7]*My + 1.);
+ double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;
+ double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;
+ double err[] = { _xi - m[i].x, _yi - m[i].y };
+ if( _JtJ || _JtErr )
+ {
+ double J[][8] =
+ {
+ { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
+ { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
+ };
+
+ for( j = 0; j < 8; j++ )
+ {
+ for( k = j; k < 8; k++ )
+ _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];
+ _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];
+ }
+ }
+ if( _errNorm )
+ *_errNorm += err[0]*err[0] + err[1]*err[1];
+ }
+ }
+
+ cvCopy( solver.param, &modelPart );
+ return true;
+}
+
+
+CV_IMPL int
+cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
+ CvMat* __H, int method, double ransacReprojThreshold,
+ CvMat* mask )
+{
+ const double confidence = 0.995;
+ const int maxIters = 2000;
+ bool result = false;
+ Ptr<CvMat> m, M, tempMask;
+
+ double H[9];
+ CvMat matH = cvMat( 3, 3, CV_64FC1, H );
+ int count;
+
+ CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
+
+ count = MAX(imagePoints->cols, imagePoints->rows);
+ CV_Assert( count >= 4 );
+
+ m = cvCreateMat( 1, count, CV_64FC2 );
+ cvConvertPointsHomogeneous( imagePoints, m );
+
+ M = cvCreateMat( 1, count, CV_64FC2 );
+ cvConvertPointsHomogeneous( objectPoints, M );
+
+ if( mask )
+ {
+ CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
+ (mask->rows == 1 || mask->cols == 1) &&
+ mask->rows*mask->cols == count );
+ tempMask = cvCloneMat(mask);
+ }
+ else if( count > 4 )
+ tempMask = cvCreateMat( 1, count, CV_8U );
+ if( !tempMask.empty() )
+ cvSet( tempMask, cvScalarAll(1.) );
+
+ CvHomographyEstimator estimator( MIN(count, 4) );
+ if( count == 4 )
+ method = 0;
+ if( method == CV_LMEDS )
+ result = estimator.runLMeDS( M, m, &matH, tempMask, confidence, maxIters );
+ else if( method == CV_RANSAC )
+ result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);
+ else
+ result = estimator.runKernel( M, m, &matH ) > 0;
+
+ if( result && count > 4 )
+ {
+ icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
+ count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
+ M->cols = m->cols = count;
+ estimator.refine( M, m, &matH, 10 );
+ }
+
+ if( result )
+ cvConvert( &matH, __H );
+
+ if( mask && tempMask )
+ cvCopy( tempMask, mask );
+
+ return (int)result;
+}
+
+
+/* Evaluation of Fundamental Matrix from point correspondences.
+ The original code has been written by Valery Mosyagin */
+
+/* The algorithms (except for RANSAC) and the notation have been taken from
+ Zhengyou Zhang's research report
+ "Determining the Epipolar Geometry and its Uncertainty: A Review"
+ that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
+
+/************************************** 7-point algorithm *******************************/
+class CvFMEstimator : public CvModelEstimator2
+{
+public:
+ CvFMEstimator( int _modelPoints );
+
+ virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
+ virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model );
+ virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model );
+protected:
+ virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
+ const CvMat* model, CvMat* error );
+};
+
+CvFMEstimator::CvFMEstimator( int _modelPoints )
+: CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 )
+{
+ assert( _modelPoints == 7 || _modelPoints == 8 );
+}
+
+
+int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
+{
+ return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model );
+}
+
+int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
+{
+ double a[7*9], w[7], v[9*9], c[4], r[3];
+ double* f1, *f2;
+ double t0, t1, t2;
+ CvMat A = cvMat( 7, 9, CV_64F, a );
+ CvMat V = cvMat( 9, 9, CV_64F, v );
+ CvMat W = cvMat( 7, 1, CV_64F, w );
+ CvMat coeffs = cvMat( 1, 4, CV_64F, c );
+ CvMat roots = cvMat( 1, 3, CV_64F, r );
+ const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
+ const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
+ double* fmatrix = _fmatrix->data.db;
+ int i, k, n;
+
+ // form a linear system: i-th row of A(=a) represents
+ // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0
+ for( i = 0; i < 7; i++ )
+ {
+ double x0 = m1[i].x, y0 = m1[i].y;
+ double x1 = m2[i].x, y1 = m2[i].y;
+
+ a[i*9+0] = x1*x0;
+ a[i*9+1] = x1*y0;
+ a[i*9+2] = x1;
+ a[i*9+3] = y1*x0;
+ a[i*9+4] = y1*y0;
+ a[i*9+5] = y1;
+ a[i*9+6] = x0;
+ a[i*9+7] = y0;
+ a[i*9+8] = 1;
+ }
+
+ // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so
+ // the solution is linear subspace of dimensionality 2.
+ // => use the last two singular vectors as a basis of the space
+ // (according to SVD properties)
+ cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
+ f1 = v + 7*9;
+ f2 = v + 8*9;
+
+ // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
+ // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
+ // so f ~ lambda*f1 + (1 - lambda)*f2.
+ // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
+ // it will be a cubic equation.
+ // find c - polynomial coefficients.
+ for( i = 0; i < 9; i++ )
+ f1[i] -= f2[i];
+
+ t0 = f2[4]*f2[8] - f2[5]*f2[7];
+ t1 = f2[3]*f2[8] - f2[5]*f2[6];
+ t2 = f2[3]*f2[7] - f2[4]*f2[6];
+
+ c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;
+
+ c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
+ f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
+ f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
+ f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
+ f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
+ f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
+ f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);
+
+ t0 = f1[4]*f1[8] - f1[5]*f1[7];
+ t1 = f1[3]*f1[8] - f1[5]*f1[6];
+ t2 = f1[3]*f1[7] - f1[4]*f1[6];
+
+ c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -
+ f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
+ f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
+ f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
+ f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
+ f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
+ f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);
+
+ c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;
+
+ // solve the cubic equation; there can be 1 to 3 roots ...
+ n = cvSolveCubic( &coeffs, &roots );
+
+ if( n < 1 || n > 3 )
+ return n;
+
+ for( k = 0; k < n; k++, fmatrix += 9 )
+ {
+ // for each root form the fundamental matrix
+ double lambda = r[k], mu = 1.;
+ double s = f1[8]*r[k] + f2[8];
+
+ // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1
+ if( fabs(s) > DBL_EPSILON )
+ {
+ mu = 1./s;
+ lambda *= mu;
+ fmatrix[8] = 1.;
+ }
+ else
+ fmatrix[8] = 0.;
+
+ for( i = 0; i < 8; i++ )
+ fmatrix[i] = f1[i]*lambda + f2[i]*mu;
+ }
+
+ return n;
+}
+
+
+int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
+{
+ double a[9*9], w[9], v[9*9];
+ CvMat W = cvMat( 1, 9, CV_64F, w );
+ CvMat V = cvMat( 9, 9, CV_64F, v );
+ CvMat A = cvMat( 9, 9, CV_64F, a );
+ CvMat U, F0, TF;
+
+ CvPoint2D64f m0c = {0,0}, m1c = {0,0};
+ double t, scale0 = 0, scale1 = 0;
+
+ const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
+ const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
+ double* fmatrix = _fmatrix->data.db;
+ int i, j, k, count = _m1->cols*_m1->rows;
+
+ // 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;
+ m0c.x += x; m0c.y += y;
+
+ x = m2[i].x, y = m2[i].y;
+ m1c.x += x; m1c.y += y;
+ }
+
+ // 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;
+ m0c.x *= t; m0c.y *= t;
+ m1c.x *= t; m1c.y *= t;
+
+ for( i = 0; i < count; i++ )
+ {
+ double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y;
+ scale0 += sqrt(x*x + y*y);
+
+ x = fabs(m2[i].x - m1c.x), y = fabs(m2[i].y - m1c.y);
+ scale1 += sqrt(x*x + y*y);
+ }
+
+ scale0 *= t;
+ scale1 *= t;
+
+ if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
+ return 0;
+
+ scale0 = sqrt(2.)/scale0;
+ scale1 = sqrt(2.)/scale1;
+
+ cvZero( &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
+ // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0.
+ for( i = 0; i < count; i++ )
+ {
+ double x0 = (m1[i].x - m0c.x)*scale0;
+ double y0 = (m1[i].y - m0c.y)*scale0;
+ double x1 = (m2[i].x - m1c.x)*scale1;
+ double y1 = (m2[i].y - m1c.y)*scale1;
+ double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 };
+ for( j = 0; j < 9; j++ )
+ for( k = 0; k < 9; k++ )
+ a[j*9+k] += r[j]*r[k];
+ }
+
+ cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
+
+ for( i = 0; i < 8; i++ )
+ {
+ if( fabs(w[i]) < DBL_EPSILON )
+ break;
+ }
+
+ if( i < 7 )
+ return 0;
+
+ F0 = cvMat( 3, 3, CV_64F, v + 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.data.db = v;
+ U.data.db = v + 9;
+ V.data.db = v + 18;
+ TF.data.db = v + 27;
+
+ cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
+ W.data.db[8] = 0.;
+
+ // F0 <- U*diag([W(1), W(2), 0])*V'
+ cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );
+ cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );
+
+ // apply the transformation that is inverse
+ // to what we used to normalize the point coordinates
+ {
+ double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };
+ double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
+ CvMat T0, T1;
+ T0 = T1 = F0;
+ T0.data.db = tt0;
+ T1.data.db = tt1;
+
+ // F0 <- T1'*F0*T0
+ cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );
+ F0.data.db = fmatrix;
+ cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );
+
+ // make F(3,3) = 1
+ if( fabs(F0.data.db[8]) > FLT_EPSILON )
+ cvScale( &F0, &F0, 1./F0.data.db[8] );
+ }
+
+ return 1;
+}
+
+
+void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2,
+ const CvMat* model, CvMat* _err )
+{
+ int i, count = _m1->rows*_m1->cols;
+ const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
+ const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
+ const double* F = model->data.db;
+ float* err = _err->data.fl;
+
+ for( i = 0; i < count; i++ )
+ {
+ double a, b, c, d1, d2, s1, s2;
+
+ a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];
+ b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];
+ c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];
+
+ s2 = 1./(a*a + b*b);
+ d2 = m2[i].x*a + m2[i].y*b + c;
+
+ a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];
+ b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];
+ c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];
+
+ s1 = 1./(a*a + b*b);
+ d1 = m1[i].x*a + m1[i].y*b + c;
+
+ err[i] = (float)std::max(d1*d1*s1, d2*d2*s2);
+ }
+}
+
+
+CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
+ CvMat* fmatrix, int method,
+ double param1, double param2, CvMat* mask )
+{
+ int result = 0;
+ Ptr<CvMat> m1, m2, tempMask;
+
+ double F[3*9];
+ CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );
+ int count;
+
+ CV_Assert( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );
+ CV_Assert( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&
+ (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) );
+
+ count = MAX(points1->cols, points1->rows);
+ if( count < 7 )
+ return 0;
+
+ m1 = cvCreateMat( 1, count, CV_64FC2 );
+ cvConvertPointsHomogeneous( points1, m1 );
+
+ m2 = cvCreateMat( 1, count, CV_64FC2 );
+ cvConvertPointsHomogeneous( points2, m2 );
+
+ if( mask )
+ {
+ CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
+ (mask->rows == 1 || mask->cols == 1) &&
+ mask->rows*mask->cols == count );
+ tempMask = cvCloneMat(mask);
+ }
+ else if( count > 8 )
+ tempMask = cvCreateMat( 1, count, CV_8U );
+ if( !tempMask.empty() )
+ cvSet( tempMask, cvScalarAll(1.) );
+
+ CvFMEstimator estimator( MIN(count, (method & 3) == CV_FM_7POINT ? 7 : 8) );
+ if( count == 7 )
+ result = estimator.run7Point(m1, m2, &_F9x3);
+ else if( count == 8 || method == CV_FM_8POINT )
+ result = estimator.run8Point(m1, m2, &_F3x3);
+ else if( count > 8 )
+ {
+ if( param1 <= 0 )
+ param1 = 3;
+ if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
+ param2 = 0.99;
+
+ if( (method & ~3) == CV_RANSAC )
+ result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 );
+ else
+ result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );
+ if( result <= 0 )
+ return 0;
+ /*icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );
+ count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count );
+ assert( count >= 8 );
+ m1->cols = m2->cols = count;
+ estimator.run8Point(m1, m2, &_F3x3);*/
+ }
+
+ if( result )
+ cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );
+
+ if( mask && tempMask )
+ cvCopy( tempMask, mask );
+
+ return result;
+}
+
+
+CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
+ const CvMat* fmatrix, CvMat* lines )
+{
+ int abc_stride, abc_plane_stride, abc_elem_size;
+ int plane_stride, stride, elem_size;
+ int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
+ uchar *ap, *bp, *cp;
+ const uchar *xp, *yp, *zp;
+ double f[9];
+ CvMat F = cvMat( 3, 3, CV_64F, f );
+
+ if( !CV_IS_MAT(points) )
+ CV_Error( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
+
+ depth = CV_MAT_DEPTH(points->type);
+ cn = CV_MAT_CN(points->type);
+ if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) )
+ CV_Error( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
+
+ if( points->rows > points->cols )
+ {
+ dims = cn*points->cols;
+ count = points->rows;
+ }
+ else
+ {
+ if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) )
+ CV_Error( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
+ dims = cn * points->rows;
+ count = points->cols;
+ }
+
+ if( dims != 2 && dims != 3 )
+ CV_Error( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
+
+ if( !CV_IS_MAT(fmatrix) )
+ CV_Error( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
+
+ if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
+ CV_Error( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
+
+ if( fmatrix->cols != 3 || fmatrix->rows != 3 )
+ CV_Error( CV_StsBadSize, "fundamental matrix must be 3x3" );
+
+ if( !CV_IS_MAT(lines) )
+ CV_Error( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
+
+ abc_depth = CV_MAT_DEPTH(lines->type);
+ abc_cn = CV_MAT_CN(lines->type);
+ if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) )
+ CV_Error( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
+
+ if( lines->rows > lines->cols )
+ {
+ abc_dims = abc_cn*lines->cols;
+ abc_count = lines->rows;
+ }
+ else
+ {
+ if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) )
+ CV_Error( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
+ abc_dims = abc_cn * lines->rows;
+ abc_count = lines->cols;
+ }
+
+ if( abc_dims != 3 )
+ CV_Error( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
+
+ if( abc_count != count )
+ CV_Error( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
+
+ elem_size = CV_ELEM_SIZE(depth);
+ abc_elem_size = CV_ELEM_SIZE(abc_depth);
+
+ if( points->rows == dims )
+ {
+ plane_stride = points->step;
+ stride = elem_size;
+ }
+ else
+ {
+ plane_stride = elem_size;
+ stride = points->rows == 1 ? dims*elem_size : points->step;
+ }
+
+ if( lines->rows == 3 )
+ {
+ abc_plane_stride = lines->step;
+ abc_stride = abc_elem_size;
+ }
+ else
+ {
+ abc_plane_stride = abc_elem_size;
+ abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
+ }
+
+ cvConvert( fmatrix, &F );
+ if( pointImageID == 2 )
+ cvTranspose( &F, &F );
+
+ xp = points->data.ptr;
+ yp = xp + plane_stride;
+ zp = dims == 3 ? yp + plane_stride : 0;
+
+ ap = lines->data.ptr;
+ bp = ap + abc_plane_stride;
+ cp = bp + abc_plane_stride;
+
+ for( i = 0; i < count; i++ )
+ {
+ double x, y, z = 1.;
+ double a, b, c, nu;
+
+ if( depth == CV_32F )
+ {
+ x = *(float*)xp; y = *(float*)yp;
+ if( zp )
+ z = *(float*)zp, zp += stride;
+ }
+ else
+ {
+ x = *(double*)xp; y = *(double*)yp;
+ if( zp )
+ z = *(double*)zp, zp += stride;
+ }
+
+ xp += stride; yp += stride;
+
+ a = f[0]*x + f[1]*y + f[2]*z;
+ b = f[3]*x + f[4]*y + f[5]*z;
+ c = f[6]*x + f[7]*y + f[8]*z;
+ nu = a*a + b*b;
+ nu = nu ? 1./sqrt(nu) : 1.;
+ a *= nu; b *= nu; c *= nu;
+
+ if( abc_depth == CV_32F )
+ {
+ *(float*)ap = (float)a;
+ *(float*)bp = (float)b;
+ *(float*)cp = (float)c;
+ }
+ else
+ {
+ *(double*)ap = a;
+ *(double*)bp = b;
+ *(double*)cp = c;
+ }
+
+ ap += abc_stride;
+ bp += abc_stride;
+ cp += abc_stride;
+ }
+}
+
+
+CV_IMPL void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
+{
+ Ptr<CvMat> temp, denom;
+
+ int i, s_count, s_dims, d_count, d_dims;
+ CvMat _src, _dst, _ones;
+ CvMat* ones = 0;
+
+ if( !CV_IS_MAT(src) )
+ CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg,
+ "The input parameter is not a valid matrix" );
+
+ if( !CV_IS_MAT(dst) )
+ CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
+ "The output parameter is not a valid matrix" );
+
+ if( src == dst || src->data.ptr == dst->data.ptr )
+ {
+ if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
+ CV_Error( CV_StsBadArg, "Invalid inplace operation" );
+ return;
+ }
+
+ if( src->rows > src->cols )
+ {
+ if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
+ CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
+
+ s_dims = CV_MAT_CN(src->type)*src->cols;
+ s_count = src->rows;
+ }
+ else
+ {
+ if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
+ CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
+
+ s_dims = CV_MAT_CN(src->type)*src->rows;
+ s_count = src->cols;
+ }
+
+ if( src->rows == 1 || src->cols == 1 )
+ src = cvReshape( src, &_src, 1, s_count );
+
+ if( dst->rows > dst->cols )
+ {
+ if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
+ CV_Error( CV_StsBadSize,
+ "Either the number of channels or columns or rows in the input matrix must be =1" );
+
+ d_dims = CV_MAT_CN(dst->type)*dst->cols;
+ d_count = dst->rows;
+ }
+ else
+ {
+ if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
+ CV_Error( CV_StsBadSize,
+ "Either the number of channels or columns or rows in the output matrix must be =1" );
+
+ d_dims = CV_MAT_CN(dst->type)*dst->rows;
+ d_count = dst->cols;
+ }
+
+ if( dst->rows == 1 || dst->cols == 1 )
+ dst = cvReshape( dst, &_dst, 1, d_count );
+
+ if( s_count != d_count )
+ CV_Error( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
+
+ if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
+ CV_Error( CV_StsUnsupportedFormat,
+ "Both matrices must be floating-point (single or double precision)" );
+
+ if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
+ CV_Error( CV_StsOutOfRange,
+ "Both input and output point dimensionality must be 2, 3 or 4" );
+
+ if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
+ CV_Error( CV_StsUnmatchedSizes,
+ "The dimensionalities of input and output point sets differ too much" );
+
+ if( s_dims == d_dims - 1 )
+ {
+ if( d_count == dst->rows )
+ {
+ ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
+ dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
+ }
+ else
+ {
+ ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
+ dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
+ }
+ }
+
+ if( s_dims <= d_dims )
+ {
+ if( src->rows == dst->rows && src->cols == dst->cols )
+ {
+ if( CV_ARE_TYPES_EQ( src, dst ) )
+ cvCopy( src, dst );
+ else
+ cvConvert( src, dst );
+ }
+ else
+ {
+ if( !CV_ARE_TYPES_EQ( src, dst ))
+ {
+ temp = cvCreateMat( src->rows, src->cols, dst->type );
+ cvConvert( src, temp );
+ src = temp;
+ }
+ cvTranspose( src, dst );
+ }
+
+ if( ones )
+ cvSet( ones, cvRealScalar(1.) );
+ }
+ else
+ {
+ int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
+
+ if( !CV_ARE_TYPES_EQ( src, dst ))
+ {
+ temp = cvCreateMat( src->rows, src->cols, dst->type );
+ cvConvert( src, temp );
+ src = temp;
+ }
+
+ elem_size = CV_ELEM_SIZE(src->type);
+
+ if( s_count == src->cols )
+ s_plane_stride = src->step / elem_size, s_stride = 1;
+ else
+ s_stride = src->step / elem_size, s_plane_stride = 1;
+
+ if( d_count == dst->cols )
+ d_plane_stride = dst->step / elem_size, d_stride = 1;
+ else
+ d_stride = dst->step / elem_size, d_plane_stride = 1;
+
+ denom = cvCreateMat( 1, d_count, dst->type );
+
+ if( CV_MAT_DEPTH(dst->type) == CV_32F )
+ {
+ const float* xs = src->data.fl;
+ const float* ys = xs + s_plane_stride;
+ const float* zs = 0;
+ const float* ws = xs + (s_dims - 1)*s_plane_stride;
+
+ float* iw = denom->data.fl;
+
+ float* xd = dst->data.fl;
+ float* yd = xd + d_plane_stride;
+ float* zd = 0;
+
+ if( d_dims == 3 )
+ {
+ zs = ys + s_plane_stride;
+ zd = yd + d_plane_stride;
+ }
+
+ for( i = 0; i < d_count; i++, ws += s_stride )
+ {
+ float t = *ws;
+ iw[i] = fabs((double)t) > FLT_EPSILON ? t : 1.f;
+ }
+
+ cvDiv( 0, denom, denom );
+
+ if( d_dims == 3 )
+ for( i = 0; i < d_count; i++ )
+ {
+ float w = iw[i];
+ float x = *xs * w, y = *ys * w, z = *zs * w;
+ xs += s_stride; ys += s_stride; zs += s_stride;
+ *xd = x; *yd = y; *zd = z;
+ xd += d_stride; yd += d_stride; zd += d_stride;
+ }
+ else
+ for( i = 0; i < d_count; i++ )
+ {
+ float w = iw[i];
+ float x = *xs * w, y = *ys * w;
+ xs += s_stride; ys += s_stride;
+ *xd = x; *yd = y;
+ xd += d_stride; yd += d_stride;
+ }
+ }
+ else
+ {
+ const double* xs = src->data.db;
+ const double* ys = xs + s_plane_stride;
+ const double* zs = 0;
+ const double* ws = xs + (s_dims - 1)*s_plane_stride;
+
+ double* iw = denom->data.db;
+
+ double* xd = dst->data.db;
+ double* yd = xd + d_plane_stride;
+ double* zd = 0;
+
+ if( d_dims == 3 )
+ {
+ zs = ys + s_plane_stride;
+ zd = yd + d_plane_stride;
+ }
+
+ for( i = 0; i < d_count; i++, ws += s_stride )
+ {
+ double t = *ws;
+ iw[i] = fabs(t) > DBL_EPSILON ? t : 1.;
+ }
+
+ cvDiv( 0, denom, denom );
+
+ if( d_dims == 3 )
+ for( i = 0; i < d_count; i++ )
+ {
+ double w = iw[i];
+ double x = *xs * w, y = *ys * w, z = *zs * w;
+ xs += s_stride; ys += s_stride; zs += s_stride;
+ *xd = x; *yd = y; *zd = z;
+ xd += d_stride; yd += d_stride; zd += d_stride;
+ }
+ else
+ for( i = 0; i < d_count; i++ )
+ {
+ double w = iw[i];
+ double x = *xs * w, y = *ys * w;
+ xs += s_stride; ys += s_stride;
+ *xd = x; *yd = y;
+ xd += d_stride; yd += d_stride;
+ }
+ }
+ }
+}
+
+namespace cv
+{
+
+static Mat _findHomography( const Mat& points1, const Mat& points2,
+ int method, double ransacReprojThreshold,
+ vector<uchar>* mask )
+{
+ CV_Assert(points1.isContinuous() && points2.isContinuous() &&
+ points1.type() == points2.type() &&
+ ((points1.rows == 1 && points1.channels() == 2) ||
+ points1.cols*points1.channels() == 2) &&
+ ((points2.rows == 1 && points2.channels() == 2) ||
+ points2.cols*points2.channels() == 2));
+
+ Mat H(3, 3, CV_64F);
+ CvMat _pt1 = Mat(points1), _pt2 = Mat(points2);
+ CvMat matH = H, _mask, *pmask = 0;
+ if( mask )
+ {
+ mask->resize(points1.cols*points1.rows*points1.channels()/2);
+ pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));
+ }
+ bool ok = cvFindHomography( &_pt1, &_pt2, &matH, method, ransacReprojThreshold, pmask ) > 0;
+ if( !ok )
+ H = Scalar(0);
+ return H;
+}
+
+static Mat _findFundamentalMat( const Mat& points1, const Mat& points2,
+ int method, double param1, double param2,
+ vector<uchar>* mask )
+{
+ CV_Assert(points1.isContinuous() && points2.isContinuous() &&
+ points1.type() == points2.type() &&
+ ((points1.rows == 1 && points1.channels() == 2) ||
+ points1.cols*points1.channels() == 2) &&
+ ((points2.rows == 1 && points2.channels() == 2) ||
+ points2.cols*points2.channels() == 2));
+
+ Mat F(3, 3, CV_64F);
+ CvMat _pt1 = Mat(points1), _pt2 = Mat(points2);
+ CvMat matF = F, _mask, *pmask = 0;
+ if( mask )
+ {
+ mask->resize(points1.cols*points1.rows*points1.channels()/2);
+ pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));
+ }
+ int n = cvFindFundamentalMat( &_pt1, &_pt2, &matF, method, param1, param2, pmask );
+ if( n <= 0 )
+ F = Scalar(0);
+ return F;
+}
+
+}
+
+
+cv::Mat cv::findHomography( const Mat& srcPoints, const Mat& dstPoints,
+ vector<uchar>& mask, int method,
+ double ransacReprojThreshold )
+{
+ return _findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, &mask);
+}
+
+cv::Mat cv::findHomography( const Mat& srcPoints, const Mat& dstPoints,
+ int method, double ransacReprojThreshold )
+{
+ return _findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, 0);
+}
+
+
+cv::Mat cv::findFundamentalMat( const Mat& points1, const Mat& points2,
+ vector<uchar>& mask, int method, double param1, double param2 )
+{
+ return _findFundamentalMat( points1, points2, method, param1, param2, &mask );
+}
+
+cv::Mat cv::findFundamentalMat( const Mat& points1, const Mat& points2,
+ int method, double param1, double param2 )
+{
+ return _findFundamentalMat( points1, points2, method, param1, param2, 0 );
+}
+
+void cv::computeCorrespondEpilines( const Mat& points, int whichImage,
+ const Mat& F, vector<Vec3f>& lines )
+{
+ CV_Assert(points.isContinuous() &&
+ (points.depth() == CV_32S || points.depth() == CV_32F) &&
+ ((points.rows == 1 && points.channels() == 2) ||
+ points.cols*points.channels() == 2));
+
+ lines.resize(points.cols*points.rows*points.channels()/2);
+ CvMat _points = points, _lines = Mat(lines), matF = F;
+ cvComputeCorrespondEpilines(&_points, whichImage, &matF, &_lines);
+}
+
+void cv::convertPointsHomogeneous( const Mat& src, vector<Point3f>& dst )
+{
+ CV_Assert(src.isContinuous() &&
+ (src.depth() == CV_32S || src.depth() == CV_32F) &&
+ ((src.rows == 1 && src.channels() == 2) ||
+ src.cols*src.channels() == 2));
+
+ dst.resize(src.cols*src.rows*src.channels()/2);
+ CvMat _src = src, _dst = Mat(dst);
+ cvConvertPointsHomogeneous(&_src, &_dst);
+}
+
+void cv::convertPointsHomogeneous( const Mat& src, vector<Point2f>& dst )
+{
+ CV_Assert(src.isContinuous() &&
+ (src.depth() == CV_32S || src.depth() == CV_32F) &&
+ ((src.rows == 1 && src.channels() == 3) ||
+ src.cols*src.channels() == 3));
+
+ dst.resize(src.cols*src.rows*src.channels()/3);
+ CvMat _src = Mat(src), _dst = Mat(dst);
+ cvConvertPointsHomogeneous(&_src, &_dst);
+}
+
+/* End of file. */
projects / opencv.git / blobdiff