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