modules/calib3d/src/fundam.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   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.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  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.
  25 //
  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.
  28 //
  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.
  39 //
  40 //M*/
  41
  42 #include "precomp.hpp"
  43 #include "_modelest.h"
  44
  45 using namespace cv;
  46
  47 template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )
  48 {
  49     int i, j;
  50     for( i = j = 0; i < count; i++ )
  51         if( mask[i*mstep] )
  52         {
  53             if( i > j )
  54                 ptr[j] = ptr[i];
  55             j++;
  56         }
  57     return j;
  58 }
  59
  60 class CvHomographyEstimator : public CvModelEstimator2
  61 {
  62 public:
  63     CvHomographyEstimator( int modelPoints );
  64
  65     virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
  66     virtual bool refine( const CvMat* m1, const CvMat* m2,
  67                          CvMat* model, int maxIters );
  68 protected:
  69     virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
  70                                      const CvMat* model, CvMat* error );
  71 };
  72
  73
  74 CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)
  75     : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)
  76 {
  77     assert( _modelPoints == 4 || _modelPoints == 5 );
  78     checkPartialSubsets = false;
  79 }
  80
  81 int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
  82 {
  83     int i, count = m1->rows*m1->cols;
  84     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
  85     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
  86
  87     double LtL[9][9], W[9][1], V[9][9];
  88     CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
  89     CvMat matW = cvMat( 9, 1, CV_64F, W );
  90     CvMat matV = cvMat( 9, 9, CV_64F, V );
  91     CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );
  92     CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );
  93     CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};
  94
  95     for( i = 0; i < count; i++ )
  96     {
  97         cm.x += m[i].x; cm.y += m[i].y;
  98         cM.x += M[i].x; cM.y += M[i].y;
  99     }
 100
 101     cm.x /= count; cm.y /= count;
 102     cM.x /= count; cM.y /= count;
 103
 104     for( i = 0; i < count; i++ )
 105     {
 106         sm.x += fabs(m[i].x - cm.x);
 107         sm.y += fabs(m[i].y - cm.y);
 108         sM.x += fabs(M[i].x - cM.x);
 109         sM.y += fabs(M[i].y - cM.y);
 110     }
 111
 112     if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
 113         fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON )
 114         return 0;
 115     sm.x = count/sm.x; sm.y = count/sm.y;
 116     sM.x = count/sM.x; sM.y = count/sM.y;
 117
 118     double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
 119     double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
 120     CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );
 121     CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
 122
 123     cvZero( &_LtL );
 124     for( i = 0; i < count; i++ )
 125     {
 126         double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
 127         double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
 128         double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
 129         double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
 130         int j, k;
 131         for( j = 0; j < 9; j++ )
 132             for( k = j; k < 9; k++ )
 133                 LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
 134     }
 135     cvCompleteSymm( &_LtL );
 136
 137     //cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
 138     cvEigenVV( &_LtL, &matV, &matW );
 139     cvMatMul( &_invHnorm, &_H0, &_Htemp );
 140     cvMatMul( &_Htemp, &_Hnorm2, &_H0 );
 141     cvConvertScale( &_H0, H, 1./_H0.data.db[8] );
 142
 143     return 1;
 144 }
 145
 146
 147 void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
 148                                                 const CvMat* model, CvMat* _err )
 149 {
 150     int i, count = m1->rows*m1->cols;
 151     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
 152     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
 153     const double* H = model->data.db;
 154     float* err = _err->data.fl;
 155
 156     for( i = 0; i < count; i++ )
 157     {
 158         double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
 159         double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;
 160         double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;
 161         err[i] = (float)(dx*dx + dy*dy);
 162     }
 163 }
 164
 165 bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )
 166 {
 167     CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
 168     int i, j, k, count = m1->rows*m1->cols;
 169     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
 170     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
 171     CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );
 172     cvCopy( &modelPart, solver.param );
 173
 174     for(;;)
 175     {
 176         const CvMat* _param = 0;
 177         CvMat *_JtJ = 0, *_JtErr = 0;
 178         double* _errNorm = 0;
 179
 180         if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))
 181             break;
 182
 183         for( i = 0; i < count; i++ )
 184         {
 185             const double* h = _param->data.db;
 186             double Mx = M[i].x, My = M[i].y;
 187             double ww = h[6]*Mx + h[7]*My + 1.;
 188             ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0;
 189             double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;
 190             double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;
 191             double err[] = { _xi - m[i].x, _yi - m[i].y };
 192             if( _JtJ || _JtErr )
 193             {
 194                 double J[][8] =
 195                 {
 196                     { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
 197                     { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
 198                 };
 199
 200                 for( j = 0; j < 8; j++ )
 201                 {
 202                     for( k = j; k < 8; k++ )
 203                         _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];
 204                     _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];
 205                 }
 206             }
 207             if( _errNorm )
 208                 *_errNorm += err[0]*err[0] + err[1]*err[1];
 209         }
 210     }
 211
 212     cvCopy( solver.param, &modelPart );
 213     return true;
 214 }
 215
 216
 217 CV_IMPL int
 218 cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
 219                   CvMat* __H, int method, double ransacReprojThreshold,
 220                   CvMat* mask )
 221 {
 222     const double confidence = 0.995;
 223     const int maxIters = 2000;
 224     const double defaultRANSACReprojThreshold = 3;
 225     bool result = false;
 226     Ptr<CvMat> m, M, tempMask;
 227
 228     double H[9];
 229     CvMat matH = cvMat( 3, 3, CV_64FC1, H );
 230     int count;
 231
 232     CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
 233
 234     count = MAX(imagePoints->cols, imagePoints->rows);
 235     CV_Assert( count >= 4 );
 236     if( ransacReprojThreshold <= 0 )
 237         ransacReprojThreshold = defaultRANSACReprojThreshold;
 238
 239     m = cvCreateMat( 1, count, CV_64FC2 );
 240     cvConvertPointsHomogeneous( imagePoints, m );
 241
 242     M = cvCreateMat( 1, count, CV_64FC2 );
 243     cvConvertPointsHomogeneous( objectPoints, M );
 244
 245     if( mask )
 246     {
 247         CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
 248             (mask->rows == 1 || mask->cols == 1) &&
 249             mask->rows*mask->cols == count );
 250     }
 251     if( mask || count > 4 )
 252         tempMask = cvCreateMat( 1, count, CV_8U );
 253     if( !tempMask.empty() )
 254         cvSet( tempMask, cvScalarAll(1.) );
 255
 256     CvHomographyEstimator estimator(4);
 257     if( count == 4 )
 258         method = 0;
 259     if( method == CV_LMEDS )
 260         result = estimator.runLMeDS( M, m, &matH, tempMask, confidence, maxIters );
 261     else if( method == CV_RANSAC )
 262         result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);
 263     else
 264         result = estimator.runKernel( M, m, &matH ) > 0;
 265
 266     if( result && count > 4 )
 267     {
 268         icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
 269         count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
 270         M->cols = m->cols = count;
 271         if( method == CV_RANSAC )
 272             estimator.runKernel( M, m, &matH );
 273         estimator.refine( M, m, &matH, 10 );
 274     }
 275
 276     if( result )
 277         cvConvert( &matH, __H );
 278
 279     if( mask && tempMask )
 280     {
 281         if( CV_ARE_SIZES_EQ(mask, tempMask) )
 282            cvCopy( tempMask, mask );
 283         else
 284            cvTranspose( tempMask, mask );
 285     }
 286
 287     return (int)result;
 288 }
 289
 290
 291 /* Evaluation of Fundamental Matrix from point correspondences.
 292    The original code has been written by Valery Mosyagin */
 293
 294 /* The algorithms (except for RANSAC) and the notation have been taken from
 295    Zhengyou Zhang's research report
 296    "Determining the Epipolar Geometry and its Uncertainty: A Review"
 297    that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
 298
 299 /************************************** 7-point algorithm *******************************/
 300 class CvFMEstimator : public CvModelEstimator2
 301 {
 302 public:
 303     CvFMEstimator( int _modelPoints );
 304
 305     virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
 306     virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model );
 307     virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model );
 308 protected:
 309     virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
 310                                      const CvMat* model, CvMat* error );
 311 };
 312
 313 CvFMEstimator::CvFMEstimator( int _modelPoints )
 314 : CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 )
 315 {
 316     assert( _modelPoints == 7 || _modelPoints == 8 );
 317 }
 318
 319
 320 int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
 321 {
 322     return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model );
 323 }
 324
 325 int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
 326 {
 327     double a[7*9], w[7], v[9*9], c[4], r[3];
 328     double* f1, *f2;
 329     double t0, t1, t2;
 330     CvMat A = cvMat( 7, 9, CV_64F, a );
 331     CvMat V = cvMat( 9, 9, CV_64F, v );
 332     CvMat W = cvMat( 7, 1, CV_64F, w );
 333     CvMat coeffs = cvMat( 1, 4, CV_64F, c );
 334     CvMat roots = cvMat( 1, 3, CV_64F, r );
 335     const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
 336     const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
 337     double* fmatrix = _fmatrix->data.db;
 338     int i, k, n;
 339
 340     // form a linear system: i-th row of A(=a) represents
 341     // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0
 342     for( i = 0; i < 7; i++ )
 343     {
 344         double x0 = m1[i].x, y0 = m1[i].y;
 345         double x1 = m2[i].x, y1 = m2[i].y;
 346
 347         a[i*9+0] = x1*x0;
 348         a[i*9+1] = x1*y0;
 349         a[i*9+2] = x1;
 350         a[i*9+3] = y1*x0;
 351         a[i*9+4] = y1*y0;
 352         a[i*9+5] = y1;
 353         a[i*9+6] = x0;
 354         a[i*9+7] = y0;
 355         a[i*9+8] = 1;
 356     }
 357
 358     // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so
 359     // the solution is linear subspace of dimensionality 2.
 360     // => use the last two singular vectors as a basis of the space
 361     // (according to SVD properties)
 362     cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
 363     f1 = v + 7*9;
 364     f2 = v + 8*9;
 365
 366     // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
 367     // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
 368     // so f ~ lambda*f1 + (1 - lambda)*f2.
 369     // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
 370     // it will be a cubic equation.
 371     // find c - polynomial coefficients.
 372     for( i = 0; i < 9; i++ )
 373         f1[i] -= f2[i];
 374
 375     t0 = f2[4]*f2[8] - f2[5]*f2[7];
 376     t1 = f2[3]*f2[8] - f2[5]*f2[6];
 377     t2 = f2[3]*f2[7] - f2[4]*f2[6];
 378
 379     c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;
 380
 381     c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
 382            f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
 383            f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
 384            f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
 385            f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
 386            f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
 387            f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);
 388
 389     t0 = f1[4]*f1[8] - f1[5]*f1[7];
 390     t1 = f1[3]*f1[8] - f1[5]*f1[6];
 391     t2 = f1[3]*f1[7] - f1[4]*f1[6];
 392
 393     c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -
 394            f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
 395            f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
 396            f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
 397            f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
 398            f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
 399            f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);
 400
 401     c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;
 402
 403     // solve the cubic equation; there can be 1 to 3 roots ...
 404     n = cvSolveCubic( &coeffs, &roots );
 405
 406     if( n < 1 || n > 3 )
 407         return n;
 408
 409     for( k = 0; k < n; k++, fmatrix += 9 )
 410     {
 411         // for each root form the fundamental matrix
 412         double lambda = r[k], mu = 1.;
 413         double s = f1[8]*r[k] + f2[8];
 414
 415         // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1
 416         if( fabs(s) > DBL_EPSILON )
 417         {
 418             mu = 1./s;
 419             lambda *= mu;
 420             fmatrix[8] = 1.;
 421         }
 422         else
 423             fmatrix[8] = 0.;
 424
 425         for( i = 0; i < 8; i++ )
 426             fmatrix[i] = f1[i]*lambda + f2[i]*mu;
 427     }
 428
 429     return n;
 430 }
 431
 432
 433 int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
 434 {
 435     double a[9*9], w[9], v[9*9];
 436     CvMat W = cvMat( 1, 9, CV_64F, w );
 437     CvMat V = cvMat( 9, 9, CV_64F, v );
 438     CvMat A = cvMat( 9, 9, CV_64F, a );
 439     CvMat U, F0, TF;
 440
 441     CvPoint2D64f m0c = {0,0}, m1c = {0,0};
 442     double t, scale0 = 0, scale1 = 0;
 443
 444     const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
 445     const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
 446     double* fmatrix = _fmatrix->data.db;
 447     CV_Assert( (_m1->cols == 1 || _m1->rows == 1) && CV_ARE_SIZES_EQ(_m1, _m2));
 448     int i, j, k, count = _m1->cols*_m1->rows;
 449
 450     // compute centers and average distances for each of the two point sets
 451     for( i = 0; i < count; i++ )
 452     {
 453         double x = m1[i].x, y = m1[i].y;
 454         m0c.x += x; m0c.y += y;
 455
 456         x = m2[i].x, y = m2[i].y;
 457         m1c.x += x; m1c.y += y;
 458     }
 459
 460     // calculate the normalizing transformations for each of the point sets:
 461     // after the transformation each set will have the mass center at the coordinate origin
 462     // and the average distance from the origin will be ~sqrt(2).
 463     t = 1./count;
 464     m0c.x *= t; m0c.y *= t;
 465     m1c.x *= t; m1c.y *= t;
 466
 467     for( i = 0; i < count; i++ )
 468     {
 469         double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y;
 470         scale0 += sqrt(x*x + y*y);
 471
 472         x = m2[i].x - m1c.x, y = m2[i].y - m1c.y;
 473         scale1 += sqrt(x*x + y*y);
 474     }
 475
 476     scale0 *= t;
 477     scale1 *= t;
 478
 479     if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
 480         return 0;
 481
 482     scale0 = sqrt(2.)/scale0;
 483     scale1 = sqrt(2.)/scale1;
 484
 485     cvZero( &A );
 486
 487     // form a linear system Ax=0: for each selected pair of points m1 & m2,
 488     // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0
 489     // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0.
 490     for( i = 0; i < count; i++ )
 491     {
 492         double x0 = (m1[i].x - m0c.x)*scale0;
 493         double y0 = (m1[i].y - m0c.y)*scale0;
 494         double x1 = (m2[i].x - m1c.x)*scale1;
 495         double y1 = (m2[i].y - m1c.y)*scale1;
 496         double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 };
 497         for( j = 0; j < 9; j++ )
 498             for( k = 0; k < 9; k++ )
 499                 a[j*9+k] += r[j]*r[k];
 500     }
 501
 502     cvEigenVV(&A, &V, &W);
 503
 504     for( i = 0; i < 9; i++ )
 505     {
 506         if( fabs(w[i]) < DBL_EPSILON )
 507             break;
 508     }
 509
 510     if( i < 8 )
 511         return 0;
 512
 513     F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
 514
 515     // make F0 singular (of rank 2) by decomposing it with SVD,
 516     // zeroing the last diagonal element of W and then composing the matrices back.
 517
 518     // use v as a temporary storage for different 3x3 matrices
 519     W = U = V = TF = F0;
 520     W.data.db = v;
 521     U.data.db = v + 9;
 522     V.data.db = v + 18;
 523     TF.data.db = v + 27;
 524
 525     cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
 526     W.data.db[8] = 0.;
 527
 528     // F0 <- U*diag([W(1), W(2), 0])*V'
 529     cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );
 530     cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );
 531
 532     // apply the transformation that is inverse
 533     // to what we used to normalize the point coordinates
 534     {
 535         double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };
 536         double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
 537         CvMat T0, T1;
 538         T0 = T1 = F0;
 539         T0.data.db = tt0;
 540         T1.data.db = tt1;
 541
 542         // F0 <- T1'*F0*T0
 543         cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );
 544         F0.data.db = fmatrix;
 545         cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );
 546
 547         // make F(3,3) = 1
 548         if( fabs(F0.data.db[8]) > FLT_EPSILON )
 549             cvScale( &F0, &F0, 1./F0.data.db[8] );
 550     }
 551
 552     return 1;
 553 }
 554
 555
 556 void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2,
 557                                         const CvMat* model, CvMat* _err )
 558 {
 559     int i, count = _m1->rows*_m1->cols;
 560     const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
 561     const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
 562     const double* F = model->data.db;
 563     float* err = _err->data.fl;
 564
 565     for( i = 0; i < count; i++ )
 566     {
 567         double a, b, c, d1, d2, s1, s2;
 568
 569         a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];
 570         b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];
 571         c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];
 572
 573         s2 = 1./(a*a + b*b);
 574         d2 = m2[i].x*a + m2[i].y*b + c;
 575
 576         a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];
 577         b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];
 578         c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];
 579
 580         s1 = 1./(a*a + b*b);
 581         d1 = m1[i].x*a + m1[i].y*b + c;
 582
 583         err[i] = (float)std::max(d1*d1*s1, d2*d2*s2);
 584     }
 585 }
 586
 587
 588 CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
 589                                   CvMat* fmatrix, int method,
 590                                   double param1, double param2, CvMat* mask )
 591 {
 592     int result = 0;
 593     Ptr<CvMat> m1, m2, tempMask;
 594
 595     double F[3*9];
 596     CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );
 597     int count;
 598
 599     CV_Assert( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );
 600     CV_Assert( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&
 601         (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) );
 602
 603     count = MAX(points1->cols, points1->rows);
 604     if( count < 7 )
 605         return 0;
 606
 607     m1 = cvCreateMat( 1, count, CV_64FC2 );
 608     cvConvertPointsHomogeneous( points1, m1 );
 609
 610     m2 = cvCreateMat( 1, count, CV_64FC2 );
 611     cvConvertPointsHomogeneous( points2, m2 );
 612
 613     if( mask )
 614     {
 615         CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
 616             (mask->rows == 1 || mask->cols == 1) &&
 617             mask->rows*mask->cols == count );
 618     }
 619     if( mask || count >= 8 )
 620         tempMask = cvCreateMat( 1, count, CV_8U );
 621     if( !tempMask.empty() )
 622         cvSet( tempMask, cvScalarAll(1.) );
 623
 624     CvFMEstimator estimator(7);
 625     if( count == 7 )
 626         result = estimator.run7Point(m1, m2, &_F9x3);
 627     else if( method == CV_FM_8POINT )
 628         result = estimator.run8Point(m1, m2, &_F3x3);
 629     else
 630     {
 631         if( param1 <= 0 )
 632             param1 = 3;
 633         if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
 634             param2 = 0.99;
 635
 636         if( (method & ~3) == CV_RANSAC && count >= 15 )
 637             result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 );
 638         else
 639             result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );
 640         if( result <= 0 )
 641             return 0;
 642         /*icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );
 643         count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count );
 644         assert( count >= 8 );
 645         m1->cols = m2->cols = count;
 646         estimator.run8Point(m1, m2, &_F3x3);*/
 647     }
 648
 649     if( result )
 650         cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );
 651
 652     if( mask && tempMask )
 653     {
 654         if( CV_ARE_SIZES_EQ(mask, tempMask) )
 655             cvCopy( tempMask, mask );
 656         else
 657             cvTranspose( tempMask, mask );
 658     }
 659
 660     return result;
 661 }
 662
 663
 664 CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
 665                                           const CvMat* fmatrix, CvMat* lines )
 666 {
 667     int abc_stride, abc_plane_stride, abc_elem_size;
 668     int plane_stride, stride, elem_size;
 669     int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
 670     uchar *ap, *bp, *cp;
 671     const uchar *xp, *yp, *zp;
 672     double f[9];
 673     CvMat F = cvMat( 3, 3, CV_64F, f );
 674
 675     if( !CV_IS_MAT(points) )
 676         CV_Error( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
 677
 678     depth = CV_MAT_DEPTH(points->type);
 679     cn = CV_MAT_CN(points->type);
 680     if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) )
 681         CV_Error( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
 682
 683     if( cn > 1 )
 684     {
 685         dims = cn;
 686         CV_Assert( points->rows == 1 || points->cols == 1 );
 687         count = points->rows * points->cols;
 688     }
 689     else if( points->rows > points->cols )
 690     {
 691         dims = cn*points->cols;
 692         count = points->rows;
 693     }
 694     else
 695     {
 696         if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) )
 697             CV_Error( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
 698         dims = points->rows;
 699         count = points->cols;
 700     }
 701
 702     if( dims != 2 && dims != 3 )
 703         CV_Error( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
 704
 705     if( !CV_IS_MAT(fmatrix) )
 706         CV_Error( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
 707
 708     if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
 709         CV_Error( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
 710
 711     if( fmatrix->cols != 3 || fmatrix->rows != 3 )
 712         CV_Error( CV_StsBadSize, "fundamental matrix must be 3x3" );
 713
 714     if( !CV_IS_MAT(lines) )
 715         CV_Error( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
 716
 717     abc_depth = CV_MAT_DEPTH(lines->type);
 718     abc_cn = CV_MAT_CN(lines->type);
 719     if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) )
 720         CV_Error( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
 721
 722     if( abc_cn > 1 )
 723     {
 724         abc_dims = abc_cn;
 725         CV_Assert( lines->rows == 1 || lines->cols == 1 );
 726         abc_count = lines->rows * lines->cols;
 727     }
 728     else if( lines->rows > lines->cols )
 729     {
 730         abc_dims = abc_cn*lines->cols;
 731         abc_count = lines->rows;
 732     }
 733     else
 734     {
 735         if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) )
 736             CV_Error( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
 737         abc_dims = lines->rows;
 738         abc_count = lines->cols;
 739     }
 740
 741     if( abc_dims != 3 )
 742         CV_Error( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
 743
 744     if( abc_count != count )
 745         CV_Error( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
 746
 747     elem_size = CV_ELEM_SIZE(depth);
 748     abc_elem_size = CV_ELEM_SIZE(abc_depth);
 749
 750     if( cn == 1 && points->rows == dims )
 751     {
 752         plane_stride = points->step;
 753         stride = elem_size;
 754     }
 755     else
 756     {
 757         plane_stride = elem_size;
 758         stride = points->rows == 1 ? dims*elem_size : points->step;
 759     }
 760
 761     if( abc_cn == 1 && lines->rows == 3 )
 762     {
 763         abc_plane_stride = lines->step;
 764         abc_stride = abc_elem_size;
 765     }
 766     else
 767     {
 768         abc_plane_stride = abc_elem_size;
 769         abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
 770     }
 771
 772     cvConvert( fmatrix, &F );
 773     if( pointImageID == 2 )
 774         cvTranspose( &F, &F );
 775
 776     xp = points->data.ptr;
 777     yp = xp + plane_stride;
 778     zp = dims == 3 ? yp + plane_stride : 0;
 779
 780     ap = lines->data.ptr;
 781     bp = ap + abc_plane_stride;
 782     cp = bp + abc_plane_stride;
 783
 784     for( i = 0; i < count; i++ )
 785     {
 786         double x, y, z = 1.;
 787         double a, b, c, nu;
 788
 789         if( depth == CV_32F )
 790         {
 791             x = *(float*)xp; y = *(float*)yp;
 792             if( zp )
 793                 z = *(float*)zp, zp += stride;
 794         }
 795         else
 796         {
 797             x = *(double*)xp; y = *(double*)yp;
 798             if( zp )
 799                 z = *(double*)zp, zp += stride;
 800         }
 801
 802         xp += stride; yp += stride;
 803
 804         a = f[0]*x + f[1]*y + f[2]*z;
 805         b = f[3]*x + f[4]*y + f[5]*z;
 806         c = f[6]*x + f[7]*y + f[8]*z;
 807         nu = a*a + b*b;
 808         nu = nu ? 1./sqrt(nu) : 1.;
 809         a *= nu; b *= nu; c *= nu;
 810
 811         if( abc_depth == CV_32F )
 812         {
 813             *(float*)ap = (float)a;
 814             *(float*)bp = (float)b;
 815             *(float*)cp = (float)c;
 816         }
 817         else
 818         {
 819             *(double*)ap = a;
 820             *(double*)bp = b;
 821             *(double*)cp = c;
 822         }
 823
 824         ap += abc_stride;
 825         bp += abc_stride;
 826         cp += abc_stride;
 827     }
 828 }
 829
 830
 831 CV_IMPL void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
 832 {
 833     Ptr<CvMat> temp, denom;
 834
 835     int i, s_count, s_dims, d_count, d_dims;
 836     CvMat _src, _dst, _ones;
 837     CvMat* ones = 0;
 838
 839     if( !CV_IS_MAT(src) )
 840         CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg,
 841         "The input parameter is not a valid matrix" );
 842
 843     if( !CV_IS_MAT(dst) )
 844         CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
 845         "The output parameter is not a valid matrix" );
 846
 847     if( src == dst || src->data.ptr == dst->data.ptr )
 848     {
 849         if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
 850             CV_Error( CV_StsBadArg, "Invalid inplace operation" );
 851         return;
 852     }
 853
 854     if( src->rows > src->cols )
 855     {
 856         if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
 857             CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
 858
 859         s_dims = CV_MAT_CN(src->type)*src->cols;
 860         s_count = src->rows;
 861     }
 862     else
 863     {
 864         if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
 865             CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
 866
 867         s_dims = CV_MAT_CN(src->type)*src->rows;
 868         s_count = src->cols;
 869     }
 870
 871     if( src->rows == 1 || src->cols == 1 )
 872         src = cvReshape( src, &_src, 1, s_count );
 873
 874     if( dst->rows > dst->cols )
 875     {
 876         if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
 877             CV_Error( CV_StsBadSize,
 878             "Either the number of channels or columns or rows in the input matrix must be =1" );
 879
 880         d_dims = CV_MAT_CN(dst->type)*dst->cols;
 881         d_count = dst->rows;
 882     }
 883     else
 884     {
 885         if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
 886             CV_Error( CV_StsBadSize,
 887             "Either the number of channels or columns or rows in the output matrix must be =1" );
 888
 889         d_dims = CV_MAT_CN(dst->type)*dst->rows;
 890         d_count = dst->cols;
 891     }
 892
 893     if( dst->rows == 1 || dst->cols == 1 )
 894         dst = cvReshape( dst, &_dst, 1, d_count );
 895
 896     if( s_count != d_count )
 897         CV_Error( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
 898
 899     if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
 900         CV_Error( CV_StsUnsupportedFormat,
 901         "Both matrices must be floating-point (single or double precision)" );
 902
 903     if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
 904         CV_Error( CV_StsOutOfRange,
 905         "Both input and output point dimensionality must be 2, 3 or 4" );
 906
 907     if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
 908         CV_Error( CV_StsUnmatchedSizes,
 909         "The dimensionalities of input and output point sets differ too much" );
 910
 911     if( s_dims == d_dims - 1 )
 912     {
 913         if( d_count == dst->rows )
 914         {
 915             ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
 916             dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
 917         }
 918         else
 919         {
 920             ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
 921             dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
 922         }
 923     }
 924
 925     if( s_dims <= d_dims )
 926     {
 927         if( src->rows == dst->rows && src->cols == dst->cols )
 928         {
 929             if( CV_ARE_TYPES_EQ( src, dst ) )
 930                 cvCopy( src, dst );
 931             else
 932                 cvConvert( src, dst );
 933         }
 934         else
 935         {
 936             if( !CV_ARE_TYPES_EQ( src, dst ))
 937             {
 938                 temp = cvCreateMat( src->rows, src->cols, dst->type );
 939                 cvConvert( src, temp );
 940                 src = temp;
 941             }
 942             cvTranspose( src, dst );
 943         }
 944
 945         if( ones )
 946             cvSet( ones, cvRealScalar(1.) );
 947     }
 948     else
 949     {
 950         int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
 951
 952         if( !CV_ARE_TYPES_EQ( src, dst ))
 953         {
 954             temp = cvCreateMat( src->rows, src->cols, dst->type );
 955             cvConvert( src, temp );
 956             src = temp;
 957         }
 958
 959         elem_size = CV_ELEM_SIZE(src->type);
 960
 961         if( s_count == src->cols )
 962             s_plane_stride = src->step / elem_size, s_stride = 1;
 963         else
 964             s_stride = src->step / elem_size, s_plane_stride = 1;
 965
 966         if( d_count == dst->cols )
 967             d_plane_stride = dst->step / elem_size, d_stride = 1;
 968         else
 969             d_stride = dst->step / elem_size, d_plane_stride = 1;
 970
 971         denom = cvCreateMat( 1, d_count, dst->type );
 972
 973         if( CV_MAT_DEPTH(dst->type) == CV_32F )
 974         {
 975             const float* xs = src->data.fl;
 976             const float* ys = xs + s_plane_stride;
 977             const float* zs = 0;
 978             const float* ws = xs + (s_dims - 1)*s_plane_stride;
 979
 980             float* iw = denom->data.fl;
 981
 982             float* xd = dst->data.fl;
 983             float* yd = xd + d_plane_stride;
 984             float* zd = 0;
 985
 986             if( d_dims == 3 )
 987             {
 988                 zs = ys + s_plane_stride;
 989                 zd = yd + d_plane_stride;
 990             }
 991
 992             for( i = 0; i < d_count; i++, ws += s_stride )
 993             {
 994                 float t = *ws;
 995                 iw[i] = fabs((double)t) > FLT_EPSILON ? t : 1.f;
 996             }
 997
 998             cvDiv( 0, denom, denom );
 999
1000             if( d_dims == 3 )
1001                 for( i = 0; i < d_count; i++ )
1002                 {
1003                     float w = iw[i];
1004                     float x = *xs * w, y = *ys * w, z = *zs * w;
1005                     xs += s_stride; ys += s_stride; zs += s_stride;
1006                     *xd = x; *yd = y; *zd = z;
1007                     xd += d_stride; yd += d_stride; zd += d_stride;
1008                 }
1009             else
1010                 for( i = 0; i < d_count; i++ )
1011                 {
1012                     float w = iw[i];
1013                     float x = *xs * w, y = *ys * w;
1014                     xs += s_stride; ys += s_stride;
1015                     *xd = x; *yd = y;
1016                     xd += d_stride; yd += d_stride;
1017                 }
1018         }
1019         else
1020         {
1021             const double* xs = src->data.db;
1022             const double* ys = xs + s_plane_stride;
1023             const double* zs = 0;
1024             const double* ws = xs + (s_dims - 1)*s_plane_stride;
1025
1026             double* iw = denom->data.db;
1027
1028             double* xd = dst->data.db;
1029             double* yd = xd + d_plane_stride;
1030             double* zd = 0;
1031
1032             if( d_dims == 3 )
1033             {
1034                 zs = ys + s_plane_stride;
1035                 zd = yd + d_plane_stride;
1036             }
1037
1038             for( i = 0; i < d_count; i++, ws += s_stride )
1039             {
1040                 double t = *ws;
1041                 iw[i] = fabs(t) > DBL_EPSILON ? t : 1.;
1042             }
1043
1044             cvDiv( 0, denom, denom );
1045
1046             if( d_dims == 3 )
1047                 for( i = 0; i < d_count; i++ )
1048                 {
1049                     double w = iw[i];
1050                     double x = *xs * w, y = *ys * w, z = *zs * w;
1051                     xs += s_stride; ys += s_stride; zs += s_stride;
1052                     *xd = x; *yd = y; *zd = z;
1053                     xd += d_stride; yd += d_stride; zd += d_stride;
1054                 }
1055             else
1056                 for( i = 0; i < d_count; i++ )
1057                 {
1058                     double w = iw[i];
1059                     double x = *xs * w, y = *ys * w;
1060                     xs += s_stride; ys += s_stride;
1061                     *xd = x; *yd = y;
1062                     xd += d_stride; yd += d_stride;
1063                 }
1064         }
1065     }
1066 }
1067
1068 cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
1069                             int method, double ransacReprojThreshold, OutputArray _mask )
1070 {
1071     Mat points1 = _points1.getMat(), points2 = _points2.getMat();
1072     int npoints = points1.checkVector(2);
1073     CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
1074                points1.type() == points2.type());
1075
1076     Mat H(3, 3, CV_64F);
1077     CvMat _pt1 = points1, _pt2 = points2;
1078     CvMat matH = H, c_mask, *p_mask = 0;
1079     if( _mask.needed() )
1080     {
1081         _mask.create(npoints, 1, CV_8U, -1, true);
1082         p_mask = &(c_mask = _mask.getMat());
1083     }
1084     bool ok = cvFindHomography( &_pt1, &_pt2, &matH, method, ransacReprojThreshold, p_mask ) > 0;
1085     if( !ok )
1086         H = Scalar(0);
1087     return H;
1088 }
1089
1090 cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
1091                             OutputArray _mask, int method, double ransacReprojThreshold )
1092 {
1093     return cv::findHomography(_points1, _points2, method, ransacReprojThreshold, _mask);
1094 }
1095
1096 cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
1097                                int method, double param1, double param2,
1098                                OutputArray _mask )
1099 {
1100     Mat points1 = _points1.getMat(), points2 = _points2.getMat();
1101     int npoints = points1.checkVector(2);
1102     CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
1103               points1.type() == points2.type());
1104
1105     Mat F(method == CV_FM_7POINT ? 9 : 3, 3, CV_64F);
1106     CvMat _pt1 = points1, _pt2 = points2;
1107     CvMat matF = F, c_mask, *p_mask = 0;
1108     if( _mask.needed() )
1109     {
1110         _mask.create(npoints, 1, CV_8U, -1, true);
1111         p_mask = &(c_mask = _mask.getMat());
1112     }
1113     int n = cvFindFundamentalMat( &_pt1, &_pt2, &matF, method, param1, param2, p_mask );
1114     if( n <= 0 )
1115         F = Scalar(0);
1116     if( n == 1 )
1117         F = F.rowRange(0, 3);
1118     return F;
1119 }
1120
1121 cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
1122                                 OutputArray _mask, int method, double param1, double param2 )
1123 {
1124     return cv::findFundamentalMat(_points1, _points2, method, param1, param2, _mask);
1125 }
1126
1127
1128 void cv::computeCorrespondEpilines( InputArray _points, int whichImage,
1129                                     InputArray _Fmat, OutputArray _lines )
1130 {
1131     Mat points = _points.getMat(), F = _Fmat.getMat();
1132     int npoints = points.checkVector(2);
1133     if( npoints < 0 )
1134         npoints = points.checkVector(3);
1135     CV_Assert( npoints >= 0 && (points.depth() == CV_32F || points.depth() == CV_32S));
1136
1137     _lines.create(npoints, 1, CV_32FC3, -1, true);
1138     CvMat c_points = points, c_lines = _lines.getMat(), c_F = F;
1139     cvComputeCorrespondEpilines(&c_points, whichImage, &c_F, &c_lines);
1140 }
1141
1142 void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst )
1143 {
1144     Mat src = _src.getMat();
1145     int npoints = src.checkVector(3), cn = 3;
1146     if( npoints < 0 )
1147     {
1148         npoints = src.checkVector(4);
1149         if( npoints >= 0 )
1150             cn = 4;
1151     }
1152     CV_Assert( npoints >= 0 && (src.depth() == CV_32F || src.depth() == CV_32S));
1153
1154     _dst.create(npoints, 1, CV_MAKETYPE(CV_32F, cn-1));
1155     CvMat c_src = src, c_dst = _dst.getMat();
1156     cvConvertPointsHomogeneous(&c_src, &c_dst);
1157 }
1158
1159 void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst )
1160 {
1161     Mat src = _src.getMat();
1162     int npoints = src.checkVector(2), cn = 2;
1163     if( npoints < 0 )
1164     {
1165         npoints = src.checkVector(3);
1166         if( npoints >= 0 )
1167             cn = 3;
1168     }
1169     CV_Assert( npoints >= 0 && (src.depth() == CV_32F || src.depth() == CV_32S));
1170
1171     _dst.create(npoints, 1, CV_MAKETYPE(CV_32F, cn+1));
1172     CvMat c_src = src, c_dst = _dst.getMat();
1173     cvConvertPointsHomogeneous(&c_src, &c_dst);
1174 }
1175
1176 void cv::convertPointsHomogeneous( InputArray _src, OutputArray _dst )
1177 {
1178     int stype = _src.type(), dtype = _dst.type();
1179     CV_Assert( _dst.fixedType() );
1180
1181     if( CV_MAT_CN(stype) > CV_MAT_CN(dtype) )
1182         convertPointsFromHomogeneous(_src, _dst);
1183     else
1184         convertPointsToHomogeneous(_src, _dst);
1185 }
1186
1187 /* End of file. */