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 #include "_cv.h"
  42
  43 /* Valery Mosyagin */
  44
  45 /*=====================================================================================*/
  46 /* new version of fundamental matrix functions */
  47 /*=====================================================================================*/
  48
  49 static int icvComputeFundamental7Point(CvMat* points1,CvMat* points2,
  50                                      CvMat* fundMatr);
  51
  52 static int icvComputeFundamental8Point(CvMat* points1,CvMat* points2,
  53                                      CvMat* fundMatr);
  54
  55
  56 static int icvComputeFundamentalRANSAC(   CvMat* points1,CvMat* points2,
  57                                         CvMat* fundMatr,
  58                                         double threshold,/* threshold for good point. Distance from epipolar line */
  59                                         double p,/* probability of good result. Usually = 0.99 */
  60                                         CvMat* status);
  61
  62 static int icvComputeFundamentalLMedS(   CvMat* points1,CvMat* points2,
  63                                         CvMat* fundMatr,
  64                                         double threshold,/* threshold for good point. Distance from epipolar line */
  65                                         double p,/* probability of good result. Usually = 0.99 */
  66                                         CvMat* status);
  67
  68 static void icvMakeFundamentalSingular(CvMat* fundMatr);
  69
  70 static void icvNormalizeFundPoints(    CvMat* points,
  71                                 CvMat* transfMatr);
  72
  73 static void icvMake2DPoints(CvMat* srcPoint,CvMat* dstPoint);
  74
  75 static void icvMake3DPoints(const CvMat* srcPoint,CvMat* dstPoint);
  76
  77 static int icvCubicV( double a2, double a1, double a0, double *squares );
  78
  79 /*=====================================================================================*/
  80
  81 /*F///////////////////////////////////////////////////////////////////////////////////////
  82 //    Name:    cvFindFundamentalMat
  83 //    Purpose: find fundamental matrix for given points using
  84 //    Context:
  85 //    Parameters:
  86 //      points1  - points on first image. Size of matrix 2xN or 3xN
  87 //      points2  - points on second image Size of matrix 2xN or 3xN
  88 //      fundMatr - found fundamental matrix (or matrixes for 7-point). Size 3x3 or 9x3
  89 //      method   - method for computing fundamental matrix
  90 //                 CV_FM_7POINT - for 7-point algorithm. Number of points == 7
  91 //                 CV_FM_8POINT - for 8-point algorithm. Number of points >= 8
  92 //                 CV_FM_RANSAC - for RANSAC  algorithm. Number of points >= 8
  93 //                 CV_FM_LMEDS  - for LMedS   algorithm. Number of points >= 8
  94 //      param1 and param2 uses for RANSAC method
  95 //        param1 - threshold distance from point to epipolar line.
  96 //                 If distance less than threshold point is good.
  97 //        param2 - probability. Usually = 0.99
  98 //        status - array, every element of which will be set to 1 if the point was good
  99 //                 and used for computation. 0 else. (for RANSAC and LMedS only)
 100 //                 (it is optional parameter, can be NULL)
 101 //
 102 //    Returns:
 103 //      number of found matrixes
 104 //F*/
 105 CV_IMPL int
 106 cvFindFundamentalMat( CvMat* points1,
 107                       CvMat* points2,
 108                       CvMat* fundMatr,
 109                       int    method,
 110                       double param1,
 111                       double param2,
 112                       CvMat* status )
 113 {
 114     int result = -1;
 115
 116     CvMat* wpoints[2]={0,0};
 117     CvMat* tmpPoints[2]={0,0};
 118
 119     CV_FUNCNAME( "icvComputeFundamentalMat" );
 120     __BEGIN__;
 121
 122     tmpPoints[0] = points1;
 123     tmpPoints[1] = points2;
 124     int numRealPoints[2];
 125     int numPoints = 0;
 126
 127     /* work with points */
 128     {
 129         int i;
 130         for( i = 0; i < 2; i++ )
 131         {
 132             int realW,realH;
 133             realW = tmpPoints[i]->cols;
 134             realH = tmpPoints[i]->rows;
 135
 136             int goodW,goodH;
 137             goodW = realW > realH ? realW : realH;
 138             goodH = realW < realH ? realW : realH;
 139
 140             if( goodH != 2 && goodH != 3 )
 141             {
 142                 CV_ERROR(CV_StsBadPoint,"Number of coordinates of points must be 2 or 3");
 143             }
 144
 145             wpoints[i] = cvCreateMat(2,goodW,CV_64F);
 146
 147             numRealPoints[i] = goodW;
 148
 149             /* Test for transpose point matrix */
 150             if( realW != goodW )
 151             {/* need to transpose point matrix */
 152                 CvMat* tmpPointMatr = 0;
 153                 tmpPointMatr = cvCreateMat(goodH,goodW,CV_64F);
 154                 cvTranspose(tmpPoints[i],tmpPointMatr);
 155                 cvMake2DPoints(tmpPointMatr,wpoints[i]);
 156                 cvReleaseMat(&tmpPointMatr);
 157             }
 158             else
 159             {
 160                 cvMake2DPoints(tmpPoints[i],wpoints[i]);
 161             }
 162
 163         }
 164
 165         if( numRealPoints[0] != numRealPoints[1] )
 166         {
 167             CV_ERROR(CV_StsBadPoint,"Number of points must be the same");
 168         }
 169
 170         numPoints = numRealPoints[0];
 171     }
 172
 173     /* work with status if use functions which don't compute it */
 174     if( status && (method == CV_FM_7POINT || method == CV_FM_8POINT ))
 175     {
 176
 177         if( !CV_IS_MAT(status) )
 178         {
 179             CV_ERROR(CV_StsBadPoint,"status is not a matrix");
 180         }
 181
 182
 183         if( !CV_IS_MAT(points1))
 184         {
 185             CV_ERROR(CV_StsBadPoint,"Points1 not a matrix");
 186         }
 187
 188         if( status->cols != numPoints || status->rows != 1 )
 189         {
 190             CV_ERROR(CV_StsBadPoint,"Size of status must be 1xN");
 191         }
 192
 193         int i;
 194         for( i = 0; i < status->cols; i++)
 195         {
 196             cvmSet(status,0,i,1.0);
 197         }
 198
 199     }
 200
 201
 202     switch( method )
 203     {
 204         case CV_FM_7POINT: result = icvComputeFundamental7Point(wpoints[1], wpoints[0], fundMatr);break;
 205
 206         case CV_FM_8POINT: result = icvComputeFundamental8Point(wpoints[1],wpoints[0], fundMatr);break;
 207
 208         case CV_FM_LMEDS : result = icvComputeFundamentalLMedS(   wpoints[1],wpoints[0], fundMatr,
 209                                         param1,param2,status);break;
 210
 211         case CV_FM_RANSAC: result = icvComputeFundamentalRANSAC(   wpoints[1],wpoints[0], fundMatr,
 212                                         param1,param2,status);break;
 213
 214         //default:return -1/*ERROR*/;
 215     }
 216
 217     __END__;
 218
 219     cvReleaseMat(&wpoints[0]);
 220     cvReleaseMat(&wpoints[1]);
 221
 222     return result;
 223 }
 224
 225 /*=====================================================================================*/
 226
 227 /* Computes 1 or 3 fundamental matrixes using 7-point algorithm */
 228 static int icvComputeFundamental7Point(CvMat* points1, CvMat* points2, CvMat* fundMatr)
 229 {
 230
 231     CvMat* squares = 0;
 232     int numberRoots = 0;
 233
 234     CV_FUNCNAME( "icvComputeFundamental7Point" );
 235     __BEGIN__;
 236
 237     /* Test correct of input data */
 238
 239     if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
 240     {
 241         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
 242     }
 243
 244     if( !CV_ARE_TYPES_EQ( points1, points2 ))
 245     {
 246         CV_ERROR( CV_StsUnmatchedSizes, "Data types of points unmatched" );
 247     }
 248
 249     int numPoint;
 250     numPoint = points1->cols;
 251
 252     /*int type;
 253     type = points1->type;*/
 254
 255     if( numPoint != points2->cols )
 256     {
 257         CV_ERROR( CV_StsBadSize, "Number of points not equal" );
 258     }
 259     if( numPoint != 7 )
 260     {
 261         CV_ERROR( CV_StsBadSize, "Number of points must be 7" );
 262     }
 263
 264     if( points1->rows != 2 && points1->rows != 3 )
 265     {
 266         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
 267     }
 268
 269     if( points2->rows != 2 && points2->rows != 3 )
 270     {
 271         CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
 272     }
 273
 274     if( ( fundMatr->rows != 3 && fundMatr->rows != 9 )|| fundMatr->cols != 3 )
 275     {
 276         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3 or 9x3" );
 277     }
 278
 279     squares = cvCreateMat(2,3,CV_64F);
 280
 281
 282     /* Make it Normalize points if need */
 283     double wPoints1_dat[2*7];
 284     double wPoints2_dat[2*7];
 285     CvMat wPoints1;
 286     CvMat wPoints2;
 287     wPoints1 = cvMat(2,7,CV_64F,wPoints1_dat);
 288     wPoints2 = cvMat(2,7,CV_64F,wPoints2_dat);
 289
 290     icvMake2DPoints(points1,&wPoints1);
 291     icvMake2DPoints(points2,&wPoints2);
 292
 293     /* fill matrix U */
 294
 295     int currPoint;
 296     CvMat matrU;
 297     double matrU_dat[7*9];
 298     matrU = cvMat(7,9,CV_64F,matrU_dat);
 299
 300     double* currDataLine;
 301     currDataLine = matrU_dat;
 302     for( currPoint = 0; currPoint < 7; currPoint++ )
 303     {
 304         double x1,y1,x2,y2;
 305         x1 = cvmGet(&wPoints1,0,currPoint);
 306         y1 = cvmGet(&wPoints1,1,currPoint);
 307         x2 = cvmGet(&wPoints2,0,currPoint);
 308         y2 = cvmGet(&wPoints2,1,currPoint);
 309
 310         currDataLine[0] = x1*x2;
 311         currDataLine[1] = x1*y2;
 312         currDataLine[2] = x1;
 313         currDataLine[3] = y1*x2;
 314         currDataLine[4] = y1*y2;
 315         currDataLine[5] = y1;
 316         currDataLine[6] = x2;
 317         currDataLine[7] = y2;
 318         currDataLine[8] = 1;
 319
 320         currDataLine += 9;
 321     }
 322
 323     CvMat matrUU;
 324     CvMat matrSS;
 325     CvMat matrVV;
 326     double matrUU_dat[7*7];
 327     double matrSS_dat[7*9];
 328     double matrVV_dat[9*9];
 329     matrUU = cvMat(7,7,CV_64F,matrUU_dat);
 330     matrSS = cvMat(7,9,CV_64F,matrSS_dat);
 331     matrVV = cvMat(9,9,CV_64F,matrVV_dat);
 332
 333     cvSVD( &matrU, &matrSS, &matrUU, &matrVV, 0/*CV_SVD_V_T*/ );/* get transposed matrix V */
 334
 335     double F111,F112,F113;
 336     double F121,F122,F123;
 337     double F131,F132,F133;
 338
 339     double F211,F212,F213;
 340     double F221,F222,F223;
 341     double F231,F232,F233;
 342
 343     F111=cvmGet(&matrVV,0,7);
 344     F112=cvmGet(&matrVV,1,7);
 345     F113=cvmGet(&matrVV,2,7);
 346     F121=cvmGet(&matrVV,3,7);
 347     F122=cvmGet(&matrVV,4,7);
 348     F123=cvmGet(&matrVV,5,7);
 349     F131=cvmGet(&matrVV,6,7);
 350     F132=cvmGet(&matrVV,7,7);
 351     F133=cvmGet(&matrVV,8,7);
 352
 353     F211=cvmGet(&matrVV,0,8);
 354     F212=cvmGet(&matrVV,1,8);
 355     F213=cvmGet(&matrVV,2,8);
 356     F221=cvmGet(&matrVV,3,8);
 357     F222=cvmGet(&matrVV,4,8);
 358     F223=cvmGet(&matrVV,5,8);
 359     F231=cvmGet(&matrVV,6,8);
 360     F232=cvmGet(&matrVV,7,8);
 361     F233=cvmGet(&matrVV,8,8);
 362
 363     double a,b,c,d;
 364
 365     a =   F231*F112*F223 + F231*F212*F123 - F231*F212*F223 + F231*F113*F122 -
 366           F231*F113*F222 - F231*F213*F122 + F231*F213*F222 - F131*F112*F223 -
 367           F131*F212*F123 + F131*F212*F223 - F131*F113*F122 + F131*F113*F222 +
 368           F131*F213*F122 - F131*F213*F222 + F121*F212*F133 - F121*F212*F233 +
 369           F121*F113*F132 - F121*F113*F232 - F121*F213*F132 + F121*F213*F232 +
 370           F221*F112*F133 - F221*F112*F233 - F221*F212*F133 + F221*F212*F233 -
 371           F221*F113*F132 + F221*F113*F232 + F221*F213*F132 - F221*F213*F232 +
 372           F121*F112*F233 - F111*F222*F133 + F111*F222*F233 - F111*F123*F132 +
 373           F111*F123*F232 + F111*F223*F132 - F111*F223*F232 - F211*F122*F133 +
 374           F211*F122*F233 + F211*F222*F133 - F211*F222*F233 + F211*F123*F132 -
 375           F211*F123*F232 - F211*F223*F132 + F211*F223*F232 + F111*F122*F133 -
 376           F111*F122*F233 - F121*F112*F133 + F131*F112*F123 - F231*F112*F123;
 377
 378     b =   2*F231*F213*F122 - 3*F231*F213*F222 + F231*F112*F123   - 2*F231*F112*F223 -
 379           2*F231*F212*F123 + 3*F231*F212*F223 - F231*F113*F122   + 2*F231*F113*F222 +
 380           F131*F212*F123   - 2*F131*F212*F223 - F131*F113*F222   - F131*F213*F122   +
 381           2*F131*F213*F222 + F121*F113*F232   + F121*F213*F132   - 2*F121*F213*F232 -
 382           F221*F112*F133   + 2*F221*F112*F233 + 2*F221*F212*F133 - 3*F221*F212*F233 +
 383           F221*F113*F132   - 2*F221*F113*F232 - 2*F221*F213*F132 + 3*F221*F213*F232 +
 384           F131*F112*F223   - 2*F211*F122*F233 - 2*F211*F222*F133 + 3*F211*F222*F233 -
 385           F211*F123*F132   + 2*F211*F123*F232 + 2*F211*F223*F132 - 3*F211*F223*F232 -
 386           F121*F112*F233   - F121*F212*F133   + 2*F121*F212*F233 - 2*F111*F222*F233 -
 387           F111*F123*F232   - F111*F223*F132   + 2*F111*F223*F232 + F111*F122*F233   +
 388           F111*F222*F133   + F211*F122*F133;
 389
 390     c =   F231*F112*F223   + F231*F212*F123   - 3*F231*F212*F223 - F231*F113*F222   -
 391           F231*F213*F122   + 3*F231*F213*F222 + F131*F212*F223   - F131*F213*F222   +
 392           F121*F213*F232   - F221*F112*F233   - F221*F212*F133   + 3*F221*F212*F233 +
 393           F221*F113*F232   + F221*F213*F132   - 3*F221*F213*F232 + F211*F122*F233   +
 394           F211*F222*F133   - 3*F211*F222*F233 - F211*F123*F232   - F211*F223*F132   +
 395           3*F211*F223*F232 - F121*F212*F233   + F111*F222*F233   - F111*F223*F232;
 396
 397     d =   F221*F213*F232 - F211*F223*F232 + F211*F222*F233 - F221*F212*F233 +
 398           F231*F212*F223 - F231*F213*F222;
 399
 400     /* find root */
 401     double coeffs_dat[4];
 402     CvMat coeffs;
 403     coeffs = cvMat(1,4,CV_64F,coeffs_dat);
 404
 405     cvmSet(&coeffs,0,0,a);
 406     cvmSet(&coeffs,0,1,b);
 407     cvmSet(&coeffs,0,2,c);
 408     cvmSet(&coeffs,0,3,d);
 409
 410     int numCubRoots;
 411     numCubRoots = cvSolveCubic(&coeffs,squares);
 412
 413     /* take real solution */
 414     /* Need test all roots */
 415
 416     int i;
 417     for( i = 0; i < numCubRoots; i++ )
 418     {
 419         if( fabs(cvmGet(squares,1,i)) < 1e-8 )
 420         {//It is real square. (Im==0)
 421
 422             double sol;
 423             sol = cvmGet(squares,0,i);
 424             //F=sol*F1+(1-sol)*F2;
 425
 426             int t;
 427             for( t = 0; t < 9; t++ )
 428             {
 429                 double f1,f2;
 430                 f1 = cvmGet(&matrVV,t,7);
 431                 f2 = cvmGet(&matrVV,t,8);
 432
 433                 double s = f1 * sol + (1-sol) * f2;
 434                 cvmSet(fundMatr,numberRoots*3 + t/3,t%3,s);
 435             }
 436             numberRoots++;
 437
 438             if( fundMatr->rows == 3 )/* not enough space to store more than one root */
 439                 break;
 440         }
 441     }
 442
 443     /* scale fundamental matrix */
 444     for( i = 0; i < numberRoots; i++ )
 445     {
 446
 447         double fsc;
 448         fsc = cvmGet(fundMatr,i*3+2,2);
 449         if( fabs(fsc) > 1e-8 )
 450         {
 451             CvMat subFund;
 452             cvGetSubArr( fundMatr, &subFund, cvRect(0,i*3,3,3) );
 453             cvScale(&subFund,&subFund,1.0/fsc);
 454         }
 455     }
 456
 457     __END__;
 458
 459     cvReleaseMat(&squares);
 460
 461
 462     return numberRoots;
 463 }
 464
 465
 466 /*=====================================================================================*/
 467
 468 static int icvComputeFundamental8Point(CvMat* points1,CvMat* points2, CvMat* fundMatr)
 469 {
 470     CvMat* wpoints[2]={0,0};
 471     CvMat* preFundMatr = 0;
 472     CvMat* sqdists = 0;
 473     CvMat* matrA = 0;
 474     CvMat* matrU = 0;
 475     CvMat* matrW = 0;
 476     CvMat* matrV = 0;
 477
 478     int numFundMatrs = 0;
 479
 480     CV_FUNCNAME( "icvComputeFundamental8Point" );
 481     __BEGIN__;
 482
 483     /* Test correct of input data */
 484
 485     if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
 486     {
 487         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
 488     }
 489
 490     if( !CV_ARE_TYPES_EQ( points1, points2 ))
 491     {
 492         CV_ERROR( CV_StsUnmatchedSizes, "Data types of points unmatched" );
 493     }
 494
 495     int numPoint;
 496     numPoint = points1->cols;
 497
 498     /*int type;
 499     type = points1->type;*/
 500
 501     if( numPoint != points2->cols )
 502     {
 503         CV_ERROR( CV_StsBadSize, "Number of points not equal" );
 504     }
 505     if( numPoint < 8 )
 506     {
 507         CV_ERROR( CV_StsBadSize, "Number of points must be at least 8" );
 508     }
 509
 510     if( points1->rows > 3 || points1->rows < 2 )
 511     {
 512         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
 513     }
 514
 515     if( points2->rows > 3 || points2->rows < 2 )
 516     {
 517         CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
 518     }
 519
 520     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
 521     {
 522         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
 523     }
 524
 525     /* allocate data */
 526     CV_CALL( wpoints[0] = cvCreateMat(2,numPoint,CV_64F) );
 527     CV_CALL( wpoints[1] = cvCreateMat(2,numPoint,CV_64F) );
 528     CV_CALL( matrA = cvCreateMat(numPoint,9,CV_64F) );
 529     CV_CALL( preFundMatr = cvCreateMat(3,3,CV_64F) );
 530     CV_CALL( matrU = cvCreateMat(numPoint,numPoint,CV_64F) );
 531     CV_CALL( matrW = cvCreateMat(numPoint,9,CV_64F) );
 532     CV_CALL( matrV = cvCreateMat(9,9,CV_64F) );
 533     CV_CALL( sqdists = cvCreateMat(1,numPoint,CV_64F) );
 534
 535     /* Create working points with just x,y */
 536
 537     CvMat transfMatr[2];
 538     double transfMatr1_dat[9];
 539     double transfMatr2_dat[9];
 540     transfMatr[0] = cvMat(3,3,CV_64F,transfMatr1_dat);
 541     transfMatr[1] = cvMat(3,3,CV_64F,transfMatr2_dat);
 542
 543     {/* transform to x,y.  tranform point and compute transform matrixes */
 544         icvMake2DPoints(points1,wpoints[0]);
 545         icvMake2DPoints(points2,wpoints[1]);
 546
 547         icvNormalizeFundPoints( wpoints[0],&transfMatr[0]);
 548         icvNormalizeFundPoints( wpoints[1],&transfMatr[1]);
 549
 550         /* we have normalized working points wpoints[0] and wpoints[1] */
 551         /* build matrix A from points coordinates */
 552
 553         int currPoint;
 554         for( currPoint = 0; currPoint < numPoint; currPoint++ )
 555         {
 556             CvMat rowA;
 557             double x1,y1;
 558             double x2,y2;
 559
 560             x1 = cvmGet(wpoints[0],0,currPoint);
 561             y1 = cvmGet(wpoints[0],1,currPoint);
 562             x2 = cvmGet(wpoints[1],0,currPoint);
 563             y2 = cvmGet(wpoints[1],1,currPoint);
 564
 565             cvGetRow(matrA,&rowA,currPoint);
 566             rowA.data.db[0] = x1*x2;
 567             rowA.data.db[1] = x1*y2;
 568             rowA.data.db[2] = x1;
 569
 570             rowA.data.db[3] = y1*x2;
 571             rowA.data.db[4] = y1*y2;
 572             rowA.data.db[5] = y1;
 573
 574             rowA.data.db[6] = x2;
 575             rowA.data.db[7] = y2;
 576             rowA.data.db[8] = 1;
 577         }
 578     }
 579
 580     /* We have matrix A. Compute svd(A). We need last column of V */
 581
 582
 583     cvSVD( matrA, matrW, matrU, matrV, CV_SVD_V_T );/* get transposed matrix V */
 584
 585     /* Compute number of non zero eigen values */
 586     int numEig;
 587     numEig = 0;
 588     {
 589         int i;
 590         for( i = 0; i < 8; i++ )
 591         {
 592             if( cvmGet(matrW,i,i) < 1e-8 )
 593             {
 594                 break;
 595             }
 596         }
 597
 598         numEig = i;
 599
 600     }
 601
 602     if( numEig < 5 )
 603     {/* Bad points */
 604         numFundMatrs = 0;
 605     }
 606     else
 607     {
 608         numFundMatrs = 1;
 609
 610         /* copy last row of matrix V to precomputed fundamental matrix */
 611
 612         CvMat preF;
 613         cvGetRow(matrV,&preF,8);
 614         cvReshape(&preF,preFundMatr,1,3);
 615
 616         /* Apply singularity condition */
 617         icvMakeFundamentalSingular(preFundMatr);
 618
 619         /* Denormalize fundamental matrix */
 620         /* Compute transformation for normalization */
 621
 622         CvMat wfundMatr;
 623         double wfundMatr_dat[9];
 624         wfundMatr = cvMat(3,3,CV_64F,wfundMatr_dat);
 625
 626         {/*  Freal = T1'*Fpre*T2 */
 627             double tmpMatr_dat[9];
 628             double tmptransfMatr_dat[9];
 629
 630             CvMat tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
 631             CvMat tmptransfMatr = cvMat(3,3,CV_64F,tmptransfMatr_dat);
 632
 633             cvTranspose(&transfMatr[0],&tmptransfMatr);
 634
 635             cvMatMul(&tmptransfMatr,preFundMatr,&tmpMatr);
 636             cvMatMul(&tmpMatr,&transfMatr[1],&wfundMatr);
 637         }
 638
 639         /* scale fundamental matrix */
 640         double fsc;
 641         fsc = cvmGet(&wfundMatr,2,2);
 642         if( fabs(fsc) > 1.0e-8 )
 643         {
 644             cvScale(&wfundMatr,&wfundMatr,1.0/fsc);
 645         }
 646
 647         /* copy result fundamental matrix */
 648         cvConvert( &wfundMatr, fundMatr );
 649
 650     }
 651
 652
 653     __END__;
 654
 655     cvReleaseMat(&matrU);
 656     cvReleaseMat(&matrW);
 657     cvReleaseMat(&matrV);
 658     cvReleaseMat(&preFundMatr);
 659     cvReleaseMat(&matrA);
 660     cvReleaseMat(&wpoints[0]);
 661     cvReleaseMat(&wpoints[1]);
 662     cvReleaseMat(&sqdists);
 663
 664     return numFundMatrs;
 665 }
 666
 667 /*=====================================================================================*/
 668
 669 /* Computes fundamental matrix using RANSAC method */
 670 /*  */
 671 static int icvComputeFundamentalRANSAC(   CvMat* points1,CvMat* points2,
 672                                         CvMat* fundMatr,
 673                                         double threshold,/* Threshold for good points */
 674                                         double p,/* Probability of good result. */
 675                                         CvMat* status)
 676 {
 677     CvMat* wpoints1 = 0;
 678     CvMat* wpoints2 = 0;
 679     CvMat* corrLines1 = 0;
 680     CvMat* corrLines2 = 0;
 681     CvMat* bestPoints1 = 0;
 682     CvMat* bestPoints2 = 0;
 683
 684     int* flags = 0;
 685     int* bestFlags = 0;
 686     int numFundMatr = 0;
 687
 688     CV_FUNCNAME( "icvComputeFundamentalRANSAC" );
 689     __BEGIN__;
 690
 691     /* Test correct of input data */
 692
 693     if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
 694     {
 695         CV_ERROR(CV_StsBadPoint,"points1 or points2 or funMatr are not a matrixes");
 696     }
 697
 698     int numPoint;
 699     numPoint = points1->cols;
 700
 701     /*int type;
 702     type = points1->type;*/
 703
 704     if( numPoint != points2->cols )
 705     {
 706         CV_ERROR( CV_StsBadSize, "Number of points not equals" );
 707     }
 708     if( numPoint < 8 )
 709     {
 710         CV_ERROR( CV_StsBadSize, "Number of points must be >= 8" );
 711     }
 712
 713     if( points1->rows != 2 && points1->rows != 3 )
 714     {
 715         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
 716     }
 717
 718     if( points2->rows != 2 && points2->rows != 3 )
 719     {
 720         CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
 721     }
 722
 723     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
 724     {
 725         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
 726     }
 727
 728     if( status )
 729     {/* status is present test it */
 730         if( !CV_IS_MAT(status) )
 731         {
 732             CV_ERROR(CV_StsBadPoint,"status is not a matrix");
 733         }
 734
 735         if( status->cols != numPoint || status->rows != 1 )
 736         {
 737             CV_ERROR(CV_StsBadPoint,"Size of status must be 1xN");
 738         }
 739     }
 740
 741     /* We will choose adaptive number of N (samples) */
 742
 743     /* Convert points to 64F working points */
 744
 745     CV_CALL( flags = (int*)cvAlloc(numPoint * sizeof(int)) );
 746     CV_CALL( bestFlags = (int*)cvAlloc(numPoint * sizeof(int)) );
 747     CV_CALL( wpoints1 = cvCreateMat(3,numPoint,CV_64F) );
 748     CV_CALL( wpoints2 = cvCreateMat(3,numPoint,CV_64F) );
 749     CV_CALL( corrLines1 = cvCreateMat(3,numPoint,CV_64F));
 750     CV_CALL( corrLines2 = cvCreateMat(3,numPoint,CV_64F));
 751     CV_CALL( bestPoints1 = cvCreateMat(3,numPoint,CV_64F));
 752     CV_CALL( bestPoints2 = cvCreateMat(3,numPoint,CV_64F));
 753
 754     icvMake3DPoints(points1,wpoints1);
 755     icvMake3DPoints(points2,wpoints2);
 756
 757     {
 758         int wasCount = 0;  //count of choosing samples
 759         int maxGoodPoints = 0;
 760         int numGoodPoints = 0;
 761
 762         double bestFund_dat[9];
 763         CvMat  bestFund;
 764         bestFund = cvMat(3,3,CV_64F,bestFund_dat);
 765
 766         /* choosen points */
 767         int NumSamples = 500;/* Initial need number of samples */
 768         while( wasCount < NumSamples )
 769         {
 770             /* select samples */
 771             int randNumbs[7];
 772             int i;
 773             int newnum;
 774             int pres;
 775             for( i = 0; i < 7; i++ )
 776             {
 777                 do
 778                 {
 779                     newnum = rand()%numPoint;
 780
 781                     /* test this number */
 782                     pres = 0;
 783                     int test;
 784                     for( test = 0; test < i; test++ )
 785                     {
 786                         if( randNumbs[test] == newnum )
 787                         {
 788                             pres = 1;
 789                             break;
 790                         }
 791                     }
 792                 }
 793                 while( pres );
 794                 randNumbs[i] = newnum;
 795             }
 796             /* random numbers of points was generated */
 797             /* select points */
 798
 799             double selPoints1_dat[2*7];
 800             double selPoints2_dat[2*7];
 801             CvMat selPoints1;
 802             CvMat selPoints2;
 803             selPoints1 = cvMat(2,7,CV_64F,selPoints1_dat);
 804             selPoints2 = cvMat(2,7,CV_64F,selPoints2_dat);
 805             /* copy points */
 806             int t;
 807             for( t = 0; t < 7; t++ )
 808             {
 809                 double x,y;
 810                 x = cvmGet(wpoints1,0,randNumbs[t]);
 811                 y = cvmGet(wpoints1,1,randNumbs[t]);
 812                 cvmSet(&selPoints1,0,t,x);
 813                 cvmSet(&selPoints1,1,t,y);
 814
 815                 x = cvmGet(wpoints2,0,randNumbs[t]);
 816                 y = cvmGet(wpoints2,1,randNumbs[t]);
 817                 cvmSet(&selPoints2,0,t,x);
 818                 cvmSet(&selPoints2,1,t,y);
 819             }
 820
 821             /* Compute fundamental matrix using 7-points algorithm */
 822
 823             double fundTriple_dat[27];
 824             CvMat fundTriple;
 825             fundTriple = cvMat(9,3,CV_64F,fundTriple_dat);
 826
 827             int numFund = icvComputeFundamental7Point(&selPoints1,&selPoints2,&fundTriple);
 828
 829             //double fund7_dat[9];
 830             CvMat fund7;
 831             //fund7 = cvMat(3,3,CV_64F,fund7_dat);
 832
 833             /* get sub matrix */
 834             for( int currFund = 0; currFund < numFund; currFund++ )
 835             {
 836                 cvGetSubArr(&fundTriple,&fund7,cvRect(0,currFund*3,3,3));
 837                 {
 838                     /* Compute inliers for this fundamental matrix */
 839                     /* Compute distances for points and correspond lines */
 840                     {
 841                         /* Create corresponde lines */
 842
 843                         cvComputeCorrespondEpilines(wpoints1,2,&fund7,corrLines2);
 844                         cvComputeCorrespondEpilines(wpoints2,1,&fund7,corrLines1);
 845                         /* compute distances for points and number of good points */
 846                         int i;
 847                         numGoodPoints = 0;
 848                         for( i = 0; i < numPoint; i++ )
 849                         {
 850                             CvMat pnt1,pnt2;
 851                             CvMat lin1,lin2;
 852                             cvGetCol(wpoints1,&pnt1,i);
 853                             cvGetCol(corrLines1,&lin1,i);
 854                             cvGetCol(wpoints2,&pnt2,i);
 855                             cvGetCol(corrLines2,&lin2,i);
 856                             double dist1,dist2;
 857                             dist1 = fabs(cvDotProduct(&pnt1,&lin1));
 858                             dist2 = fabs(cvDotProduct(&pnt2,&lin2));
 859                             flags[i] = ( dist1 < threshold && dist2 < threshold )?1:0;
 860                             numGoodPoints += flags[i];
 861                         }
 862                     }
 863                 }
 864
 865                 if( numGoodPoints > maxGoodPoints )
 866                 {/* good matrix */
 867                     cvCopy(&fund7,&bestFund);
 868                     maxGoodPoints = numGoodPoints;
 869                     /* copy best flags */
 870                     int i;
 871                     for(i=0;i<numPoint;i++)
 872                     {
 873                         bestFlags[i] = flags[i];
 874                     }
 875
 876                     /* Recompute new number of need steps */
 877
 878                     /* Adaptive number of samples to count*/
 879                     double ep = 1 - (double)numGoodPoints / (double)numPoint;
 880                     if( ep == 1 )
 881                     {
 882                         ep = 0.5;//if there is not good points set ration of outliers to 50%
 883                     }
 884
 885                     double newNumSamples = log(1-p);
 886                     newNumSamples /= log(1-pow(1-ep,7));
 887
 888                     if( newNumSamples < (double)NumSamples )
 889                     {
 890                         NumSamples = cvRound(newNumSamples);
 891                     }
 892
 893                 }
 894             }
 895
 896             wasCount++;
 897         }
 898
 899         /* we have best 7-point fundamental matrix. */
 900         /* and best points */
 901         /* use these points to improve matrix */
 902
 903         /* collect best points */
 904         /* copy points */
 905
 906         /* Test number of points. And if number >=8 improove found fundamental matrix  */
 907
 908         if( maxGoodPoints < 7 )
 909         {
 910             /* Fundamental matrix not found */
 911             numFundMatr = 0;
 912         }
 913         else
 914         {
 915             if( maxGoodPoints > 7 )
 916             {
 917                 /* Found >= 8 point. Improove matrix */
 918                 int i;
 919                 int currPnt;
 920                 currPnt = 0;
 921
 922                 for( i = 0; i < numPoint; i++ )
 923                 {
 924                     if( bestFlags[i] )
 925                     {
 926                         CvMat wPnt;
 927                         CvMat bestPnt;
 928                         cvGetCol( wpoints1,&wPnt, i );
 929                         cvGetCol( bestPoints1,&bestPnt, currPnt );
 930                         cvCopy(&wPnt,&bestPnt);
 931                         cvGetCol( wpoints2,&wPnt, i );
 932                         cvGetCol( bestPoints2,&bestPnt, currPnt );
 933                         cvCopy(&wPnt,&bestPnt);
 934                         currPnt++;
 935                     }
 936                 }
 937
 938                 CvMat wbestPnts1;
 939                 CvMat wbestPnts2;
 940
 941                 cvGetSubArr( bestPoints1, &wbestPnts1, cvRect(0,0,maxGoodPoints,3) );
 942                 cvGetSubArr( bestPoints2, &wbestPnts2, cvRect(0,0,maxGoodPoints,3) );
 943
 944                 /* best points was collectet. Improve fundamental matrix */
 945                 /* Just use 8-point algorithm */
 946                 double impFundMatr_dat[9];
 947                 CvMat impFundMatr;
 948                 impFundMatr = cvMat(3,3,CV_64F,impFundMatr_dat);
 949                 numFundMatr = icvComputeFundamental8Point(&wbestPnts1,&wbestPnts2,&impFundMatr);
 950
 951                 cvConvert(&impFundMatr,fundMatr);
 952                 //cvConvert(&bestFund,fundMatr); // This line must be deleted
 953             }
 954             else
 955             {
 956                 /* 7 point. Just copy to result */
 957                 cvConvert(&bestFund,fundMatr);
 958                 numFundMatr = 1;
 959             }
 960
 961             /* copy flag to status if need */
 962             if( status )
 963             {
 964                 for( int i = 0; i < numPoint; i++)
 965                 {
 966                     cvmSet(status,0,i,(double)bestFlags[i]);
 967                 }
 968             }
 969         }
 970
 971
 972     }
 973
 974     __END__;
 975
 976     /* free allocated memory */
 977
 978     cvReleaseMat(&corrLines1);
 979     cvReleaseMat(&corrLines2);
 980     cvReleaseMat(&wpoints1);
 981     cvReleaseMat(&wpoints2);
 982     cvReleaseMat(&bestPoints1);
 983     cvReleaseMat(&bestPoints2);
 984     cvFree((void**)&flags);
 985     cvFree((void**)&bestFlags);
 986
 987     return numFundMatr;
 988 }//icvComputeFundamentalRANSAC
 989
 990 /*=====================================================================================*/
 991
 992 static void icvCompPointLineDists(CvMat* points,CvMat* lines,CvMat* distances)
 993 {/* Line must be normalized */
 994
 995     int numPoints;
 996     numPoints = points->cols;
 997     if( numPoints != lines->cols && numPoints != distances->cols )
 998     {
 999         //printf("Size if arrays not equals\n");
1000         return;//error
1001     }
1002
1003     int i;
1004     for( i = 0; i < numPoints; i++ )
1005     {
1006         CvMat pnt;
1007         CvMat lin;
1008         cvGetCol(points,&pnt,i);
1009         cvGetCol(lines,&lin,i);
1010         double dist;
1011         dist = fabs(cvDotProduct(&pnt,&lin));
1012         cvmSet(distances,0,i,dist);
1013     }
1014
1015     return;
1016 }
1017
1018 /*=====================================================================================*/
1019
1020
1021
1022 #define _compVals( v1, v2 )  ((v1) < (v2))
1023
1024 /* Create function to sort vector */
1025 static CV_IMPLEMENT_QSORT( _SortCvMatVect, double, _compVals )
1026
1027 /*=====================================================================================*/
1028 static int icvComputeFundamentalLMedS(    CvMat* points1,CvMat* points2,
1029                                     CvMat* fundMatr,
1030                                     double threshold,/* Threshold for good points */
1031                                     double p,/* Probability of good result. */
1032                                     CvMat* status)
1033 {
1034     CvMat* wpoints1 = 0;
1035     CvMat* wpoints2 = 0;
1036     CvMat* corrLines1 = 0;
1037     CvMat* corrLines2 = 0;
1038     CvMat* bestPoints1 = 0;
1039     CvMat* bestPoints2 = 0;
1040     CvMat* dists1 = 0;
1041     CvMat* dists2 = 0;
1042     CvMat* distsSq1 = 0;
1043     CvMat* distsSq2 = 0;
1044     CvMat* allDists = 0;
1045
1046     int* flags = 0;
1047     int* bestFlags = 0;
1048     int numFundMatr = 0;
1049
1050     CV_FUNCNAME( "icvComputeFundamentalLMedS" );
1051     __BEGIN__;
1052
1053     /* Test correct of input data */
1054
1055     if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
1056     {
1057         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
1058     }
1059
1060     int numPoint;
1061     numPoint = points1->cols;
1062
1063     /*int type;
1064     type = points1->type;*/
1065
1066     if( numPoint != points2->cols )
1067     {
1068         CV_ERROR( CV_StsBadSize, "Number of points not equal" );
1069     }
1070     if( numPoint < 8 )
1071     {
1072         CV_ERROR( CV_StsBadSize, "Number of points must be >= 8" );
1073     }
1074
1075     if( points1->rows != 2 && points1->rows != 3 )
1076     {
1077         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
1078     }
1079
1080     if( points2->rows != 2 && points2->rows != 3 )
1081     {
1082         CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
1083     }
1084
1085     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
1086     {
1087         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
1088     }
1089
1090     if( status )
1091     {/* status is present test it */
1092         if( !CV_IS_MAT(status) )
1093         {
1094             CV_ERROR(CV_StsBadPoint,"status is not a matrix");
1095         }
1096
1097         if( status->cols != numPoint || status->rows != 1 )
1098         {
1099             CV_ERROR(CV_StsBadPoint,"Size of status must be 1xN");
1100         }
1101     }
1102
1103     /* We will choose adaptive number of N (samples) */
1104
1105     /* Convert points to 64F working points */
1106
1107     CV_CALL( flags = (int*)cvAlloc(numPoint * sizeof(int)) );
1108     CV_CALL( bestFlags = (int*)cvAlloc(numPoint * sizeof(int)) );
1109     CV_CALL( wpoints1 = cvCreateMat(3,numPoint,CV_64F) );
1110     CV_CALL( wpoints2 = cvCreateMat(3,numPoint,CV_64F) );
1111     CV_CALL( corrLines1 = cvCreateMat(3,numPoint,CV_64F));
1112     CV_CALL( corrLines2 = cvCreateMat(3,numPoint,CV_64F));
1113     CV_CALL( bestPoints1 = cvCreateMat(3,numPoint,CV_64F));
1114     CV_CALL( bestPoints2 = cvCreateMat(3,numPoint,CV_64F));
1115     CV_CALL( dists1 = cvCreateMat(1,numPoint,CV_64F));
1116     CV_CALL( dists2 = cvCreateMat(1,numPoint,CV_64F));
1117     CV_CALL( distsSq1 = cvCreateMat(1,numPoint,CV_64F));
1118     CV_CALL( distsSq2 = cvCreateMat(1,numPoint,CV_64F));
1119     CV_CALL( allDists = cvCreateMat(1,numPoint,CV_64F));
1120
1121     icvMake3DPoints(points1,wpoints1);
1122     icvMake3DPoints(points2,wpoints2);
1123
1124     {
1125         int NumSamples = 500;//Maximux number of steps
1126         int wasCount = 0;  //count of choosing samples
1127
1128         double goodMean = FLT_MAX;
1129         double currMean;
1130
1131         int numGoodPoints = 0;
1132
1133         double bestFund_dat[9];
1134         CvMat  bestFund;
1135         bestFund = cvMat(3,3,CV_64F,bestFund_dat);
1136
1137         /* choosen points */
1138         while( wasCount < NumSamples )
1139         {
1140             /* select samples */
1141             int randNumbs[7];
1142             int i;
1143             int newnum;
1144             int pres;
1145             for( i = 0; i < 7; i++ )
1146             {
1147                 do
1148                 {
1149                     newnum = rand()%numPoint;
1150
1151                     /* test this number */
1152                     pres = 0;
1153                     int test;
1154                     for( test = 0; test < i; test++ )
1155                     {
1156                         if( randNumbs[test] == newnum )
1157                         {
1158                             pres = 1;
1159                             break;
1160                         }
1161                     }
1162                 }
1163                 while( pres );
1164                 randNumbs[i] = newnum;
1165             }
1166             /* random numbers of points was generated */
1167             /* select points */
1168
1169             double selPoints1_dat[2*7];
1170             double selPoints2_dat[2*7];
1171             CvMat selPoints1;
1172             CvMat selPoints2;
1173             selPoints1 = cvMat(2,7,CV_64F,selPoints1_dat);
1174             selPoints2 = cvMat(2,7,CV_64F,selPoints2_dat);
1175             /* copy points */
1176             int t;
1177             for( t = 0; t < 7; t++ )
1178             {
1179                 double x,y;
1180                 x = cvmGet(wpoints1,0,randNumbs[t]);
1181                 y = cvmGet(wpoints1,1,randNumbs[t]);
1182                 cvmSet(&selPoints1,0,t,x);
1183                 cvmSet(&selPoints1,1,t,y);
1184
1185                 x = cvmGet(wpoints2,0,randNumbs[t]);
1186                 y = cvmGet(wpoints2,1,randNumbs[t]);
1187                 cvmSet(&selPoints2,0,t,x);
1188                 cvmSet(&selPoints2,1,t,y);
1189             }
1190             /* Compute fundamental matrix using 7-points algorithm */
1191
1192             double fundTriple_dat[27];
1193             CvMat fundTriple;
1194             fundTriple = cvMat(9,3,CV_64F,fundTriple_dat);
1195
1196             int numFund = icvComputeFundamental7Point(&selPoints1,&selPoints2,&fundTriple);
1197
1198             //double fund7_dat[9];
1199             CvMat fund7;
1200             //fund7 = cvMat(3,3,CV_64F,fund7_dat);
1201
1202             /* get sub matrix */
1203             for( int currFund = 0; currFund < numFund; currFund++ )
1204             {
1205
1206                 cvGetSubArr(&fundTriple,&fund7,cvRect(0,currFund*3,3,3));
1207                 {
1208                     /* Compute median error for this matrix */
1209                     {
1210                         cvComputeCorrespondEpilines(wpoints1,2,&fund7,corrLines2);
1211                         cvComputeCorrespondEpilines(wpoints2,1,&fund7,corrLines1);
1212
1213                         icvCompPointLineDists(wpoints1,corrLines1,dists1);
1214                         icvCompPointLineDists(wpoints2,corrLines2,dists2);
1215
1216                         /* add distances for points (d1*d1+d2*d2) */
1217                         cvMul(dists1,dists1,distsSq1);
1218                         cvMul(dists2,dists2,distsSq2);
1219
1220                         cvAdd(distsSq1,distsSq2,allDists);
1221
1222                         /* sort distances */
1223                         _SortCvMatVect(allDists->data.db,numPoint,0);
1224
1225                         /* get median error */
1226                         currMean = allDists->data.db[numPoint/2];
1227                     }
1228                 }
1229
1230                 if( currMean < goodMean )
1231                 {/* good matrix */
1232                     cvCopy(&fund7,&bestFund);
1233                     goodMean = currMean;
1234
1235                     /* Compute number of good points using threshold */
1236                     int i;
1237                     numGoodPoints = 0;
1238                     for( i = 0 ; i < numPoint; i++ )
1239                     {
1240                         if( dists1->data.db[i] < threshold && dists2->data.db[i] < threshold )
1241                         {
1242                             numGoodPoints++;
1243                         }
1244                     }
1245
1246
1247                     /* Compute adaptive number of steps */
1248                     double ep = 1 - (double)numGoodPoints / (double)numPoint;
1249                     if( ep == 1 )
1250                     {
1251                         ep = 0.5;//if there is not good points set ration of outliers to 50%
1252                     }
1253
1254                     double newNumSamples = log(1-p);
1255                     newNumSamples /= log(1-pow(1-ep,7));
1256                     if( newNumSamples < (double)NumSamples )
1257                     {
1258                         NumSamples = cvRound(newNumSamples);
1259                     }
1260                 }
1261             }
1262
1263             wasCount++;
1264         }
1265
1266         /* Select just good points using threshold */
1267         /* Compute distances for all points for best fundamental matrix */
1268
1269         /* Test if we have computed fundamental matrix*/
1270         if( goodMean == FLT_MAX )
1271         {
1272             numFundMatr = 0;
1273
1274         }
1275         else
1276         {/* we have computed fundamental matrix */
1277             {
1278                 cvComputeCorrespondEpilines(wpoints1,2,&bestFund,corrLines2);
1279                 cvComputeCorrespondEpilines(wpoints2,1,&bestFund,corrLines1);
1280
1281                 icvCompPointLineDists(wpoints1,corrLines1,dists1);
1282                 icvCompPointLineDists(wpoints2,corrLines2,dists2);
1283
1284                 /* test dist for each point and set status for each point if need */
1285                 int i;
1286                 int currPnt = 0;
1287                 for( i = 0; i < numPoint; i++ )
1288                 {
1289                     if( dists1->data.db[i] < threshold && dists2->data.db[i] < threshold )
1290                     {
1291                         CvMat wPnt;
1292                         CvMat bestPnt;
1293                         cvGetCol( wpoints1,&wPnt, i );
1294                         cvGetCol( bestPoints1,&bestPnt, currPnt );
1295                         cvCopy(&wPnt,&bestPnt);
1296                         cvGetCol( wpoints2,&wPnt, i );
1297                         cvGetCol( bestPoints2,&bestPnt, currPnt );
1298                         cvCopy(&wPnt,&bestPnt);
1299                         currPnt++;
1300
1301                         if( status )
1302                             cvmSet(status,0,i,1.0);
1303                     }
1304                     else
1305                     {
1306                         if( status )
1307                             cvmSet(status,0,i,0.0);
1308                     }
1309
1310                 }
1311                 numGoodPoints = currPnt;
1312             }
1313
1314             /* we have best 7-point fundamental matrix. */
1315             /* and best points */
1316             /* use these points to improove matrix */
1317
1318             /* Test number of points. And if number >=8 improove found fundamental matrix  */
1319
1320             if( numGoodPoints < 7 )
1321             {
1322                 /* Fundamental matrix not found */
1323                 numFundMatr = 0;
1324             }
1325             else
1326             {
1327                 if( numGoodPoints > 7 )
1328                 {
1329                     /* Found >= 8 point. Improove matrix */
1330
1331                     CvMat wbestPnts1;
1332                     CvMat wbestPnts2;
1333
1334                     cvGetSubArr( bestPoints1, &wbestPnts1, cvRect(0,0,numGoodPoints,3) );
1335                     cvGetSubArr( bestPoints2, &wbestPnts2, cvRect(0,0,numGoodPoints,3) );
1336
1337                     /* best points was collectet. Improve fundamental matrix */
1338                     /* Just use 8-point algorithm */
1339                     double impFundMatr_dat[9];
1340                     CvMat impFundMatr;
1341                     impFundMatr = cvMat(3,3,CV_64F,impFundMatr_dat);
1342                     numFundMatr = icvComputeFundamental8Point(&wbestPnts1,&wbestPnts2,&impFundMatr);
1343
1344                     cvConvert(&impFundMatr,fundMatr);
1345                 }
1346                 else
1347                 {
1348                     /* 7 point. Just copy to result */
1349                     cvConvert(&bestFund,fundMatr);
1350                     numFundMatr = 1;
1351                 }
1352             }
1353
1354         }
1355     }
1356
1357     __END__;
1358
1359     /* free allocated memory */
1360
1361     cvReleaseMat(&corrLines1);
1362     cvReleaseMat(&corrLines2);
1363     cvReleaseMat(&wpoints1);
1364     cvReleaseMat(&wpoints2);
1365     cvReleaseMat(&bestPoints1);
1366     cvReleaseMat(&bestPoints2);
1367     cvReleaseMat(&dists1);
1368     cvReleaseMat(&dists2);
1369     cvReleaseMat(&distsSq1);
1370     cvReleaseMat(&distsSq2);
1371     cvReleaseMat(&allDists);
1372     cvFree((void**)&flags);
1373     cvFree((void**)&bestFlags);
1374
1375     return numFundMatr;
1376 }//icvComputeFundamentalLMedS
1377
1378 /*=====================================================================================*/
1379
1380
1381
1382 static void icvMakeFundamentalSingular(CvMat* fundMatr)
1383 {
1384     CV_FUNCNAME( "icvFundSingular" );
1385     __BEGIN__;
1386
1387     if( !CV_IS_MAT(fundMatr) )
1388     {
1389         CV_ERROR(CV_StsBadPoint,"Input data is not matrix");
1390     }
1391
1392     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
1393     {
1394         CV_ERROR( CV_StsBadSize, "Size of fundametal matrix must be 3x3" );
1395     }
1396
1397
1398     {/* Apply singularity condition */
1399         CvMat matrFU;
1400         CvMat matrFW;
1401         CvMat matrFVt;
1402         CvMat tmpMatr;
1403         CvMat preFundMatr;
1404         double matrFU_dat[9];
1405         double matrFW_dat[9];
1406         double matrFVt_dat[9];
1407         double tmpMatr_dat[9];
1408         double preFundMatr_dat[9];
1409
1410
1411         matrFU  = cvMat(3,3,CV_64F,matrFU_dat);
1412         matrFW  = cvMat(3,3,CV_64F,matrFW_dat);
1413         matrFVt = cvMat(3,3,CV_64F,matrFVt_dat);
1414         tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
1415         preFundMatr = cvMat(3,3,CV_64F,preFundMatr_dat);
1416
1417         cvConvert(fundMatr,&preFundMatr);
1418         cvSVD( &preFundMatr, &matrFW, &matrFU, &matrFVt, CV_SVD_V_T );
1419         cvmSet(&matrFW,2,2,0);
1420         /* multiply U*W*V' */
1421
1422         cvMatMul(&matrFU,&matrFW,&tmpMatr);
1423         cvMatMul(&tmpMatr,&matrFVt,&preFundMatr);
1424         cvConvert(&preFundMatr,fundMatr);
1425     }
1426
1427
1428     __END__;
1429 }
1430
1431
1432 /*=====================================================================================*/
1433 /* Normalize points for computing fundamental matrix */
1434 /* and compute transform matrix */
1435 /* Points:  2xN  */
1436 /* Matrix:  3x3 */
1437 /* place centroid of points to (0,0) */
1438 /* set mean distance from (0,0) by sqrt(2) */
1439
1440 static void icvNormalizeFundPoints( CvMat* points, CvMat* transfMatr )
1441 {
1442     CvMat* subwpointsx = 0;
1443     CvMat* subwpointsy = 0;
1444     CvMat* sqdists     = 0;
1445     CvMat* pointsxx    = 0;
1446     CvMat* pointsyy    = 0;
1447
1448     int numPoint;
1449     double shiftx,shifty;
1450     double meand;
1451     double scale;
1452
1453     CvMat tmpwpointsx;
1454     CvMat tmpwpointsy;
1455
1456     CvScalar sumx;
1457     CvScalar sumy;
1458
1459     CV_FUNCNAME( "icvNormalizeFundPoints" );
1460     __BEGIN__;
1461
1462     /* Test for correct input data */
1463
1464     if( !CV_IS_MAT(points) || !CV_IS_MAT(transfMatr) )
1465     {
1466         CV_ERROR(CV_StsBadPoint,"Input data is not matrixes");
1467     }
1468
1469     numPoint = points->cols;
1470
1471     if( numPoint < 1 )
1472     {
1473         CV_ERROR( CV_StsBadSize, "Number of points must be at least 1" );
1474     }
1475
1476     if( points->rows != 2 )
1477     {
1478         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2" );
1479     }
1480
1481     if( transfMatr->rows != 3 || transfMatr->cols != 3 )
1482     {
1483         CV_ERROR( CV_StsBadSize, "Size of transform matrix must be 3x3" );
1484     }
1485
1486     CV_CALL( subwpointsx  =  cvCreateMat(1,numPoint,CV_64F) );
1487     CV_CALL( subwpointsy  =  cvCreateMat(1,numPoint,CV_64F) );
1488     CV_CALL( sqdists      =  cvCreateMat(1,numPoint,CV_64F) );
1489     CV_CALL( pointsxx     =  cvCreateMat(1,numPoint,CV_64F) );
1490     CV_CALL( pointsyy     =  cvCreateMat(1,numPoint,CV_64F) );
1491
1492     /* get x,y coordinates of points */
1493
1494     {
1495         cvGetRow( points, &tmpwpointsx, 0 );
1496         cvGetRow( points, &tmpwpointsy, 1 );
1497
1498         /* Copy to working data 64F */
1499         cvConvert(&tmpwpointsx,subwpointsx);
1500         cvConvert(&tmpwpointsy,subwpointsy);
1501     }
1502
1503     /* Compute center of points */
1504
1505     sumx = cvSum(subwpointsx);
1506     sumy = cvSum(subwpointsy);
1507
1508     sumx.val[0] /= (double)numPoint;
1509     sumy.val[0] /= (double)numPoint;
1510
1511     shiftx = sumx.val[0];
1512     shifty = sumy.val[0];
1513
1514     /* Shift points center to 0 */
1515
1516     cvSubS( subwpointsx, sumx, subwpointsx);
1517     cvSubS( subwpointsy, sumy, subwpointsy);
1518
1519     /* Compute x*x and y*y */
1520
1521     cvMul(subwpointsx,subwpointsx,pointsxx);
1522     cvMul(subwpointsy,subwpointsy,pointsyy);
1523
1524     /* add  */
1525     cvAdd( pointsxx, pointsyy, sqdists);
1526
1527     /* compute sqrt of each component*/
1528
1529     cvPow(sqdists,sqdists,0.5);
1530
1531     /* in vector sqdists we have distances */
1532     /* compute mean value and scale */
1533
1534     meand = cvAvg(sqdists).val[0];
1535
1536     if( fabs(meand) > 1e-8  )
1537     {
1538         scale = 0.70710678118654752440084436210485/meand;
1539     }
1540     else
1541     {
1542         scale = 1.0;
1543     }
1544
1545     /* scale working points */
1546     cvScale(subwpointsx,subwpointsx,scale);
1547     cvScale(subwpointsy,subwpointsy,scale);
1548
1549     /* copy output data */
1550     {
1551         cvGetRow( points, &tmpwpointsx, 0 );
1552         cvGetRow( points, &tmpwpointsy, 1 );
1553
1554         /* Copy to output data 64F */
1555         cvConvert(subwpointsx,&tmpwpointsx);
1556         cvConvert(subwpointsy,&tmpwpointsy);
1557     }
1558
1559     /* Set transform matrix */
1560
1561     cvmSet(transfMatr,0,0, scale);
1562     cvmSet(transfMatr,0,1, 0);
1563     cvmSet(transfMatr,0,2, -scale*shiftx);
1564
1565     cvmSet(transfMatr,1,0, 0);
1566     cvmSet(transfMatr,1,1, scale);
1567     cvmSet(transfMatr,1,2, -scale*shifty);
1568
1569     cvmSet(transfMatr,2,0, 0);
1570     cvmSet(transfMatr,2,1, 0);
1571     cvmSet(transfMatr,2,2, 1);
1572
1573     __END__;
1574
1575     /* Free data */
1576     cvReleaseMat(&subwpointsx);
1577     cvReleaseMat(&subwpointsy);
1578     cvReleaseMat(&sqdists);
1579     cvReleaseMat(&pointsxx);
1580     cvReleaseMat(&pointsyy);
1581
1582 }
1583
1584 /*=====================================================================================*/
1585 // Solve cubic equation and returns number of roots
1586 // Also returns 0 if all values are possible
1587 // Test for very big coefficients
1588 // Input params 1x3 or 1x4
1589 CV_IMPL int cvSolveCubic(CvMat* coeffs,CvMat* result)
1590 {/* solve a*x^3 + b+x^2 + c*x + d = 0 */
1591     /* coeffs a,b,c,d or b,c,d if a== 1*/
1592     /* test input params */
1593     /* result 2x3  */
1594
1595     int numRoots = 0;
1596
1597     CV_FUNCNAME( "icvSolveCubic" );
1598     __BEGIN__;
1599
1600     /* Test correct of input data */
1601
1602     if( !CV_IS_MAT(coeffs) || !CV_IS_MAT(result) )
1603     {
1604         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
1605     }
1606
1607     if( !(coeffs->rows == 1 && (coeffs->cols == 3 || coeffs->cols == 4) ))
1608     {
1609         CV_ERROR( CV_StsBadSize, "Number of coeffs must be 3 or 4" );
1610     }
1611
1612
1613     double squares[6];
1614     double cc[4];
1615     cc[0] = cvmGet(coeffs,0,0);
1616     cc[1] = cvmGet(coeffs,0,1);
1617     cc[2] = cvmGet(coeffs,0,2);
1618
1619     if( fabs(cc[0]) > FLT_MAX || fabs(cc[1]) > FLT_MAX || fabs(cc[2]) > FLT_MAX )
1620     {
1621         return 0;//Coeffs too big
1622     }
1623
1624     double a0,a1,a2;
1625     if( coeffs->cols == 3 )
1626     {
1627         a0 = cc[0];
1628         a1 = cc[1];
1629         a2 = cc[2];
1630         numRoots = icvCubicV(a0,a1,a2,squares);
1631     }
1632     else
1633     {// We have for coeffs
1634         /* Test for very big coeffs */
1635         cc[3] = cvmGet(coeffs,0,3);
1636
1637         if( fabs(cc[3]) > FLT_MAX )
1638         {
1639             return 0;//Coeffs too big
1640         }
1641
1642         double a = cc[0];
1643         if( fabs(a) > FLT_MIN)
1644         {
1645             a = 1. / a;
1646             a0 = cc[1] * a;
1647             a1 = cc[2] * a;
1648             a2 = cc[3] * a;
1649             numRoots = icvCubicV(a0,a1,a2,squares);
1650         }
1651         else
1652         {// It's a square eqaution.
1653             double a,b,c;
1654             a = cc[1];
1655             b = cc[2];
1656             c = cc[3];
1657             if( fabs(a) > 1e-8 )
1658             {
1659                 double D;
1660                 D = b*b-4*a*c;
1661                 if( D > FLT_MIN )
1662                 {// Two roots
1663                     numRoots = 2;
1664                     squares[0] = (-b + sqrt(D))/(2*a);
1665                     squares[1] = 0;
1666                     squares[2] = (-b - sqrt(D))/(2*a);
1667                     squares[3] = 0;
1668                 }
1669                 else
1670                 {
1671                     if( D < FLT_MIN  )
1672                     {/* Two Im values */
1673                         numRoots = 2;
1674                         squares[0] = (-b)/(2*a);
1675                         squares[1] = (  sqrt(-D))/(2*a);
1676
1677                         squares[2] = (-b)/(2*a);
1678                         squares[3] = ( -sqrt(-D))/(2*a);
1679                     }
1680                     else
1681                     {/* D==0 */
1682                         numRoots = 2;
1683                         squares[0] = (-b)/(2*a);
1684                         squares[1] = 0;
1685                         squares[2] = (-b)/(2*a);
1686                         squares[3] = 0;
1687                     }
1688                 }
1689             }
1690             else
1691             {// Linear equation
1692                 if( fabs(b) > FLT_MIN )
1693                 {
1694                     squares[0] = -c/b;
1695                     squares[1] = 0;
1696                     numRoots = 1;
1697                 }
1698                 else
1699                 {
1700                     if( fabs(c) > FLT_MIN)
1701                     {
1702                         numRoots = 0;
1703                     }
1704                     else
1705                     {
1706                         /* All values are posible */
1707                         numRoots = 0;// !!!
1708                         //cvmSet(result,0,0,0);
1709                         //cvmSet(result,1,0,0);
1710                     }
1711                 }
1712             }
1713         }
1714     }
1715
1716     /* copy result  */
1717     int i;
1718
1719     for( i=0;i<numRoots;i++ )
1720     {
1721         cvmSet(result,0,i,squares[i*2]);
1722         cvmSet(result,1,i,squares[i*2+1]);
1723     }
1724     __END__;
1725
1726     return numRoots;
1727 }
1728
1729 /*=====================================================================================*/
1730 void cvMake2DPoints(CvMat* srcPoint,CvMat* dstPoint)
1731 {
1732     icvMake2DPoints(srcPoint,dstPoint);
1733     return;
1734 }
1735
1736 /*
1737   Convert 2D or 3D points to 2D points
1738
1739   for 3D: x = x/z;
1740           y = y/z
1741   for 2D just copy and maybe convert type
1742
1743   Source and destiantion may be the same and in this case src must be 2D
1744 */
1745 static void icvMake2DPoints(CvMat* srcPoint,CvMat* dstPoint)
1746 {
1747     CvMat* submatx = 0;
1748     CvMat* submaty = 0;
1749     CvMat* submatz = 0;
1750
1751     CV_FUNCNAME( "icvMake2DPoints" );
1752     __BEGIN__;
1753
1754     if( !CV_IS_MAT(srcPoint) || !CV_IS_MAT(dstPoint) )
1755     {
1756         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
1757     }
1758
1759     int numPoint;
1760     numPoint = srcPoint->cols;
1761
1762     if( numPoint != dstPoint->cols )
1763     {
1764         CV_ERROR( CV_StsBadSize, "Number of points not equal" );
1765     }
1766     if( numPoint < 1 )
1767     {
1768         CV_ERROR( CV_StsBadSize, "Number of points must > 0" );
1769     }
1770
1771     if( srcPoint->rows > 3 || srcPoint->rows < 2 )
1772     {
1773         CV_ERROR( CV_StsBadSize, "Number of coordinates of srcPoint must be 2 or 3" );
1774     }
1775
1776     if( dstPoint->rows != 2 )
1777     {
1778         CV_ERROR( CV_StsBadSize, "Number of coordinates of dstPoint must be 2" );
1779     }
1780
1781     CV_CALL( submatx = cvCreateMat(1,numPoint,CV_64F) );
1782     CV_CALL( submaty = cvCreateMat(1,numPoint,CV_64F) );
1783     CV_CALL( submatz = cvCreateMat(1,numPoint,CV_64F) );
1784
1785     CvMat subwpointsx;
1786     CvMat subwpointsy;
1787
1788     CvMat tmpSubmatx;
1789     CvMat tmpSubmaty;
1790     CvMat tmpSubmatz;
1791
1792     cvGetRow( dstPoint, &subwpointsx, 0 );
1793     cvGetRow( dstPoint, &subwpointsy, 1 );
1794
1795     cvGetRow( srcPoint, &tmpSubmatx, 0 );
1796     cvGetRow( srcPoint, &tmpSubmaty, 1 );
1797
1798     cvConvert(&tmpSubmatx,submatx);
1799     cvConvert(&tmpSubmaty,submaty);
1800
1801     if( srcPoint->rows == 3 )
1802     {
1803         cvGetRow( srcPoint, &tmpSubmatz, 2 );
1804         cvConvert(&tmpSubmatz,submatz);
1805
1806         cvDiv( submatx, submatz, &subwpointsx);
1807         cvDiv( submaty, submatz, &subwpointsy);
1808     }
1809     else
1810     {
1811         cvConvert(submatx,&subwpointsx);
1812         cvConvert(submaty,&subwpointsy);
1813     }
1814
1815     __END__;
1816
1817     cvReleaseMat(&submatx);
1818     cvReleaseMat(&submaty);
1819     cvReleaseMat(&submatz);
1820 }
1821
1822 /*=====================================================================================*/
1823
1824 void cvMake3DPoints(CvMat* srcPoint,CvMat* dstPoint)
1825 {
1826     icvMake3DPoints((const CvMat*)srcPoint,dstPoint);
1827     return;
1828 }
1829
1830
1831 /*
1832   Convert 2D or 3D points to 3D points
1833
1834   for 2D: x = x;
1835           y = y;
1836           z = 1;
1837
1838   for 3D: x = x;
1839           y = y;
1840           z = z;
1841
1842   Source and destiantion may be the same and in this case src must be 2D
1843 */
1844 static void icvMake3DPoints(const CvMat* srcPoint,CvMat* dstPoint)
1845 {
1846     CvMat* tmpSubmatz = 0;
1847
1848     CV_FUNCNAME( "icvMake3DPoints" );
1849     __BEGIN__;
1850
1851     if( !CV_IS_MAT(srcPoint) || !CV_IS_MAT(dstPoint) )
1852     {
1853         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
1854     }
1855
1856     int numPoint;
1857     numPoint = srcPoint->cols;
1858
1859     if( numPoint != dstPoint->cols )
1860     {
1861         CV_ERROR( CV_StsBadSize, "Number of points not equal" );
1862     }
1863     if( numPoint < 1 )
1864     {
1865         CV_ERROR( CV_StsBadSize, "Number of points must > 0" );
1866     }
1867
1868     if( srcPoint->rows > 3 || srcPoint->rows < 2 )
1869     {
1870         CV_ERROR( CV_StsBadSize, "Number of coordinates of srcPoint must be 2 or 3" );
1871     }
1872
1873     if( dstPoint->rows != 3 )
1874     {
1875         CV_ERROR( CV_StsBadSize, "Number of coordinates of dstPoint must be 3" );
1876     }
1877
1878     CV_CALL( tmpSubmatz = cvCreateMat(1,numPoint,CV_64F) );
1879
1880     if( srcPoint->rows == 3 )
1881     {
1882         /* Just copy all points */
1883         cvConvert(srcPoint,dstPoint);
1884     }
1885     else
1886     {
1887         CvMat subwpointsx;
1888         CvMat subwpointsy;
1889         CvMat subwpointsz;
1890
1891         cvGetRow( dstPoint, &subwpointsx, 0 );
1892         cvGetRow( dstPoint, &subwpointsy, 1 );
1893         cvGetRow( dstPoint, &subwpointsz, 2 );
1894
1895         CvMat tmpSubmatx;
1896         CvMat tmpSubmaty;
1897
1898         cvGetRow( srcPoint, &tmpSubmatx, 0 );
1899         cvGetRow( srcPoint, &tmpSubmaty, 1 );
1900
1901         cvConvert( &tmpSubmatx, &subwpointsx );
1902         cvConvert( &tmpSubmaty, &subwpointsy );
1903
1904         /* fill z by 1 */
1905         int i;
1906         for( i = 0; i < numPoint; i++ )
1907         {
1908             cvmSet(&subwpointsz,0,i,1.0);
1909         }
1910     }
1911
1912     __END__;
1913
1914     cvReleaseMat(&tmpSubmatz);
1915 }
1916
1917 /*=====================================================================================*/
1918 CV_IMPL void
1919 cvComputeCorrespondEpilines(const CvMat* points,int pointImageID,const CvMat* fundMatr,CvMat* corrLines)
1920 {
1921
1922     CvMat* wpoints = 0;
1923     CvMat* wcorrLines = 0;
1924
1925     pointImageID = 3-pointImageID;
1926
1927     CV_FUNCNAME( "icvComputeCorrespondEpilines" );
1928     __BEGIN__;
1929
1930     /* Test correct of input data */
1931
1932     if( !CV_IS_MAT(points) || !CV_IS_MAT(fundMatr)|| !CV_IS_MAT(corrLines))
1933     {
1934         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
1935     }
1936
1937     /*  */
1938
1939     int numPoint;
1940     numPoint = points->cols;
1941
1942     if( numPoint != corrLines->cols )
1943     {
1944         CV_ERROR( CV_StsBadSize, "Number of points and lines are not equal" );
1945     }
1946
1947     if( numPoint < 1 )
1948     {
1949         CV_ERROR( CV_StsBadSize, "Number of points must > 0" );
1950     }
1951
1952     if( points->rows != 2 && points->rows != 3 )
1953     {
1954         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
1955     }
1956
1957     if( corrLines->rows != 3 )
1958     {
1959         CV_ERROR( CV_StsBadSize, "Number of coordinates of corrLines must be 3" );
1960     }
1961
1962     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
1963     {
1964         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
1965     }
1966
1967     double wfundMatr_dat[9];
1968     CvMat wfundMatr;
1969     wfundMatr = cvMat(3,3,CV_64F,wfundMatr_dat);
1970     cvConvert(fundMatr,&wfundMatr);
1971
1972     if( pointImageID == 1 )
1973     {// get transformed fundamental matrix
1974         double tmpMatr_dat[9];
1975         CvMat tmpMatr;
1976         tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
1977         cvConvert(fundMatr,&tmpMatr);
1978         cvTranspose(&tmpMatr,&wfundMatr);
1979     }
1980     else if( pointImageID != 2 )
1981     {
1982         CV_ERROR( CV_StsBadArg, "Image ID must be 1 or 2" );
1983     }
1984     /* if wfundMatr we have good fundamental matrix */
1985     /* compute corr epi line for given points */
1986
1987     CV_CALL( wpoints = cvCreateMat(3,numPoint,CV_64F) );
1988     CV_CALL( wcorrLines = cvCreateMat(3,numPoint,CV_64F) );
1989
1990
1991     /* if points has 2 coordinates trandform them to 3D */
1992     icvMake3DPoints(points,wpoints);
1993
1994     cvMatMul(&wfundMatr,wpoints,wcorrLines);
1995
1996     /* normalise line coordinates */
1997     int i;
1998     for( i = 0; i < numPoint; i++ )
1999     {
2000         CvMat line;
2001         cvGetCol(wcorrLines,&line,i);
2002         double a,b;
2003         a = cvmGet(&line,0,0);
2004         b = cvmGet(&line,1,0);
2005         double nv;
2006         nv = sqrt(a*a+b*b);
2007         cvConvertScale(&line,&line,1.0 / nv);
2008     }
2009     cvConvert(wcorrLines,corrLines);
2010
2011
2012     __END__;
2013
2014     cvReleaseMat(&wpoints);
2015     cvReleaseMat(&wcorrLines);
2016
2017 }
2018
2019 /*=====================================================================================*/
2020
2021 #define SIGN(x) ( (x)<0 ? -1:((x)>0?1:0 ) )
2022 //#define REAL_ZERO(x) ( (x) < EPSILON && (x) > -EPSILON)
2023 #define REAL_ZERO(x) ( (x) < 1e-8 && (x) > -1e-8)
2024
2025 /* function return squares for cubic equation. 6 params - two for each square (Re,Im) */
2026 static int
2027 icvCubicV( double a2, double a1, double a0, double *squares )
2028 {
2029     double p, q, D, c1, c2, b1, b2, ro1, ro2, fi1, fi2;
2030     double x[6][3];
2031     int i, j, t;
2032
2033     if( !squares )
2034         return CV_BADFACTOR_ERR;
2035
2036     if( fabs(a0) > FLT_MAX || fabs(a1) > FLT_MAX || fabs(a2) > FLT_MAX )
2037     {
2038         return 0;//Coeffs too big
2039     }
2040
2041
2042     p = a1 - a2 * a2 / 3;
2043     q = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 27;
2044     D = q * q / 4 + p * p * p / 27;
2045
2046     if( fabs(p) > FLT_MAX || fabs(q) > FLT_MAX || fabs(D) > FLT_MAX )
2047     {
2048         return 0;//Coeffs too big
2049     }
2050
2051     if( D < 0 )
2052     {
2053
2054         c1 = q / 2;
2055         c2 = c1;
2056         b1 = sqrt( -D );
2057         b2 = -b1;
2058
2059         ro1 = sqrt( c1 * c1 - D );
2060         ro2 = ro1;
2061
2062         fi1 = atan2( b1, c1 );
2063         fi2 = -fi1;
2064     }
2065     else
2066     {
2067
2068         c1 = q / 2 + sqrt( D );
2069         c2 = q / 2 - sqrt( D );
2070         b1 = 0;
2071         b2 = 0;
2072
2073         ro1 = fabs( c1 );
2074         ro2 = fabs( c2 );
2075         fi1 = CV_PI * (1 - SIGN( c1 )) / 2;
2076         fi2 = CV_PI * (1 - SIGN( c2 )) / 2;
2077     }                           /* if */
2078
2079     for( i = 0; i < 6; i++ )
2080     {
2081
2082         x[i][0] = -a2 / 3;
2083         x[i][1] = 0;
2084         x[i][2] = 0;
2085
2086         squares[i] = x[i][i % 2];
2087     }                           /* for */
2088
2089     if( !REAL_ZERO( ro1 ))
2090     {
2091         c1 = SIGN( ro1 ) * pow( fabs( ro1 ), 1. / 3 );
2092         c1 -= SIGN( ro1 ) * p / 3. * pow( fabs( ro1 ), -1. / 3 );
2093
2094         c2 = SIGN( ro1 ) * pow( fabs( ro1 ), 1. / 3 );
2095         c2 += SIGN( ro1 ) * p / 3. * pow( fabs( ro1 ), -1. / 3 );
2096     }                           /* if */
2097
2098     if( !REAL_ZERO( ro2 ))
2099     {
2100         b1 = SIGN( ro2 ) * pow( fabs( ro2 ), 1. / 3 );
2101         b1 -= SIGN( ro2 ) * p / 3. * pow( fabs( ro2 ), -1. / 3 );
2102
2103         b2 = SIGN( ro2 ) * pow( fabs( ro2 ), 1. / 3 );
2104         b2 += SIGN( ro2 ) * p / 3. * pow( fabs( ro2 ), -1. / 3 );
2105     }
2106
2107     for( i = 0; i < 6; i++ )
2108     {
2109
2110         if( i < 3 )
2111         {
2112
2113             if( !REAL_ZERO( ro1 ))
2114             {
2115
2116                 x[i][0] = cos( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c1 - a2 / 3;
2117                 x[i][1] = sin( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c2;
2118             }
2119             else
2120             {
2121
2122                 //x[i][2] = 1;!!!
2123             }                   /* if */
2124         }
2125         else
2126         {
2127
2128             if( !REAL_ZERO( ro2 ))
2129             {
2130
2131                 x[i][0] = cos( fi2 / 3. + 2 * CV_PI * (i % 3) / 3. ) * b1 - a2 / 3;
2132                 x[i][1] = sin( fi2 / 3. + 2 * CV_PI * (i % 3) / 3. ) * b2;
2133             }
2134             else
2135             {
2136
2137                 //x[i][2] = 1;!!!
2138             }                   /* if */
2139         }                       /* if */
2140     }                           /* for */
2141
2142     t = 0;
2143
2144     //int numRoots = 6;
2145     for( i = 0; i < 6 && t < 6; i++ )
2146     {
2147
2148         if( !x[i][2] )
2149         {
2150
2151             squares[t++] = x[i][0];
2152             squares[t++] = x[i][1];
2153             x[i][2] = 1;
2154
2155             for( j = i + 1; j < 6; j++ )
2156             {/* delete equal root from rest */
2157
2158                 if( !x[j][2] && REAL_ZERO( x[i][0] - x[j][0] )
2159                     && REAL_ZERO( x[i][1] - x[j][1] ))
2160                 {
2161
2162                     x[j][2] = 1;
2163                     break;
2164                 }               /* if */
2165             }                   /* for */
2166         }                       /* if */
2167     }                           /* for */
2168     return 3;
2169 }                               /* icvCubic */
2170
2171 /*=====================================================================================*/
2172