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 #include "_cvvm.h"
  43
  44
  45 /*=====================================================================================*/
  46 /* new version of fundamental matrix functions */
  47 /*=====================================================================================*/
  48
  49 int icvComputeFundamental7Point(CvMat* points1,CvMat* points2,
  50                                      CvMat* fundMatr);
  51
  52 int icvComputeFundamental8Point(CvMat* points1,CvMat* points2,
  53                                      CvMat* fundMatr);
  54
  55
  56 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 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 void icvMakeFundamentalSingular(CvMat* fundMatr);
  69
  70 void icvNormalizeFundPoints(    CvMat* points,
  71                                                                 CvMat* transfMatr);
  72
  73 int icvSolveCubic(CvMat* coeffs,CvMat* result);
  74
  75 void icvMake2DPoints(CvMat* srcPoint,CvMat* dstPoint);
  76
  77 void icvMake3DPoints(CvMat* srcPoint,CvMat* dstPoint);
  78
  79 void icvComputeCorrespondEpilines(CvMat* points,int pointImageID,CvMat* fundMatr,CvMat* corrLines);
  80
  81 int icvCubicV( double a2, double a1, double a0, double *squares );
  82
  83 /*=====================================================================================*/
  84
  85 /*F///////////////////////////////////////////////////////////////////////////////////////
  86 //    Name:    cvFindFundamentalMat
  87 //    Purpose: find fundamental matrix for given points using
  88 //    Context:
  89 //    Parameters:
  90 //      points1  - points on first image. Size of matrix 2xN or 3xN
  91 //      points2  - points on second image Size of matrix 2xN or 3xN
  92 //      fundMatr - found fundamental matrix (or matrixes for 7-point). Size 3x3 or 9x3
  93 //      method   - method for computing fundamental matrix
  94 //                 CV_FM_7POINT - for 7-point algorithm. Number of points == 7
  95 //                 CV_FM_8POINT - for 8-point algorithm. Number of points >= 8
  96 //                 CV_FM_RANSAC - for RANSAC  algorithm. Number of points >= 8
  97 //                 CV_FM_LMEDS  - for LMedS   algorithm. Number of points >= 8
  98 //      param1 and param2 uses for RANSAC method
  99 //        param1 - threshold distance from point to epipolar line.
 100 //                 If distance less than threshold point is good.
 101 //        param2 - probability. Usually = 0.99
 102 //        status - array, every element of which will be set to 1 if the point was good
 103 //                 and used for computation. 0 else. (for RANSAC and LMedS only)
 104 //                 (it is optional parameter, can be NULL)
 105 //
 106 //    Returns:
 107 //      number of found matrixes
 108 //F*/
 109 int cvFindFundamentalMat(   CvMat* points1,
 110                                                         CvMat* points2,
 111                                                         CvMat* fundMatr,
 112                                                         int    method,
 113                                                         double param1,
 114                                                         double param2,
 115                             CvMat* status)
 116 {
 117     int result = -1;
 118
 119     CvMat* wpoints[2]={0,0};
 120     CvMat* tmpPoints[2]={0,0};
 121
 122     CV_FUNCNAME( "icvComputeFundamentalMat" );
 123     __BEGIN__;
 124
 125     tmpPoints[0] = points1;
 126     tmpPoints[1] = points2;
 127     int numRealPoints[2];
 128     int numPoints = 0;
 129
 130     /* work with points */
 131     {
 132         int i;
 133         for( i = 0; i < 2; i++ )
 134         {
 135             int realW,realH;
 136             realW = tmpPoints[i]->cols;
 137             realH = tmpPoints[i]->rows;
 138
 139             int goodW,goodH;
 140             goodW = realW > realH ? realW : realH;
 141             goodH = realW < realH ? realW : realH;
 142
 143             if( goodH != 2 && goodH != 3 )
 144             {
 145                 CV_ERROR(CV_StsBadPoint,"Number of coordinates of points must be 2 or 3");
 146             }
 147
 148             wpoints[i] = cvCreateMat(2,goodW,CV_64F);
 149
 150             numRealPoints[i] = goodW;
 151
 152             /* Test for transpose point matrix */
 153             if( realW != goodW )
 154             {/* need to transpose point matrix */
 155                 CvMat* tmpPointMatr = 0;
 156                 tmpPointMatr = cvCreateMat(goodH,goodW,CV_64F);
 157                 cvTranspose(tmpPoints[i],tmpPointMatr);
 158                 cvMake2DPoints(tmpPointMatr,wpoints[i]);
 159                 cvReleaseMat(&tmpPointMatr);
 160             }
 161             else
 162             {
 163                 cvMake2DPoints(tmpPoints[i],wpoints[i]);
 164             }
 165
 166         }
 167
 168         if( numRealPoints[0] != numRealPoints[1] )
 169         {
 170             CV_ERROR(CV_StsBadPoint,"Number of points must be the same");
 171         }
 172
 173         numPoints = numRealPoints[0];
 174     }
 175
 176     /* work with status if use functions which don't compute it */
 177     if( status && (method == CV_FM_7POINT || method == CV_FM_8POINT ))
 178     {
 179
 180         if( !CV_IS_MAT(status) )
 181         {
 182             CV_ERROR(CV_StsBadPoint,"status is not a matrix");
 183         }
 184
 185
 186         if( !CV_IS_MAT(points1))
 187         {
 188             CV_ERROR(CV_StsBadPoint,"Points1 not a matrix");
 189         }
 190
 191         if( status->cols != numPoints || status->rows != 1 )
 192         {
 193             CV_ERROR(CV_StsBadPoint,"Size of status must be 1xN");
 194         }
 195
 196         int i;
 197         for( i = 0; i < status->cols; i++)
 198         {
 199             cvmSet(status,0,i,1.0);
 200         }
 201
 202     }
 203
 204
 205         switch( method )
 206         {
 207                 case CV_FM_7POINT: result = icvComputeFundamental7Point(wpoints[1], wpoints[0], fundMatr);break;
 208
 209                 case CV_FM_8POINT: result = icvComputeFundamental8Point(wpoints[1],wpoints[0], fundMatr);break;
 210
 211                 case CV_FM_LMEDS : result = icvComputeFundamentalLMedS(   wpoints[1],wpoints[0], fundMatr,
 212                                         param1,param2,status);break;
 213
 214                 case CV_FM_RANSAC: result = icvComputeFundamentalRANSAC(   wpoints[1],wpoints[0], fundMatr,
 215                                         param1,param2,status);break;
 216
 217                 //default:return -1/*ERROR*/;
 218         }
 219
 220     __END__;
 221
 222     cvReleaseMat(&wpoints[0]);
 223     cvReleaseMat(&wpoints[1]);
 224
 225     return result;
 226 }
 227
 228 /*=====================================================================================*/
 229
 230 /* Computes 1 or 3 fundamental matrixes using 7-point algorithm */
 231 int icvComputeFundamental7Point(CvMat* points1, CvMat* points2, CvMat* fundMatr)
 232 {/* !!! Use just first real square of cubic equation */
 233
 234     CvMat* squares = 0;
 235     int numberRoots = 0;
 236
 237     CV_FUNCNAME( "icvComputeFundamental7Point" );
 238     __BEGIN__;
 239
 240     /* Test correct of input data */
 241
 242     if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
 243     {
 244         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
 245     }
 246
 247     if( !CV_ARE_TYPES_EQ( points1, points2 ))
 248     {
 249         CV_ERROR( CV_StsUnmatchedSizes, "Data types of points unmatched" );
 250     }
 251
 252     int numPoint;
 253     numPoint = points1->cols;
 254
 255     int type;
 256     type = points1->type;
 257
 258     if( numPoint != points2->cols )
 259     {
 260         CV_ERROR( CV_StsBadSize, "Number of points not equal" );
 261     }
 262     if( numPoint != 7 )
 263     {
 264         CV_ERROR( CV_StsBadSize, "Number of points must be 7" );
 265     }
 266
 267     if( points1->rows != 2 && points1->rows != 3 )
 268     {
 269         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
 270     }
 271
 272     if( points2->rows != 2 && points2->rows != 3 )
 273     {
 274         CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
 275     }
 276
 277     if( ( fundMatr->rows != 3 && fundMatr->rows != 9 )|| fundMatr->cols != 3 )
 278     {
 279         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3 or 9x3" );
 280     }
 281
 282     squares = cvCreateMat(2,3,CV_64F);
 283
 284
 285     /* Make it Normalize points if need */
 286     double wPoints1_dat[2*7];
 287     double wPoints2_dat[2*7];
 288     CvMat wPoints1;
 289     CvMat wPoints2;
 290     wPoints1 = cvMat(2,7,CV_64F,wPoints1_dat);
 291     wPoints2 = cvMat(2,7,CV_64F,wPoints2_dat);
 292
 293     icvMake2DPoints(points1,&wPoints1);
 294     icvMake2DPoints(points2,&wPoints2);
 295
 296     /* fill matrix U */
 297
 298     int currPoint;
 299     CvMat matrU;
 300     double matrU_dat[7*9];
 301     matrU = cvMat(7,9,CV_64F,matrU_dat);
 302
 303     double* currDataLine;
 304     currDataLine = matrU_dat;
 305     for( currPoint = 0; currPoint < 7; currPoint++ )
 306     {
 307         double x1,y1,x2,y2;
 308         x1 = cvmGet(&wPoints1,0,currPoint);
 309         y1 = cvmGet(&wPoints1,1,currPoint);
 310         x2 = cvmGet(&wPoints2,0,currPoint);
 311         y2 = cvmGet(&wPoints2,1,currPoint);
 312
 313         currDataLine[0] = x1*x2;
 314         currDataLine[1] = x1*y2;
 315         currDataLine[2] = x1;
 316         currDataLine[3] = y1*x2;
 317         currDataLine[4] = y1*y2;
 318         currDataLine[5] = y1;
 319         currDataLine[6] = x2;
 320         currDataLine[7] = y2;
 321         currDataLine[8] = 1;
 322
 323         currDataLine += 9;
 324     }
 325
 326     CvMat matrUU;
 327     CvMat matrSS;
 328     CvMat matrVV;
 329     double matrUU_dat[7*7];
 330     double matrSS_dat[7*9];
 331     double matrVV_dat[9*9];
 332     matrUU = cvMat(7,7,CV_64F,matrUU_dat);
 333     matrSS = cvMat(7,9,CV_64F,matrSS_dat);
 334     matrVV = cvMat(9,9,CV_64F,matrVV_dat);
 335
 336     cvSVD( &matrU, &matrSS, &matrUU, &matrVV, 0/*CV_SVD_V_T*/ );/* get transposed matrix V */
 337
 338         double F111,F112,F113;
 339         double F121,F122,F123;
 340         double F131,F132,F133;
 341
 342         double F211,F212,F213;
 343         double F221,F222,F223;
 344         double F231,F232,F233;
 345
 346         F111=cvmGet(&matrVV,0,7);
 347     F112=cvmGet(&matrVV,1,7);
 348     F113=cvmGet(&matrVV,2,7);
 349         F121=cvmGet(&matrVV,3,7);
 350     F122=cvmGet(&matrVV,4,7);
 351     F123=cvmGet(&matrVV,5,7);
 352         F131=cvmGet(&matrVV,6,7);
 353     F132=cvmGet(&matrVV,7,7);
 354     F133=cvmGet(&matrVV,8,7);
 355
 356         F211=cvmGet(&matrVV,0,8);
 357     F212=cvmGet(&matrVV,1,8);
 358     F213=cvmGet(&matrVV,2,8);
 359         F221=cvmGet(&matrVV,3,8);
 360     F222=cvmGet(&matrVV,4,8);
 361     F223=cvmGet(&matrVV,5,8);
 362         F231=cvmGet(&matrVV,6,8);
 363     F232=cvmGet(&matrVV,7,8);
 364     F233=cvmGet(&matrVV,8,8);
 365
 366     double a,b,c,d;
 367
 368         a =   F231*F112*F223 + F231*F212*F123 - F231*F212*F223 + F231*F113*F122 -
 369           F231*F113*F222 - F231*F213*F122 + F231*F213*F222 - F131*F112*F223 -
 370           F131*F212*F123 + F131*F212*F223 - F131*F113*F122 + F131*F113*F222 +
 371           F131*F213*F122 - F131*F213*F222 + F121*F212*F133 - F121*F212*F233 +
 372           F121*F113*F132 - F121*F113*F232 - F121*F213*F132 + F121*F213*F232 +
 373           F221*F112*F133 - F221*F112*F233 - F221*F212*F133 + F221*F212*F233 -
 374           F221*F113*F132 + F221*F113*F232 + F221*F213*F132 - F221*F213*F232 +
 375           F121*F112*F233 - F111*F222*F133 + F111*F222*F233 - F111*F123*F132 +
 376           F111*F123*F232 + F111*F223*F132 - F111*F223*F232 - F211*F122*F133 +
 377           F211*F122*F233 + F211*F222*F133 - F211*F222*F233 + F211*F123*F132 -
 378           F211*F123*F232 - F211*F223*F132 + F211*F223*F232 + F111*F122*F133 -
 379           F111*F122*F233 - F121*F112*F133 + F131*F112*F123 - F231*F112*F123;
 380
 381         b =   2*F231*F213*F122 - 3*F231*F213*F222 + F231*F112*F123   - 2*F231*F112*F223 -
 382           2*F231*F212*F123 + 3*F231*F212*F223 - F231*F113*F122   + 2*F231*F113*F222 +
 383           F131*F212*F123   - 2*F131*F212*F223 - F131*F113*F222   - F131*F213*F122   +
 384           2*F131*F213*F222 + F121*F113*F232   + F121*F213*F132   - 2*F121*F213*F232 -
 385           F221*F112*F133   + 2*F221*F112*F233 + 2*F221*F212*F133 - 3*F221*F212*F233 +
 386           F221*F113*F132   - 2*F221*F113*F232 - 2*F221*F213*F132 + 3*F221*F213*F232 +
 387           F131*F112*F223   - 2*F211*F122*F233 - 2*F211*F222*F133 + 3*F211*F222*F233 -
 388           F211*F123*F132   + 2*F211*F123*F232 + 2*F211*F223*F132 - 3*F211*F223*F232 -
 389           F121*F112*F233   - F121*F212*F133   + 2*F121*F212*F233 - 2*F111*F222*F233 -
 390           F111*F123*F232   - F111*F223*F132   + 2*F111*F223*F232 + F111*F122*F233   +
 391           F111*F222*F133   + F211*F122*F133;
 392
 393         c =   F231*F112*F223   + F231*F212*F123   - 3*F231*F212*F223 - F231*F113*F222   -
 394           F231*F213*F122   + 3*F231*F213*F222 + F131*F212*F223   - F131*F213*F222   +
 395           F121*F213*F232   - F221*F112*F233   - F221*F212*F133   + 3*F221*F212*F233 +
 396           F221*F113*F232   + F221*F213*F132   - 3*F221*F213*F232 + F211*F122*F233   +
 397           F211*F222*F133   - 3*F211*F222*F233 - F211*F123*F232   - F211*F223*F132   +
 398           3*F211*F223*F232 - F121*F212*F233   + F111*F222*F233   - F111*F223*F232;
 399
 400         d =   F221*F213*F232 - F211*F223*F232 + F211*F222*F233 - F221*F212*F233 +
 401           F231*F212*F223 - F231*F213*F222;
 402
 403     /* find root */
 404     double coeffs_dat[4];
 405     CvMat coeffs;
 406     coeffs = cvMat(1,4,CV_64F,coeffs_dat);
 407
 408     cvmSet(&coeffs,0,0,a);
 409     cvmSet(&coeffs,0,1,b);
 410     cvmSet(&coeffs,0,2,c);
 411     cvmSet(&coeffs,0,3,d);
 412
 413     int numCubRoots;
 414     numCubRoots = icvSolveCubic(&coeffs,squares);
 415
 416     /* take real solution */
 417     /* !!! Need test whole 3 roots */
 418
 419     int i;
 420     for( i = 0; i < numCubRoots; i++ )
 421     {
 422         if( fabs(cvmGet(squares,1,i)) < 1e-8 )
 423         {//It is real square. (Im==0)
 424
 425             double sol;
 426             sol = cvmGet(squares,0,i);
 427             //F=sol*F1+(1-sol)*F2;
 428
 429             int t;
 430             for( t = 0; t < 9; t++ )
 431             {
 432                 double f1,f2;
 433                 f1 = cvmGet(&matrVV,t,7);
 434                 f2 = cvmGet(&matrVV,t,8);
 435
 436                 double s = f1 * sol + (1-sol) * f2;
 437                 cvmSet(fundMatr,numberRoots*3 + t/3,t%3,s);
 438             }
 439             numberRoots++;
 440
 441             if( fundMatr->rows == 3 )/* not enough space to store more than one root */
 442                 break;
 443         }
 444     }
 445
 446     /* scale fundamental matrix */
 447     for( i = 0; i < numberRoots; i++ )
 448     {
 449
 450         double fsc;
 451         fsc = cvmGet(fundMatr,i*3+2,2);
 452         if( fabs(fsc) > 1e-8 )
 453         {
 454             CvMat subFund;
 455             cvGetSubArr( fundMatr, &subFund, cvRect(0,i*3,3,3) );
 456             cvmScale(&subFund,&subFund,1.0/fsc);
 457         }
 458     }
 459
 460     __END__;
 461
 462     cvReleaseMat(&squares);
 463
 464
 465     return numberRoots;
 466 }
 467
 468
 469 /*=====================================================================================*/
 470
 471 int icvComputeFundamental8Point(CvMat* points1,CvMat* points2, CvMat* fundMatr)
 472 {
 473     CvMat* wpoints[2]={0,0};
 474     CvMat* preFundMatr = 0;
 475     CvMat* sqdists = 0;
 476     CvMat* matrA = 0;
 477     CvMat* matrU = 0;
 478     CvMat* matrW = 0;
 479     CvMat* matrV = 0;
 480
 481     int numFundMatrs = 0;
 482
 483     CV_FUNCNAME( "icvComputeFundamental8Point" );
 484     __BEGIN__;
 485
 486     /* Test correct of input data */
 487
 488     if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
 489     {
 490         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
 491     }
 492
 493     if( !CV_ARE_TYPES_EQ( points1, points2 ))
 494     {
 495         CV_ERROR( CV_StsUnmatchedSizes, "Data types of points unmatched" );
 496     }
 497
 498     int numPoint;
 499     numPoint = points1->cols;
 500
 501     int type;
 502     type = points1->type;
 503
 504     if( numPoint != points2->cols )
 505     {
 506         CV_ERROR( CV_StsBadSize, "Number of points not equal" );
 507     }
 508     if( numPoint < 8 )
 509     {
 510         CV_ERROR( CV_StsBadSize, "Number of points must be at least 8" );
 511     }
 512
 513     if( points1->rows > 3 || points1->rows < 2 )
 514     {
 515         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
 516     }
 517
 518     if( points2->rows > 3 || points2->rows < 2 )
 519     {
 520         CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
 521     }
 522
 523     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
 524     {
 525         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
 526     }
 527
 528     /* allocate data */
 529     CV_CALL( wpoints[0] = cvCreateMat(2,numPoint,CV_64F) );
 530     CV_CALL( wpoints[1] = cvCreateMat(2,numPoint,CV_64F) );
 531     CV_CALL( matrA = cvCreateMat(numPoint,9,CV_64F) );
 532     CV_CALL( preFundMatr = cvCreateMat(3,3,CV_64F) );
 533     CV_CALL( matrU = cvCreateMat(numPoint,numPoint,CV_64F) );
 534     CV_CALL( matrW = cvCreateMat(numPoint,9,CV_64F) );
 535     CV_CALL( matrV = cvCreateMat(9,9,CV_64F) );
 536     CV_CALL( sqdists = cvCreateMat(1,numPoint,CV_64F) );
 537
 538     /* Create working points with just x,y */
 539
 540     CvMat transfMatr[2];
 541     double transfMatr1_dat[9];
 542     double transfMatr2_dat[9];
 543     transfMatr[0] = cvMat(3,3,CV_64F,transfMatr1_dat);
 544     transfMatr[1] = cvMat(3,3,CV_64F,transfMatr2_dat);
 545
 546     {/* transform to x,y.  tranform point and compute transform matrixes */
 547         icvMake2DPoints(points1,wpoints[0]);
 548         icvMake2DPoints(points2,wpoints[1]);
 549
 550         icvNormalizeFundPoints( wpoints[0],&transfMatr[0]);
 551         icvNormalizeFundPoints( wpoints[1],&transfMatr[1]);
 552
 553         /* we have normalized working points wpoints[0] and wpoints[1] */
 554         /* build matrix A from points coordinates */
 555
 556         int currPoint;
 557         for( currPoint = 0; currPoint < numPoint; currPoint++ )
 558         {
 559             CvMat rowA;
 560             double x1,y1;
 561             double x2,y2;
 562
 563             x1 = cvmGet(wpoints[0],0,currPoint);
 564             y1 = cvmGet(wpoints[0],1,currPoint);
 565             x2 = cvmGet(wpoints[1],0,currPoint);
 566             y2 = cvmGet(wpoints[1],1,currPoint);
 567
 568             cvGetRow(matrA,&rowA,currPoint);
 569             rowA.data.db[0] = x1*x2;
 570             rowA.data.db[1] = x1*y2;
 571             rowA.data.db[2] = x1;
 572
 573             rowA.data.db[3] = y1*x2;
 574             rowA.data.db[4] = y1*y2;
 575             rowA.data.db[5] = y1;
 576
 577             rowA.data.db[6] = x2;
 578             rowA.data.db[7] = y2;
 579             rowA.data.db[8] = 1;
 580         }
 581     }
 582
 583     /* We have matrix A. Compute svd(A). We need last column of V */
 584
 585
 586     cvSVD( matrA, matrW, matrU, matrV, CV_SVD_V_T );/* get transposed matrix V */
 587
 588     /* Compute number of non zero eigen values */
 589     int numEig;
 590     numEig = 0;
 591     {
 592         int i;
 593         for( i = 0; i < 9; i++ )
 594         {
 595             if( cvmGet(matrW,i,i) < 1e-8 )
 596             {
 597                 break;
 598             }
 599         }
 600
 601         numEig = i;
 602
 603     }
 604
 605     if( numEig < 5 )
 606     {/* Bad points */
 607         numFundMatrs = 0;
 608     }
 609     else
 610     {
 611         numFundMatrs = 1;
 612
 613         /* copy last row of matrix V to precomputed fundamental matrix */
 614
 615         CvMat preF;
 616         cvGetRow(matrV,&preF,8);
 617         cvReshape(&preF,preFundMatr,1,3);
 618
 619         /* Apply singularity condition */
 620         icvMakeFundamentalSingular(preFundMatr);
 621
 622         /* Denormalize fundamental matrix */
 623         /* Copmute transformation for normalization */
 624
 625         CvMat wfundMatr;
 626         double wfundMatr_dat[9];
 627         wfundMatr = cvMat(3,3,CV_64F,wfundMatr_dat);
 628
 629         {/*  Freal = T1'*Fpre*T2 */
 630             double tmpMatr_dat[9];
 631             double tmptransfMatr_dat[9];
 632
 633             CvMat tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
 634             CvMat tmptransfMatr = cvMat(3,3,CV_64F,tmptransfMatr_dat);
 635
 636             cvTranspose(&transfMatr[0],&tmptransfMatr);
 637
 638             cvmMul(&tmptransfMatr,preFundMatr,&tmpMatr);
 639             cvmMul(&tmpMatr,&transfMatr[1],&wfundMatr);
 640         }
 641
 642         /* scale fundamental matrix */
 643         double fsc;
 644         fsc = cvmGet(&wfundMatr,2,2);
 645         if( fabs(fsc) > 1.0e-8 )
 646         {
 647             cvmScale(&wfundMatr,&wfundMatr,1.0/fsc);
 648         }
 649
 650         /* copy result fundamental matrix */
 651         cvConvert( &wfundMatr, fundMatr );
 652
 653     }
 654
 655
 656     __END__;
 657
 658     cvReleaseMat(&matrU);
 659     cvReleaseMat(&matrW);
 660     cvReleaseMat(&matrV);
 661     cvReleaseMat(&preFundMatr);
 662     cvReleaseMat(&matrA);
 663     cvReleaseMat(&wpoints[0]);
 664     cvReleaseMat(&wpoints[1]);
 665     cvReleaseMat(&sqdists);
 666
 667     return numFundMatrs;
 668 }
 669
 670 /*=====================================================================================*/
 671
 672 /* Computes fundamental matrix using RANSAC method */
 673 /*  */
 674 int icvComputeFundamentalRANSAC(   CvMat* points1,CvMat* points2,
 675                                         CvMat* fundMatr,
 676                                         double threshold,/* Threshold for good points */
 677                                                                                 double p,/* Probability of good result. */
 678                                         CvMat* status)
 679 {
 680     CvMat* wpoints1 = 0;
 681     CvMat* wpoints2 = 0;
 682     CvMat* corrLines1 = 0;
 683     CvMat* corrLines2 = 0;
 684     CvMat* bestPoints1 = 0;
 685     CvMat* bestPoints2 = 0;
 686
 687     int* flags = 0;
 688     int* bestFlags = 0;
 689     int numFundMatr = 0;
 690
 691     CV_FUNCNAME( "icvComputeFundamentalRANSAC" );
 692     __BEGIN__;
 693
 694     /* Test correct of input data */
 695
 696     if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
 697     {
 698         CV_ERROR(CV_StsBadPoint,"points1 or points2 or funMatr are not a matrixes");
 699     }
 700
 701     int numPoint;
 702     numPoint = points1->cols;
 703
 704     int type;
 705     type = points1->type;
 706
 707     if( numPoint != points2->cols )
 708     {
 709         CV_ERROR( CV_StsBadSize, "Number of points not equals" );
 710     }
 711     if( numPoint < 8 )
 712     {
 713         CV_ERROR( CV_StsBadSize, "Number of points must be >= 8" );
 714     }
 715
 716     if( points1->rows != 2 && points1->rows != 3 )
 717     {
 718         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
 719     }
 720
 721     if( points2->rows != 2 && points2->rows != 3 )
 722     {
 723         CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
 724     }
 725
 726     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
 727     {
 728         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
 729     }
 730
 731     if( status )
 732     {/* status is present test it */
 733         if( !CV_IS_MAT(status) )
 734         {
 735             CV_ERROR(CV_StsBadPoint,"status is not a matrix");
 736         }
 737
 738         if( status->cols != numPoint || status->rows != 1 )
 739         {
 740             CV_ERROR(CV_StsBadPoint,"Size of status must be 1xN");
 741         }
 742     }
 743
 744     /* We will choose adaptive number of N (samples) */
 745
 746     /* Convert points to 64F working points */
 747
 748     CV_CALL( flags = (int*)cvAlloc(numPoint * sizeof(int)) );
 749     CV_CALL( bestFlags = (int*)cvAlloc(numPoint * sizeof(int)) );
 750     CV_CALL( wpoints1 = cvCreateMat(3,numPoint,CV_64F) );
 751     CV_CALL( wpoints2 = cvCreateMat(3,numPoint,CV_64F) );
 752     CV_CALL( corrLines1 = cvCreateMat(3,numPoint,CV_64F));
 753     CV_CALL( corrLines2 = cvCreateMat(3,numPoint,CV_64F));
 754     CV_CALL( bestPoints1 = cvCreateMat(3,numPoint,CV_64F));
 755     CV_CALL( bestPoints2 = cvCreateMat(3,numPoint,CV_64F));
 756
 757     icvMake3DPoints(points1,wpoints1);
 758     icvMake3DPoints(points2,wpoints2);
 759
 760     srand(12345);
 761
 762     {
 763         int NumSamples = 1;//just init number of samples
 764         int wasCount = 0;  //count of choosing samples
 765         int maxGoodPoints = 0;
 766         int numGoodPoints = 0;
 767
 768         double bestFund_dat[9];
 769         CvMat  bestFund;
 770         bestFund = cvMat(3,3,CV_64F,bestFund_dat);
 771
 772         /* choosen points */
 773         while( NumSamples > wasCount )
 774         {
 775             /* select samples */
 776             int randNumbs[7];
 777             int i;
 778             int newnum;
 779             int pres;
 780             for( i = 0; i < 7; i++ )
 781             {
 782                 do
 783                 {
 784                     newnum = rand()%numPoint;
 785
 786                     /* test this number */
 787                     pres = 0;
 788                     int test;
 789                     for( test = 0; test < i; test++ )
 790                     {
 791                         if( randNumbs[test] == newnum )
 792                         {
 793                             pres = 1;
 794                             break;
 795                         }
 796                     }
 797                 }
 798                 while( pres );
 799                 randNumbs[i] = newnum;
 800             }
 801             /* random numbers of points was generated */
 802             /* select points */
 803
 804             double selPoints1_dat[2*7];
 805             double selPoints2_dat[2*7];
 806             CvMat selPoints1;
 807             CvMat selPoints2;
 808             selPoints1 = cvMat(2,7,CV_64F,selPoints1_dat);
 809             selPoints2 = cvMat(2,7,CV_64F,selPoints2_dat);
 810             /* copy points */
 811             int t;
 812             for( t = 0; t < 7; t++ )
 813             {
 814                 double x,y;
 815                 x = cvmGet(wpoints1,0,randNumbs[t]);
 816                 y = cvmGet(wpoints1,1,randNumbs[t]);
 817                 cvmSet(&selPoints1,0,t,x);
 818                 cvmSet(&selPoints1,1,t,y);
 819
 820                 x = cvmGet(wpoints2,0,randNumbs[t]);
 821                 y = cvmGet(wpoints2,1,randNumbs[t]);
 822                 cvmSet(&selPoints2,0,t,x);
 823                 cvmSet(&selPoints2,1,t,y);
 824             }
 825
 826             /* Copmute fundamental matrix using 7-points algorithm */
 827
 828             double fundTriple_dat[27];
 829             CvMat fundTriple;
 830             fundTriple = cvMat(9,3,CV_64F,fundTriple_dat);
 831
 832             int numFund = icvComputeFundamental7Point(&selPoints1,&selPoints2,&fundTriple);
 833
 834             //double fund7_dat[9];
 835             CvMat fund7;
 836             //fund7 = cvMat(3,3,CV_64F,fund7_dat);
 837
 838             /* get sub matrix */
 839             for( int currFund = 0; currFund < numFund; currFund++ )
 840             {
 841                 cvGetSubArr(&fundTriple,&fund7,cvRect(0,currFund*3,3,3));
 842                 {
 843                     /* Compute inliers for this fundamental matrix */
 844                     /* Compute distances for points and correspond lines */
 845                     {
 846                         /* Create corresponde lines */
 847
 848                         icvComputeCorrespondEpilines(wpoints1,2,&fund7,corrLines2);
 849                         icvComputeCorrespondEpilines(wpoints2,1,&fund7,corrLines1);
 850                         /* compute distances for points and number of good points */
 851                         int i;
 852                         numGoodPoints = 0;
 853                         for( i = 0; i < numPoint; i++ )
 854                         {
 855                             CvMat pnt1,pnt2;
 856                             CvMat lin1,lin2;
 857                             cvGetCol(wpoints1,&pnt1,i);
 858                             cvGetCol(corrLines1,&lin1,i);
 859                             cvGetCol(wpoints2,&pnt2,i);
 860                             cvGetCol(corrLines2,&lin2,i);
 861                             double dist1,dist2;
 862                             dist1 = fabs(cvDotProduct(&pnt1,&lin1));
 863                             dist2 = fabs(cvDotProduct(&pnt2,&lin2));
 864                             flags[i] = ( dist1 < threshold && dist2 < threshold )?1:0;
 865                             numGoodPoints += flags[i];
 866                         }
 867                     }
 868                 }
 869
 870                 if( numGoodPoints > maxGoodPoints )
 871                 {/* good matrix */
 872                     cvCopy(&fund7,&bestFund);
 873                     maxGoodPoints = numGoodPoints;
 874                     /* copy best flags */
 875                     int i;
 876                     for(i=0;i<numPoint;i++)
 877                     {
 878                         bestFlags[i] = flags[i];
 879                     }
 880                 }
 881             }
 882
 883                         /* Adaptive number of samples to count*/
 884                         double ep = 1 - (double)numGoodPoints / (double)numPoint;
 885             if( ep == 1 )
 886             {
 887                 ep = 0.5;//if there is not good points set ration of outliers to 50%
 888             }
 889
 890                         NumSamples = cvRound(log(1-p) / log(1-pow(1-ep,7)));
 891             wasCount++;
 892         }
 893
 894         /* we have best 7-point fundamental matrix. */
 895         /* and best points */
 896         /* use these points to improve matrix */
 897
 898         /* collect best points */
 899         /* copy points */
 900
 901         /* Test number of points. And if number >=8 improove found fundamental matrix  */
 902
 903         if( maxGoodPoints < 7 )
 904         {
 905             /* Fundamental matrix not found */
 906             numFundMatr = 0;
 907         }
 908         else
 909         {
 910             if( maxGoodPoints > 7 )
 911             {
 912                 /* Found >= 8 point. Improove matrix */
 913                 int i;
 914                 int currPnt;
 915                 currPnt = 0;
 916
 917                 for( i = 0; i < numPoint; i++ )
 918                 {
 919                     if( bestFlags[i] )
 920                     {
 921                         CvMat wPnt;
 922                         CvMat bestPnt;
 923                         cvGetCol( wpoints1,&wPnt, i );
 924                         cvGetCol( bestPoints1,&bestPnt, currPnt );
 925                         cvCopy(&wPnt,&bestPnt);
 926                         cvGetCol( wpoints2,&wPnt, i );
 927                         cvGetCol( bestPoints2,&bestPnt, currPnt );
 928                         cvCopy(&wPnt,&bestPnt);
 929                         currPnt++;
 930                     }
 931                 }
 932
 933                 CvMat wbestPnts1;
 934                 CvMat wbestPnts2;
 935
 936                 cvGetSubArr( bestPoints1, &wbestPnts1, cvRect(0,0,maxGoodPoints,3) );
 937                 cvGetSubArr( bestPoints2, &wbestPnts2, cvRect(0,0,maxGoodPoints,3) );
 938
 939                 /* best points was collectet. Improve fundamental matrix */
 940                 /* !!! Just use 8-point algorithm */
 941                 double impFundMatr_dat[9];
 942                 CvMat impFundMatr;
 943                 impFundMatr = cvMat(3,3,CV_64F,impFundMatr_dat);
 944                 numFundMatr = icvComputeFundamental8Point(&wbestPnts1,&wbestPnts2,&impFundMatr);
 945
 946                 cvConvert(&impFundMatr,fundMatr);
 947                 //cvConvert(&bestFund,fundMatr); // This line must be deleted
 948             }
 949             else
 950             {
 951                 /* 7 point. Just copy to result */
 952                 cvConvert(&bestFund,fundMatr);
 953                 numFundMatr = 1;
 954             }
 955
 956             /* copy flag to status if need */
 957             if( status )
 958             {
 959                 for( int i = 0; i < numPoint; i++)
 960                 {
 961                     cvmSet(status,0,i,(double)bestFlags[i]);
 962                 }
 963             }
 964         }
 965
 966
 967     }
 968
 969     __END__;
 970
 971     /* free allocated memory */
 972
 973     cvReleaseMat(&corrLines1);
 974     cvReleaseMat(&corrLines2);
 975     cvReleaseMat(&wpoints1);
 976     cvReleaseMat(&wpoints2);
 977     cvReleaseMat(&bestPoints1);
 978     cvReleaseMat(&bestPoints2);
 979     cvFree((void**)&flags);
 980     cvFree((void**)&bestFlags);
 981
 982     return numFundMatr;
 983 }//icvComputeFundamentalRANSAC
 984
 985 /*=====================================================================================*/
 986
 987 void icvCompPointLineDists(CvMat* points,CvMat* lines,CvMat* distances)
 988 {/* Line must be normalized */
 989
 990     int numPoints;
 991     numPoints = points->cols;
 992     if( numPoints != lines->cols && numPoints != distances->cols )
 993     {
 994         //printf("Size if arrays not equals\n");
 995         return;//error
 996     }
 997
 998     int i;
 999     for( i = 0; i < numPoints; i++ )
1000     {
1001         CvMat pnt;
1002         CvMat lin;
1003         cvGetCol(points,&pnt,i);
1004         cvGetCol(lines,&lin,i);
1005         double dist;
1006         dist = fabs(cvDotProduct(&pnt,&lin));
1007         cvmSet(distances,0,i,dist);
1008     }
1009
1010     return;
1011 }
1012
1013 /*=====================================================================================*/
1014
1015
1016
1017 #define _compVals( v1, v2 )  ((v1) < (v2))
1018
1019 /* Create function to sort vector */
1020 CV_IMPLEMENT_QSORT( _SortCvMatVect, double, _compVals )
1021
1022 /*=====================================================================================*/
1023 int icvComputeFundamentalLMedS(    CvMat* points1,CvMat* points2,
1024                                     CvMat* fundMatr,
1025                                     double threshold,/* Threshold for good points */
1026                                                                         double p,/* Probability of good result. */
1027                                     CvMat* status)
1028 {
1029     CvMat* wpoints1 = 0;
1030     CvMat* wpoints2 = 0;
1031     CvMat* corrLines1 = 0;
1032     CvMat* corrLines2 = 0;
1033     CvMat* bestPoints1 = 0;
1034     CvMat* bestPoints2 = 0;
1035     CvMat* dists1 = 0;
1036     CvMat* dists2 = 0;
1037     CvMat* distsSq1 = 0;
1038     CvMat* distsSq2 = 0;
1039     CvMat* allDists = 0;
1040
1041     int* flags = 0;
1042     int* bestFlags = 0;
1043     int numFundMatr = 0;
1044
1045     CV_FUNCNAME( "icvComputeFundamentalLMedS" );
1046     __BEGIN__;
1047
1048     /* Test correct of input data */
1049
1050     if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
1051     {
1052         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
1053     }
1054
1055     int numPoint;
1056     numPoint = points1->cols;
1057
1058     int type;
1059     type = points1->type;
1060
1061     if( numPoint != points2->cols )
1062     {
1063         CV_ERROR( CV_StsBadSize, "Number of points not equal" );
1064     }
1065     if( numPoint < 8 )
1066     {
1067         CV_ERROR( CV_StsBadSize, "Number of points must be >= 8" );
1068     }
1069
1070     if( points1->rows != 2 && points1->rows != 3 )
1071     {
1072         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
1073     }
1074
1075     if( points2->rows != 2 && points2->rows != 3 )
1076     {
1077         CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
1078     }
1079
1080     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
1081     {
1082         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
1083     }
1084
1085     if( status )
1086     {/* status is present test it */
1087         if( !CV_IS_MAT(status) )
1088         {
1089             CV_ERROR(CV_StsBadPoint,"status is not a matrix");
1090         }
1091
1092         if( status->cols != numPoint || status->rows != 1 )
1093         {
1094             CV_ERROR(CV_StsBadPoint,"Size of status must be 1xN");
1095         }
1096     }
1097
1098     /* We will choose adaptive number of N (samples) */
1099
1100     /* Convert points to 64F working points */
1101
1102     CV_CALL( flags = (int*)cvAlloc(numPoint * sizeof(int)) );
1103     CV_CALL( bestFlags = (int*)cvAlloc(numPoint * sizeof(int)) );
1104     CV_CALL( wpoints1 = cvCreateMat(3,numPoint,CV_64F) );
1105     CV_CALL( wpoints2 = cvCreateMat(3,numPoint,CV_64F) );
1106     CV_CALL( corrLines1 = cvCreateMat(3,numPoint,CV_64F));
1107     CV_CALL( corrLines2 = cvCreateMat(3,numPoint,CV_64F));
1108     CV_CALL( bestPoints1 = cvCreateMat(3,numPoint,CV_64F));
1109     CV_CALL( bestPoints2 = cvCreateMat(3,numPoint,CV_64F));
1110     CV_CALL( dists1 = cvCreateMat(1,numPoint,CV_64F));
1111     CV_CALL( dists2 = cvCreateMat(1,numPoint,CV_64F));
1112     CV_CALL( distsSq1 = cvCreateMat(1,numPoint,CV_64F));
1113     CV_CALL( distsSq2 = cvCreateMat(1,numPoint,CV_64F));
1114     CV_CALL( allDists = cvCreateMat(1,numPoint,CV_64F));
1115
1116     icvMake3DPoints(points1,wpoints1);
1117     icvMake3DPoints(points2,wpoints2);
1118
1119     srand(54321);
1120
1121     {
1122         int NumSamples = 1;//just init number of samples
1123         int wasCount = 0;  //count of choosing samples
1124
1125                 double goodMean = FLT_MAX;
1126                 double currMean;
1127
1128         int numGoodPoints = 0;
1129
1130         double bestFund_dat[9];
1131         CvMat  bestFund;
1132         bestFund = cvMat(3,3,CV_64F,bestFund_dat);
1133
1134         /* choosen points */
1135         while( NumSamples > wasCount )
1136         {
1137             /* select samples */
1138             int randNumbs[7];
1139             int i;
1140             int newnum;
1141             int pres;
1142             for( i = 0; i < 7; i++ )
1143             {
1144                 do
1145                 {
1146                     newnum = rand()%numPoint;
1147
1148                     /* test this number */
1149                     pres = 0;
1150                     int test;
1151                     for( test = 0; test < i; test++ )
1152                     {
1153                         if( randNumbs[test] == newnum )
1154                         {
1155                             pres = 1;
1156                             break;
1157                         }
1158                     }
1159                 }
1160                 while( pres );
1161                 randNumbs[i] = newnum;
1162             }
1163             /* random numbers of points was generated */
1164             /* select points */
1165
1166             double selPoints1_dat[2*7];
1167             double selPoints2_dat[2*7];
1168             CvMat selPoints1;
1169             CvMat selPoints2;
1170             selPoints1 = cvMat(2,7,CV_64F,selPoints1_dat);
1171             selPoints2 = cvMat(2,7,CV_64F,selPoints2_dat);
1172             /* copy points */
1173             int t;
1174             for( t = 0; t < 7; t++ )
1175             {
1176                 double x,y;
1177                 x = cvmGet(wpoints1,0,randNumbs[t]);
1178                 y = cvmGet(wpoints1,1,randNumbs[t]);
1179                 cvmSet(&selPoints1,0,t,x);
1180                 cvmSet(&selPoints1,1,t,y);
1181
1182                 x = cvmGet(wpoints2,0,randNumbs[t]);
1183                 y = cvmGet(wpoints2,1,randNumbs[t]);
1184                 cvmSet(&selPoints2,0,t,x);
1185                 cvmSet(&selPoints2,1,t,y);
1186             }
1187             /* Compute fundamental matrix using 7-points algorithm */
1188
1189             double fundTriple_dat[27];
1190             CvMat fundTriple;
1191             fundTriple = cvMat(9,3,CV_64F,fundTriple_dat);
1192
1193             int numFund = icvComputeFundamental7Point(&selPoints1,&selPoints2,&fundTriple);
1194
1195             //double fund7_dat[9];
1196             CvMat fund7;
1197             //fund7 = cvMat(3,3,CV_64F,fund7_dat);
1198
1199             /* get sub matrix */
1200             for( int currFund = 0; currFund < numFund; currFund++ )
1201             {
1202
1203                 cvGetSubArr(&fundTriple,&fund7,cvRect(0,currFund*3,3,3));
1204                 {
1205                                     /* Compute median error for this matrix */
1206                                     {
1207                         icvComputeCorrespondEpilines(wpoints1,2,&fund7,corrLines2);
1208                         icvComputeCorrespondEpilines(wpoints2,1,&fund7,corrLines1);
1209
1210                                             icvCompPointLineDists(wpoints1,corrLines1,dists1);
1211                                             icvCompPointLineDists(wpoints2,corrLines2,dists2);
1212
1213                                             /* add distances for points (d1*d1+d2*d2) */
1214                                             cvMul(dists1,dists1,distsSq1);
1215                                             cvMul(dists2,dists2,distsSq2);
1216
1217                                             cvAdd(distsSq1,distsSq2,allDists);
1218
1219                                             /* sort distances */
1220                                             _SortCvMatVect(allDists->data.db,numPoint,0);
1221
1222                                             /* get median error */
1223                                             currMean = allDists->data.db[numPoint/2];
1224                                     }
1225                 }
1226
1227                 if( currMean < goodMean )
1228                 {/* good matrix */
1229                     cvCopy(&fund7,&bestFund);
1230                     goodMean = currMean;
1231
1232                     /* Compute number of good points using threshold */
1233                     int i;
1234                     numGoodPoints = 0;
1235                     for( i = 0 ; i < numPoint; i++ )
1236                     {
1237                         if( dists1->data.db[i] < threshold && dists2->data.db[i] < threshold )
1238                         {
1239                             numGoodPoints++;
1240                         }
1241                     }
1242                 }
1243
1244             }
1245
1246             /* Compute adaptive number of steps */
1247                         double ep = 1 - (double)numGoodPoints / (double)numPoint;
1248             if( ep == 1 )
1249             {
1250                 ep = 0.5;//if there is not good points set ration of outliers to 50%
1251             }
1252
1253                         NumSamples = cvRound(log(1-p) / log(1-pow(1-ep,7)));
1254
1255             wasCount++;
1256         }
1257
1258         /* Select just good points using threshold */
1259         /* Compute distances for all points for best fundamental matrix */
1260
1261         /* Test if we have computed fundamental matrix*/
1262         if( goodMean == FLT_MAX )
1263         {
1264             numFundMatr = 0;
1265
1266         }
1267         else
1268         {/* we have computed fundamental matrix */
1269                     {
1270                 icvComputeCorrespondEpilines(wpoints1,2,&bestFund,corrLines2);
1271                 icvComputeCorrespondEpilines(wpoints2,1,&bestFund,corrLines1);
1272
1273                             icvCompPointLineDists(wpoints1,corrLines1,dists1);
1274                             icvCompPointLineDists(wpoints2,corrLines2,dists2);
1275
1276                 /* test dist for each point and set status for each point if need */
1277                 int i;
1278                 int currPnt = 0;
1279                 for( i = 0; i < numPoint; i++ )
1280                 {
1281                     if( dists1->data.db[i] < threshold && dists2->data.db[i] < threshold )
1282                     {
1283                         CvMat wPnt;
1284                         CvMat bestPnt;
1285                         cvGetCol( wpoints1,&wPnt, i );
1286                         cvGetCol( bestPoints1,&bestPnt, currPnt );
1287                         cvCopy(&wPnt,&bestPnt);
1288                         cvGetCol( wpoints2,&wPnt, i );
1289                         cvGetCol( bestPoints2,&bestPnt, currPnt );
1290                         cvCopy(&wPnt,&bestPnt);
1291                         currPnt++;
1292
1293                         if( status )
1294                             cvmSet(status,0,i,1.0);
1295                     }
1296                     else
1297                     {
1298                         if( status )
1299                             cvmSet(status,0,i,0.0);
1300                     }
1301
1302                 }
1303                 numGoodPoints = currPnt;
1304                     }
1305
1306             /* we have best 7-point fundamental matrix. */
1307             /* and best points */
1308             /* use these points to improove matrix */
1309
1310             /* Test number of points. And if number >=8 improove found fundamental matrix  */
1311
1312             if( numGoodPoints < 7 )
1313             {
1314                 /* Fundamental matrix not found */
1315                 numFundMatr = 0;
1316             }
1317             else
1318             {
1319                 if( numGoodPoints > 7 )
1320                 {
1321                     /* Found >= 8 point. Improove matrix */
1322
1323                     CvMat wbestPnts1;
1324                     CvMat wbestPnts2;
1325
1326                     cvGetSubArr( bestPoints1, &wbestPnts1, cvRect(0,0,numGoodPoints,3) );
1327                     cvGetSubArr( bestPoints2, &wbestPnts2, cvRect(0,0,numGoodPoints,3) );
1328
1329                     /* best points was collectet. Improve fundamental matrix */
1330                     /* !!! Just use 8-point algorithm */
1331                     double impFundMatr_dat[9];
1332                     CvMat impFundMatr;
1333                     impFundMatr = cvMat(3,3,CV_64F,impFundMatr_dat);
1334                     numFundMatr = icvComputeFundamental8Point(&wbestPnts1,&wbestPnts2,&impFundMatr);
1335
1336                     cvConvert(&impFundMatr,fundMatr);
1337                 }
1338                 else
1339                 {
1340                     /* 7 point. Just copy to result */
1341                     cvConvert(&bestFund,fundMatr);
1342                     numFundMatr = 1;
1343                 }
1344             }
1345
1346         }
1347     }
1348
1349     __END__;
1350
1351     /* free allocated memory */
1352
1353     cvReleaseMat(&corrLines1);
1354     cvReleaseMat(&corrLines2);
1355     cvReleaseMat(&wpoints1);
1356     cvReleaseMat(&wpoints2);
1357     cvReleaseMat(&bestPoints1);
1358     cvReleaseMat(&bestPoints2);
1359     cvReleaseMat(&dists1);
1360     cvReleaseMat(&dists2);
1361     cvReleaseMat(&distsSq1);
1362     cvReleaseMat(&distsSq2);
1363     cvReleaseMat(&allDists);
1364     cvFree((void**)&flags);
1365     cvFree((void**)&bestFlags);
1366
1367     return numFundMatr;
1368 }//icvComputeFundamentalLMedS
1369
1370 /*=====================================================================================*/
1371
1372
1373
1374 void icvMakeFundamentalSingular(CvMat* fundMatr)
1375 {
1376     CV_FUNCNAME( "icvFundSingular" );
1377     __BEGIN__;
1378
1379     if( !CV_IS_MAT(fundMatr) )
1380     {
1381         CV_ERROR(CV_StsBadPoint,"Input data is not matrix");
1382     }
1383
1384     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
1385     {
1386         CV_ERROR( CV_StsBadSize, "Size of fundametal matrix must be 3x3" );
1387     }
1388
1389
1390     {/* Apply singularity condition */
1391         CvMat matrFU;
1392         CvMat matrFW;
1393         CvMat matrFVt;
1394         CvMat tmpMatr;
1395         CvMat preFundMatr;
1396         double matrFU_dat[9];
1397         double matrFW_dat[9];
1398         double matrFVt_dat[9];
1399         double tmpMatr_dat[9];
1400         double preFundMatr_dat[9];
1401
1402
1403         matrFU  = cvMat(3,3,CV_64F,matrFU_dat);
1404         matrFW  = cvMat(3,3,CV_64F,matrFW_dat);
1405         matrFVt = cvMat(3,3,CV_64F,matrFVt_dat);
1406         tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
1407         preFundMatr = cvMat(3,3,CV_64F,preFundMatr_dat);
1408
1409         cvConvert(fundMatr,&preFundMatr);
1410         cvSVD( &preFundMatr, &matrFW, &matrFU, &matrFVt, CV_SVD_V_T );
1411         cvmSet(&matrFW,2,2,0);
1412         /* multiply U*W*V' */
1413
1414         cvmMul(&matrFU,&matrFW,&tmpMatr);
1415         cvmMul(&tmpMatr,&matrFVt,&preFundMatr);
1416         cvConvert(&preFundMatr,fundMatr);
1417     }
1418
1419
1420     __END__;
1421 }
1422
1423 /*=====================================================================================*/
1424 /* Normalize points for computing fundamental matrix */
1425 /* and compute transform matrix */
1426 /* Points:  2xN  */
1427 /* Matrix:  3x3 */
1428 /* place centroid of points to (0,0) */
1429 /* set mean distance from (0,0) by sqrt(2) */
1430
1431 void icvNormalizeFundPoints( CvMat* points,
1432                         CvMat* transfMatr)
1433 {
1434     CvMat* subwpointsx = 0;
1435     CvMat* subwpointsy = 0;
1436     CvMat* sqdists     = 0;
1437     CvMat* pointsxx    = 0;
1438     CvMat* pointsyy    = 0;
1439
1440     CV_FUNCNAME( "icvNormalizeFundPoints" );
1441     __BEGIN__;
1442
1443     /* Test for correct input data */
1444
1445     if( !CV_IS_MAT(points) || !CV_IS_MAT(transfMatr) )
1446     {
1447         CV_ERROR(CV_StsBadPoint,"Input data is not matrixes");
1448     }
1449
1450     int numPoint;
1451     numPoint = points->cols;
1452
1453     int type;
1454     type = points->type;
1455
1456     if( numPoint < 1 )
1457     {
1458         CV_ERROR( CV_StsBadSize, "Number of points must be at least 1" );
1459     }
1460
1461     if( points->rows != 2 )
1462     {
1463         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2" );
1464     }
1465
1466     if( transfMatr->rows != 3 || transfMatr->cols != 3 )
1467     {
1468         CV_ERROR( CV_StsBadSize, "Size of transform matrix must be 3x3" );
1469     }
1470
1471     CV_CALL( subwpointsx = cvCreateMat(1,numPoint,CV_64F));
1472     CV_CALL( subwpointsy = cvCreateMat(1,numPoint,CV_64F));
1473     CV_CALL( sqdists     = cvCreateMat(1,numPoint,CV_64F));
1474     CV_CALL( pointsxx    = cvCreateMat(1,numPoint,CV_64F));
1475     CV_CALL( pointsyy    = cvCreateMat(1,numPoint,CV_64F));
1476
1477     /* get x,y coordinates of points */
1478
1479     {
1480         CvMat tmpwpointsx;
1481         CvMat tmpwpointsy;
1482         cvGetRow( points, &tmpwpointsx, 0 );
1483         cvGetRow( points, &tmpwpointsy, 1 );
1484
1485         /* Copy to working data 64F */
1486         cvConvert(&tmpwpointsx,subwpointsx);
1487         cvConvert(&tmpwpointsy,subwpointsy);
1488     }
1489
1490     double shiftx,shifty;
1491
1492     /* Compute center of points */
1493
1494     CvScalar sumx = cvSum(subwpointsx);
1495     CvScalar sumy = cvSum(subwpointsy);
1496
1497     sumx.val[0] /= (double)numPoint;
1498     sumy.val[0] /= (double)numPoint;
1499
1500     shiftx = sumx.val[0];
1501     shifty = sumy.val[0];
1502
1503     /* Shift points center to 0 */
1504
1505     cvSubS( subwpointsx, sumx, subwpointsx);
1506     cvSubS( subwpointsy, sumy, subwpointsy);
1507
1508     /* Compute x*x and y*y */
1509
1510     cvMul(subwpointsx,subwpointsx,pointsxx);
1511     cvMul(subwpointsy,subwpointsy,pointsyy);
1512
1513     /* add  */
1514     cvAdd( pointsxx, pointsyy, sqdists);
1515
1516     /* compute sqrt of each component*/
1517
1518     cvPow(sqdists,sqdists,0.5);
1519
1520     /* in vector sqdists we have distances */
1521     /* compute mean value and scale */
1522
1523     double meand;
1524     meand = cvMean(sqdists);
1525     double scale;
1526
1527     if( fabs(meand) > 1e-8  )
1528     {
1529         scale = sqrt(2)/meand;
1530     }
1531     else
1532     {
1533         scale = 1.0;
1534     }
1535
1536
1537     /* scale working points */
1538     cvScale(subwpointsx,subwpointsx,scale);
1539     cvScale(subwpointsy,subwpointsy,scale);
1540
1541     /* copy output data */
1542     {
1543         CvMat tmpwpointsx;
1544         CvMat tmpwpointsy;
1545         cvGetRow( points, &tmpwpointsx, 0 );
1546         cvGetRow( points, &tmpwpointsy, 1 );
1547
1548         /* Copy to output data 64F */
1549         cvConvert(subwpointsx,&tmpwpointsx);
1550         cvConvert(subwpointsy,&tmpwpointsy);
1551     }
1552
1553     /* Set transform matrix */
1554
1555     cvmSet(transfMatr,0,0, scale);
1556     cvmSet(transfMatr,0,1, 0);
1557     cvmSet(transfMatr,0,2, -scale*shiftx);
1558
1559     cvmSet(transfMatr,1,0, 0);
1560     cvmSet(transfMatr,1,1, scale);
1561     cvmSet(transfMatr,1,2, -scale*shifty);
1562
1563     cvmSet(transfMatr,2,0, 0);
1564     cvmSet(transfMatr,2,1, 0);
1565     cvmSet(transfMatr,2,2, 1);
1566
1567     __END__;
1568
1569     /* Free data */
1570     cvReleaseMat(&subwpointsx);
1571     cvReleaseMat(&subwpointsy);
1572     cvReleaseMat(&sqdists);
1573     cvReleaseMat(&pointsxx);
1574     cvReleaseMat(&pointsyy);
1575
1576 }
1577 /*=====================================================================================*/
1578 // Solve cubic equation and returns number of roots
1579 int cvSolveCubic(CvMat* coeffs,CvMat* result)
1580 {
1581     return icvSolveCubic(coeffs, result);
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 int icvSolveCubic(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 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(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 void icvMake3DPoints(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 void cvComputeCorrespondEpilines(CvMat* points,int pointImageID,CvMat* fundMatr,CvMat* corrLines)
1919 {
1920         icvComputeCorrespondEpilines(points, pointImageID, fundMatr, corrLines);
1921         return;
1922 }
1923 /*=====================================================================================*/
1924 void icvComputeCorrespondEpilines(CvMat* points,int pointImageID,CvMat* fundMatr,CvMat* corrLines)
1925 {
1926
1927     CvMat* wpoints = 0;
1928     CvMat* wcorrLines = 0;
1929
1930     pointImageID = 3-pointImageID;
1931
1932     CV_FUNCNAME( "icvComputeCorrespondEpilines" );
1933     __BEGIN__;
1934
1935     /* Test correct of input data */
1936
1937     if( !CV_IS_MAT(points) || !CV_IS_MAT(fundMatr)|| !CV_IS_MAT(corrLines))
1938     {
1939         CV_ERROR(CV_StsBadPoint,"Not a matrixes");
1940     }
1941
1942     /*  */
1943
1944     int numPoint;
1945     numPoint = points->cols;
1946
1947     if( numPoint != corrLines->cols )
1948     {
1949         CV_ERROR( CV_StsBadSize, "Number of points and lines are not equal" );
1950     }
1951
1952     if( numPoint < 1 )
1953     {
1954         CV_ERROR( CV_StsBadSize, "Number of points must > 0" );
1955     }
1956
1957     if( points->rows != 2 && points->rows != 3 )
1958     {
1959         CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
1960     }
1961
1962     if( corrLines->rows != 3 )
1963     {
1964         CV_ERROR( CV_StsBadSize, "Number of coordinates of corrLines must be 3" );
1965     }
1966
1967     if( fundMatr->rows != 3 || fundMatr->cols != 3 )
1968     {
1969         CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
1970     }
1971
1972     double wfundMatr_dat[9];
1973     CvMat wfundMatr;
1974     wfundMatr = cvMat(3,3,CV_64F,wfundMatr_dat);
1975     cvConvert(fundMatr,&wfundMatr);
1976
1977     if( pointImageID == 1 )
1978     {// get transformed fundamental matrix
1979         double tmpMatr_dat[9];
1980         CvMat tmpMatr;
1981         tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
1982         cvConvert(fundMatr,&tmpMatr);
1983         cvTranspose(&tmpMatr,&wfundMatr);
1984     }
1985     else if( pointImageID != 2 )
1986     {
1987         CV_ERROR( CV_StsBadArg, "Image ID must be 1 or 2" );
1988     }
1989     /* if wfundMatr we have good fundamental matrix */
1990     /* compute corr epi line for given points */
1991
1992     CV_CALL( wpoints = cvCreateMat(3,numPoint,CV_64F) );
1993     CV_CALL( wcorrLines = cvCreateMat(3,numPoint,CV_64F) );
1994
1995
1996     /* if points has 2 coordinates trandform them to 3D */
1997     icvMake3DPoints(points,wpoints);
1998
1999     cvmMul(&wfundMatr,wpoints,wcorrLines);
2000
2001     /* normalise line coordinates */
2002     int i;
2003     for( i = 0; i < numPoint; i++ )
2004     {
2005         CvMat line;
2006         cvGetCol(wcorrLines,&line,i);
2007         double a,b;
2008         a = cvmGet(&line,0,0);
2009         b = cvmGet(&line,1,0);
2010         double nv;
2011         nv = sqrt(a*a+b*b);
2012         cvConvertScale(&line,&line,1.0 / nv);
2013     }
2014     cvConvert(wcorrLines,corrLines);
2015
2016
2017     __END__;
2018
2019     cvReleaseMat(&wpoints);
2020     cvReleaseMat(&wcorrLines);
2021
2022 }
2023
2024 /*=====================================================================================*/
2025
2026 #define SIGN(x) ( (x)<0 ? -1:((x)>0?1:0 ) )
2027 //#define REAL_ZERO(x) ( (x) < EPSILON && (x) > -EPSILON)
2028 #define REAL_ZERO(x) ( (x) < 1e-8 && (x) > -1e-8)
2029
2030 /* function return squares for cubic equation. 6 params - two for each square (Re,Im) */
2031 int
2032 icvCubicV( double a2, double a1, double a0, double *squares )
2033 {
2034     double p, q, D, c1, c2, b1, b2, ro1, ro2, fi1, fi2;
2035     double x[6][3];
2036     int i, j, t;
2037
2038     if( !squares )
2039         return CV_BADFACTOR_ERR;
2040
2041     if( fabs(a0) > FLT_MAX || fabs(a1) > FLT_MAX || fabs(a2) > FLT_MAX )
2042     {
2043         return 0;//Coeffs too big
2044     }
2045
2046
2047     p = a1 - a2 * a2 / 3;
2048     q = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 27;
2049     D = q * q / 4 + p * p * p / 27;
2050
2051     if( fabs(p) > FLT_MAX || fabs(q) > FLT_MAX || fabs(D) > FLT_MAX )
2052     {
2053         return 0;//Coeffs too big
2054     }
2055
2056     if( D < 0 )
2057     {
2058
2059         c1 = q / 2;
2060         c2 = c1;
2061         b1 = sqrt( -D );
2062         b2 = -b1;
2063
2064         ro1 = sqrt( c1 * c1 - D );
2065         ro2 = ro1;
2066
2067         fi1 = atan2( b1, c1 );
2068         fi2 = -fi1;
2069     }
2070     else
2071     {
2072
2073         c1 = q / 2 + sqrt( D );
2074         c2 = q / 2 - sqrt( D );
2075         b1 = 0;
2076         b2 = 0;
2077
2078         ro1 = fabs( c1 );
2079         ro2 = fabs( c2 );
2080         fi1 = CV_PI * (1 - SIGN( c1 )) / 2;
2081         fi2 = CV_PI * (1 - SIGN( c2 )) / 2;
2082     }                           /* if */
2083
2084     for( i = 0; i < 6; i++ )
2085     {
2086
2087         x[i][0] = -a2 / 3;
2088         x[i][1] = 0;
2089         x[i][2] = 0;
2090
2091         squares[i] = x[i][i % 2];
2092     }                           /* for */
2093
2094     if( !REAL_ZERO( ro1 ))
2095     {
2096
2097         c1 = SIGN( ro1 ) * pow( fabs( ro1 ), 1. / 3 ) -
2098             SIGN( ro1 ) * p / 3. * pow( fabs( ro1 ), -1. / 3 );
2099
2100         c2 = SIGN( ro1 ) * pow( fabs( ro1 ), 1. / 3 ) +
2101             SIGN( ro1 ) * p / 3. * pow( fabs( ro1 ), -1. / 3 );
2102     }                           /* if */
2103
2104     if( !REAL_ZERO( ro2 ))
2105     {
2106
2107         b1 = SIGN( ro2 ) * pow( fabs( ro2 ), 1. / 3 ) -
2108             SIGN( ro2 ) * p / 3. * pow( fabs( ro2 ), -1. / 3 );
2109
2110         b2 = SIGN( ro2 ) * pow( fabs( ro2 ), 1. / 3 ) +
2111             SIGN( ro2 ) * p / 3. * pow( fabs( ro2 ), -1. / 3 );
2112     }                           /* if */
2113
2114     for( i = 0; i < 6; i++ )
2115     {
2116
2117         if( i < 3 )
2118         {
2119
2120             if( !REAL_ZERO( ro1 ))
2121             {
2122
2123                 x[i][0] = cos( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c1 - a2 / 3;
2124                 x[i][1] = sin( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c2;
2125             }
2126             else
2127             {
2128
2129                 //x[i][2] = 1;!!!
2130             }                   /* if */
2131         }
2132         else
2133         {
2134
2135             if( !REAL_ZERO( ro2 ))
2136             {
2137
2138                 x[i][0] = cos( fi2 / 3. + 2 * CV_PI * (i % 3) / 3. ) * b1 - a2 / 3;
2139                 x[i][1] = sin( fi2 / 3. + 2 * CV_PI * (i % 3) / 3. ) * b2;
2140             }
2141             else
2142             {
2143
2144                 //x[i][2] = 1;!!!
2145             }                   /* if */
2146         }                       /* if */
2147     }                           /* for */
2148
2149     t = 0;
2150
2151     //int numRoots = 6;
2152     for( i = 0; i < 6 && t < 6; i++ )
2153     {
2154
2155         if( !x[i][2] )
2156         {
2157
2158             squares[t++] = x[i][0];
2159             squares[t++] = x[i][1];
2160             x[i][2] = 1;
2161
2162             for( j = i + 1; j < 6; j++ )
2163             {/* delete equal root from rest */
2164
2165                 if( !x[j][2] && REAL_ZERO( x[i][0] - x[j][0] )
2166                     && REAL_ZERO( x[i][1] - x[j][1] ))
2167                 {
2168
2169                     x[j][2] = 1;
2170                     break;
2171                 }               /* if */
2172             }                   /* for */
2173         }                       /* if */
2174     }                           /* for */
2175     return 3;
2176 }                               /* icvCubic */
2177
2178 /*=====================================================================================*/
2179
2180
2181
2182
2183
2184
2185
2186
2187
2188 /* Obsolete functions. Just for ViewMorping */
2189 /*=====================================================================================*/
2190
2191 int
2192 icvGaussMxN( double *A, double *B, int M, int N, double **solutions )
2193 {
2194     int *variables;
2195     int row, swapi, i, i_best = 0, j, j_best = 0, t;
2196     double swapd, ratio, bigest;
2197
2198     if( !A || !B || !M || !N )
2199         return -1;
2200
2201     variables = (int *) icvAlloc( (long) N * sizeof( int ));
2202
2203     if( variables == 0 )
2204         return -1;
2205
2206     for( i = 0; i < N; i++ )
2207     {
2208         variables[i] = i;
2209     }                           /* for */
2210
2211     /* -----  Direct way  ----- */
2212
2213     for( row = 0; row < M; row++ )
2214     {
2215
2216         bigest = 0;
2217
2218         for( j = row; j < M; j++ )
2219         {                       /* search non null element */
2220             for( i = row; i < N; i++ )
2221             {
2222
2223                 if( fabs( A[j * N + i] ) > fabs( bigest ))
2224                 {
2225                     bigest = A[j * N + i];
2226                     i_best = i;
2227                     j_best = j;
2228                 }               /* if */
2229             }                   /* for */
2230         }                       /* for */
2231
2232         if( REAL_ZERO( bigest ))
2233             break;              /* if all shank elements are null */
2234
2235         if( j_best - row )
2236         {
2237
2238             for( t = 0; t < N; t++ )
2239             {                   /* swap a rows */
2240
2241                 swapd = A[row * N + t];
2242                 A[row * N + t] = A[j_best * N + t];
2243                 A[j_best * N + t] = swapd;
2244             }                   /* for */
2245
2246             swapd = B[row];
2247             B[row] = B[j_best];
2248             B[j_best] = swapd;
2249         }                       /* if */
2250
2251         if( i_best - row )
2252         {
2253
2254             for( t = 0; t < M; t++ )
2255             {                   /* swap a columns  */
2256
2257                 swapd = A[t * N + i_best];
2258                 A[t * N + i_best] = A[t * N + row];
2259                 A[t * N + row] = swapd;
2260             }                   /* for */
2261
2262             swapi = variables[row];
2263             variables[row] = variables[i_best];
2264             variables[i_best] = swapi;
2265         }                       /* if */
2266
2267         for( i = row + 1; i < M; i++ )
2268         {                       /* recounting A and B */
2269
2270             ratio = -A[i * N + row] / A[row * N + row];
2271             B[i] += B[row] * ratio;
2272
2273             for( j = N - 1; j >= row; j-- )
2274             {
2275
2276                 A[i * N + j] += A[row * N + j] * ratio;
2277             }                   /* for */
2278         }                       /* for */
2279     }                           /* for */
2280
2281     if( row < M )
2282     {                           /* if rank(A)<M */
2283
2284         for( j = row; j < M; j++ )
2285         {
2286             if( !REAL_ZERO( B[j] ))
2287             {
2288
2289                 icvFree( &variables );
2290                 return -1;      /* if system is antithetic */
2291             }                   /* if */
2292         }                       /* for */
2293
2294         M = row;                /* decreasing size of the task */
2295     }                           /* if */
2296
2297     /* ----- Reverse way ----- */
2298
2299     if( M < N )
2300     {                           /* if solution are not exclusive */
2301
2302         *solutions = (double *) icvAlloc( ((N - M + 1) * N) * sizeof( double ));
2303
2304         if( *solutions == 0 )
2305         {
2306             icvFree( &variables );
2307             return -1;
2308         }
2309
2310
2311         for( t = M; t <= N; t++ )
2312         {
2313             for( j = M; j < N; j++ )
2314             {
2315
2316                 (*solutions)[(t - M) * N + variables[j]] = (double) (t == j);
2317             }                   /* for */
2318
2319             for( i = M - 1; i >= 0; i-- )
2320             {                   /* finding component of solution */
2321
2322                 if( t < N )
2323                 {
2324                     (*solutions)[(t - M) * N + variables[i]] = 0;
2325                 }
2326                 else
2327                 {
2328                     (*solutions)[(t - M) * N + variables[i]] = B[i] / A[i * N + i];
2329                 }               /* if */
2330
2331                 for( j = i + 1; j < N; j++ )
2332                 {
2333
2334                     (*solutions)[(t - M) * N + variables[i]] -=
2335                         (*solutions)[(t - M) * N + variables[j]] * A[i * N + j] / A[i * N + i];
2336                 }               /* for */
2337             }                   /* for */
2338         }                       /* for */
2339
2340         icvFree( &variables );
2341         return N - M;
2342     }                           /* if */
2343
2344     *solutions = (double *) icvAlloc( (N) * sizeof( double ));
2345
2346     if( solutions == 0 )
2347         return -1;
2348
2349     for( i = N - 1; i >= 0; i-- )
2350     {                           /* finding exclusive solution */
2351
2352         (*solutions)[variables[i]] = B[i] / A[i * N + i];
2353
2354         for( j = i + 1; j < N; j++ )
2355         {
2356
2357             (*solutions)[variables[i]] -=
2358                 (*solutions)[variables[j]] * A[i * N + j] / A[i * N + i];
2359         }                       /* for */
2360     }                           /* for */
2361
2362     icvFree( &variables );
2363     return 0;
2364
2365 }                               /* icvGaussMxN */
2366
2367 /*=====================================================================================*/
2368
2369 CvStatus
2370 icvGetCoof( double *f1, double *f2, double *a2, double *a1, double *a0 )
2371 {
2372     double G[9], a3;
2373     int i;
2374
2375     if( !f1 || !f2 || !a0 || !a1 || !a2 )
2376         return CV_BADFACTOR_ERR;
2377
2378     for( i = 0; i < 9; i++ )
2379     {
2380
2381         G[i] = f1[i] - f2[i];
2382     }                           /* for */
2383
2384     a3 = icvDet( G );
2385
2386     if( REAL_ZERO( a3 ))
2387         return CV_BADFACTOR_ERR;
2388
2389     *a2 = 0;
2390     *a1 = 0;
2391     *a0 = icvDet( f2 );
2392
2393     for( i = 0; i < 9; i++ )
2394     {
2395
2396         *a2 += f2[i] * icvMinor( G, (int) (i % 3), (int) (i / 3) );
2397         *a1 += G[i] * icvMinor( f2, (int) (i % 3), (int) (i / 3) );
2398     }                           /* for */
2399
2400     *a0 /= a3;
2401     *a1 /= a3;
2402     *a2 /= a3;
2403
2404     return CV_NO_ERR;
2405
2406 }                               /* icvGetCoof */
2407
2408
2409
2410 /*======================================================================================*/
2411
2412 /*F///////////////////////////////////////////////////////////////////////////////////////
2413 //    Name:    icvLMedS7
2414 //    Purpose:
2415 //
2416 //
2417 //    Context:
2418 //    Parameters:
2419 //
2420 //
2421 //
2422 //
2423 //
2424 //
2425 //
2426 //    Returns:
2427 //      CV_NO_ERR if all Ok or error code
2428 //    Notes:
2429 //F*/
2430
2431 CvStatus
2432 icvLMedS7( int *points1, int *points2, CvMatrix3 * matrix )
2433 {                               /* Incorrect realization */
2434     CvStatus error = CV_NO_ERR;
2435
2436 /*    int         amount; */
2437     matrix = matrix;
2438     points1 = points1;
2439     points2 = points2;
2440
2441 /*    error = cs_Point7( points1, points2, matrix ); */
2442 /*    error = icvPoint7    ( points1, points2, matrix,&amount ); */
2443     return error;
2444
2445 }                               /* icvLMedS7 */
2446
2447
2448 /*======================================================================================*/
2449
2450 /*F///////////////////////////////////////////////////////////////////////////////////////
2451 //    Name:    icvPoint7
2452 //    Purpose:
2453 //
2454 //
2455 //    Context:
2456 //    Parameters:
2457 //
2458 //
2459 //
2460 //
2461 //
2462 //
2463 //
2464 //    Returns:
2465 //      CV_NO_ERR if all Ok or error code
2466 //    Notes:
2467 //F*/
2468
2469 CvStatus
2470 icvPoint7( int *ml, int *mr, double *F, int *amount )
2471 {
2472     double A[63], B[7];
2473     double *solutions;
2474     double a2, a1, a0;
2475     double squares[6];
2476     int i, j;
2477
2478 /*    int         amount; */
2479 /*    float*     F; */
2480
2481     CvStatus error = CV_BADFACTOR_ERR;
2482
2483 /*    F = (float*)matrix->m; */
2484
2485     if( !ml || !mr || !F )
2486         return CV_BADFACTOR_ERR;
2487
2488     for( i = 0; i < 7; i++ )
2489     {
2490         for( j = 0; j < 9; j++ )
2491         {
2492
2493             A[i * 9 + j] = (double) ml[i * 3 + j / 3] * (double) mr[i * 3 + j % 3];
2494         }                       /* for */
2495         B[i] = 0;
2496     }                           /* for */
2497
2498     *amount = 0;
2499
2500     if( icvGaussMxN( A, B, 7, 9, &solutions ) == 2 )
2501     {
2502         if( icvGetCoef( solutions, solutions + 9, &a2, &a1, &a0 ) == CV_NO_ERR )
2503         {
2504             icvCubic( a2, a1, a0, squares );
2505
2506             for( i = 0; i < 1; i++ )
2507             {
2508
2509                 if( REAL_ZERO( squares[i * 2 + 1] ))
2510                 {
2511
2512                     for( j = 0; j < 9; j++ )
2513                     {
2514
2515                         F[*amount + j] = (float) (squares[i] * solutions[j] +
2516                                                   (1 - squares[i]) * solutions[j + 9]);
2517                     }           /* for */
2518
2519                     *amount += 9;
2520
2521                     error = CV_NO_ERR;
2522                 }               /* if */
2523             }                   /* for */
2524
2525             icvFree( &solutions );
2526             return error;
2527         }
2528         else
2529         {
2530             icvFree( &solutions );
2531         }                       /* if */
2532
2533     }
2534     else
2535     {
2536         icvFree( &solutions );
2537     }                           /* if */
2538
2539     return error;
2540 }                               /* icvPoint7 */
2541