git.maemo.org Git - opencv/blob - src/cv/cvtriangulate.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) 2009, Intel Corporation and others, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cv.h"
  43
  44 // cvCorrectMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen.
  45 // cvTriangulatePoints function is derived from icvReconstructPointsFor3View, originally by Valery Mosyagin.
  46
  47 // HZ, R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, Cambridge Univ. Press, 2003.
  48
  49
  50
  51 // This method is the same as icvReconstructPointsFor3View, with only a few numbers adjusted for two-view geometry
  52 CV_IMPL void
  53 cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D)
  54 {
  55   CV_FUNCNAME( "cvTriangulatePoints" );
  56   __BEGIN__;
  57
  58   if( projMatr1 == 0 || projMatr2 == 0 ||
  59       projPoints1 == 0 || projPoints2 == 0 ||
  60       points4D == 0)
  61     {
  62       CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
  63     }
  64
  65   if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) ||
  66       !CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) ||
  67       !CV_IS_MAT(points4D) )
  68     {
  69       CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
  70     }
  71
  72   int numPoints;
  73   numPoints = projPoints1->cols;
  74
  75   if( numPoints < 1 )
  76     {
  77       CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );
  78     }
  79
  80   if( projPoints2->cols != numPoints || points4D->cols != numPoints )
  81     {
  82       CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" );
  83     }
  84
  85   if( projPoints1->rows != 2 || projPoints2->rows != 2)
  86     {
  87       CV_ERROR( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
  88     }
  89
  90   if( points4D->rows != 4 )
  91     {
  92       CV_ERROR( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
  93     }
  94
  95   if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
  96       projMatr2->cols != 4 || projMatr2->rows != 3)
  97     {
  98       CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
  99     }
 100
 101   CvMat matrA;
 102   double matrA_dat[24];
 103   matrA = cvMat(6,4,CV_64F,matrA_dat);
 104
 105   //CvMat matrU;
 106   CvMat matrW;
 107   CvMat matrV;
 108   //double matrU_dat[9*9];
 109   double matrW_dat[6*4];
 110   double matrV_dat[4*4];
 111
 112   //matrU = cvMat(6,6,CV_64F,matrU_dat);
 113   matrW = cvMat(6,4,CV_64F,matrW_dat);
 114   matrV = cvMat(4,4,CV_64F,matrV_dat);
 115
 116   CvMat* projPoints[2];
 117   CvMat* projMatrs[2];
 118
 119   projPoints[0] = projPoints1;
 120   projPoints[1] = projPoints2;
 121
 122   projMatrs[0] = projMatr1;
 123   projMatrs[1] = projMatr2;
 124
 125   /* Solve system for each point */
 126   int i,j;
 127   for( i = 0; i < numPoints; i++ )/* For each point */
 128     {
 129       /* Fill matrix for current point */
 130       for( j = 0; j < 2; j++ )/* For each view */
 131         {
 132           double x,y;
 133           x = cvmGet(projPoints[j],0,i);
 134           y = cvmGet(projPoints[j],1,i);
 135           for( int k = 0; k < 4; k++ )
 136             {
 137               cvmSet(&matrA, j*3+0, k, x * cvmGet(projMatrs[j],2,k) -     cvmGet(projMatrs[j],0,k) );
 138               cvmSet(&matrA, j*3+1, k, y * cvmGet(projMatrs[j],2,k) -     cvmGet(projMatrs[j],1,k) );
 139               cvmSet(&matrA, j*3+2, k, x * cvmGet(projMatrs[j],1,k) - y * cvmGet(projMatrs[j],0,k) );
 140             }
 141         }
 142       /* Solve system for current point */
 143       {
 144         cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
 145
 146         /* Copy computed point */
 147         cvmSet(points4D,0,i,cvmGet(&matrV,3,0));/* X */
 148         cvmSet(points4D,1,i,cvmGet(&matrV,3,1));/* Y */
 149         cvmSet(points4D,2,i,cvmGet(&matrV,3,2));/* Z */
 150         cvmSet(points4D,3,i,cvmGet(&matrV,3,3));/* W */
 151       }
 152     }
 153
 154   /* Points was reconstructed. Try to reproject points */
 155   /* We can compute reprojection error if need */
 156   {
 157     int i;
 158     CvMat point3D;
 159     double point3D_dat[4];
 160     point3D = cvMat(4,1,CV_64F,point3D_dat);
 161
 162     CvMat point2D;
 163     double point2D_dat[3];
 164     point2D = cvMat(3,1,CV_64F,point2D_dat);
 165
 166     for( i = 0; i < numPoints; i++ )
 167       {
 168         double W = cvmGet(points4D,3,i);
 169
 170         point3D_dat[0] = cvmGet(points4D,0,i)/W;
 171         point3D_dat[1] = cvmGet(points4D,1,i)/W;
 172         point3D_dat[2] = cvmGet(points4D,2,i)/W;
 173         point3D_dat[3] = 1;
 174
 175         /* !!! Project this point for each camera */
 176         for( int currCamera = 0; currCamera < 2; currCamera++ )
 177           {
 178             cvmMul(projMatrs[currCamera], &point3D, &point2D);
 179
 180             float x,y;
 181             float xr,yr,wr;
 182             x = (float)cvmGet(projPoints[currCamera],0,i);
 183             y = (float)cvmGet(projPoints[currCamera],1,i);
 184
 185             wr = (float)point2D_dat[2];
 186             xr = (float)(point2D_dat[0]/wr);
 187             yr = (float)(point2D_dat[1]/wr);
 188
 189             float deltaX,deltaY;
 190             deltaX = (float)fabs(x-xr);
 191             deltaY = (float)fabs(y-yr);
 192           }
 193       }
 194   }
 195
 196   __END__;
 197   return;
 198 }
 199
 200
 201 static void writePoint( double x, double y, CvMat* ptvec, int p )
 202 {
 203     uchar* ptr = ptvec->data.ptr;
 204     int depth = CV_MAT_DEPTH(ptvec->type);
 205     switch (depth) {
 206     case CV_64F:
 207         ((double*)ptr)[p*2] = x;
 208         ((double*)ptr)[p*2+1] = y;
 209         break;
 210     case CV_32F:
 211         ((float*)ptr)[p*2] = (float)x;
 212         ((float*)ptr)[p*2+1] = (float)y;
 213         break;
 214     case CV_32S:
 215         ((int*)ptr)[p*2] = cv::saturate_cast<int>(x);
 216         ((int*)ptr)[p*2+1] = cv::saturate_cast<int>(y);
 217         break;
 218     case CV_16U:
 219         ((ushort*)ptr)[p*2] = cv::saturate_cast<ushort>(x);
 220         ((ushort*)ptr)[p*2+1] = cv::saturate_cast<ushort>(y);
 221         break;
 222     case CV_16S:
 223         ((short*)ptr)[p*2] = cv::saturate_cast<short>(x);
 224         ((short*)ptr)[p*2+1] = cv::saturate_cast<short>(y);
 225         break;
 226     case CV_8S:
 227         ((schar*)ptr)[p*2] = cv::saturate_cast<schar>(x);
 228         ((schar*)ptr)[p*2+1] = cv::saturate_cast<schar>(y);
 229         break;
 230     case CV_8U:
 231         ((uchar*)ptr)[p*2] = cv::saturate_cast<uchar>(x);
 232         ((uchar*)ptr)[p*2+1] = cv::saturate_cast<uchar>(y);
 233         break;
 234     default:
 235         CV_Error(CV_StsUnsupportedFormat, "");
 236     }
 237 }
 238
 239 /*
 240  *      The Optimal Triangulation Method (see HZ for details)
 241  *              For each given point correspondence points1[i] <-> points2[i], and a fundamental matrix F,
 242  *              computes the corrected correspondences new_points1[i] <-> new_points2[i] that minimize the
 243  *              geometric error d(points1[i],new_points1[i])^2 + d(points2[i],new_points2[i])^2 (where d(a,b)
 244  *              is the geometric distance between points a and b) subject to the epipolar constraint
 245  *              new_points2' * F * new_points1 = 0.
 246  *
 247  *              F_                      :       3x3 fundamental matrix
 248  *              points1_        :       2xN matrix containing the first set of points
 249  *              points2_        :       2xN matrix containing the second set of points
 250  *              new_points1     :       the optimized points1_. if this is NULL, the corrected points are placed back in points1_
 251  *              new_points2     :       the optimized points2_. if this is NULL, the corrected points are placed back in points2_
 252  */
 253 CV_IMPL void
 254 cvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points1, CvMat *new_points2) {
 255   CvMat *tmp33 = NULL;
 256   CvMat *tmp31 = NULL, *tmp31_2 = NULL;
 257   CvMat *T1i = NULL, *T2i = NULL;
 258   CvMat *R1 = NULL, *R2 = NULL;
 259   CvMat *TFT = NULL, *TFTt = NULL, *RTFTR = NULL;
 260   CvMat *U = NULL, *S = NULL, *V = NULL;
 261   CvMat *e1 = NULL, *e2 = NULL;
 262   CvMat *polynomial = NULL;
 263   CvMat *result = NULL;
 264   CvMat *points1 = NULL, *points2 = NULL;
 265   CvMat *F = NULL;
 266   int F_type = -1, p1_type = -1, p2_type = -1, np1_type = -1, np2_type = -1;
 267
 268   CV_FUNCNAME( "cvCorrectMatches" );
 269   __BEGIN__;
 270
 271   if (!CV_IS_MAT(F_) || !CV_IS_MAT(points1_) || !CV_IS_MAT(points2_) )
 272     CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
 273   if (!( F_->cols == 3 && F_->rows == 3))
 274     CV_ERROR( CV_StsUnmatchedSizes, "The fundamental matrix must be a 3x3 matrix");
 275   if (!(((F_->type & CV_MAT_TYPE_MASK) >> 3) == 0 ))
 276     CV_ERROR( CV_StsUnsupportedFormat, "The fundamental matrix must be a single-channel matrix" );
 277   if (!(points1_->rows == 1 && points2_->rows == 1 && points1_->cols == points2_->cols))
 278     CV_ERROR( CV_StsUnmatchedSizes, "The point-matrices must have two rows, and an equal number of columns" );
 279   if (((points1_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
 280     CV_ERROR( CV_StsUnmatchedSizes, "The first set of points must contain two channels; one for x and one for y" );
 281   if (((points2_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
 282     CV_ERROR( CV_StsUnmatchedSizes, "The second set of points must contain two channels; one for x and one for y" );
 283   if (new_points1 != NULL && CV_IS_MAT(new_points1)) {
 284     if (new_points1->cols != points1_->cols || new_points1->rows != 1)
 285       CV_ERROR( CV_StsUnmatchedSizes, "The first output matrix must have the same dimensions as the input matrices" );
 286     if (((new_points1->type & CV_MAT_TYPE_MASK) >> 3) != 1)
 287       CV_ERROR( CV_StsUnsupportedFormat, "The first output matrix must have two channels; one for x and one for y" );
 288   }
 289   if (new_points2 != NULL && CV_IS_MAT(new_points2)) {
 290     if (new_points2->cols != points2_->cols || new_points2->rows != 1)
 291       CV_ERROR( CV_StsUnmatchedSizes, "The second output matrix must have the same dimensions as the input matrices" );
 292     if (((new_points2->type & CV_MAT_TYPE_MASK) >> 3) != 1)
 293       CV_ERROR( CV_StsUnsupportedFormat, "The second output matrix must have two channels; one for x and one for y" );
 294   }
 295
 296   // Make sure F uses double precision
 297   F_type = ((F_->type & CV_MAT_TYPE_MASK) & 0x07); // 0b111
 298   if (F_type != 6) {
 299     F = cvCreateMat(3,3,CV_64FC1);
 300     switch (F_type) {
 301     case 5: for (int i = 0; i < 9; ++i) F->data.db[i] = F_->data.fl[i]; break;
 302     case 4: for (int i = 0; i < 9; ++i) F->data.db[i] = F_->data.i[i]; break;
 303     case 3:
 304     case 2: for (int i = 0; i < 9; ++i) F->data.db[i] = F_->data.s[i]; break;
 305     case 1:
 306     case 0: for (int i = 0; i < 9; ++i) F->data.db[i] = F_->data.ptr[i]; break;
 307     }
 308   }
 309   else F = F_;
 310
 311   // Make sure points1 uses double precision
 312   p1_type = ((points1_->type & CV_MAT_TYPE_MASK) & 0x07); // 0b111
 313   points1 = points1_;
 314   if (p1_type != 6) {
 315     points1 = cvCreateMat(1,points1_->cols,CV_64FC2);
 316     switch (p1_type) {
 317     case 5: for (int i = 0; i < 2*points1_->cols; ++i) points1->data.db[i] = points1_->data.fl[i]; break;
 318     case 4: for (int i = 0; i < 2*points1_->cols; ++i) points1->data.db[i] = points1_->data.i[i]; break;
 319     case 3:
 320     case 2: for (int i = 0; i < 2*points1_->cols; ++i) points1->data.db[i] = points1_->data.s[i]; break;
 321     case 1:
 322     case 0: for (int i = 0; i < 2*points1_->cols; ++i) points1->data.db[i] = points1_->data.ptr[i]; break;
 323     }
 324   }
 325
 326   // Make sure points2 uses double precision
 327   p2_type = ((points2_->type & CV_MAT_TYPE_MASK) & 0x07); // 0b111
 328   points2 = points2_;
 329   if (p2_type != 6) {
 330     points2 = cvCreateMat(1,points2_->cols,CV_64FC2);
 331     switch (p2_type) {
 332     case 5: for (int i = 0; i < 2*points2_->cols; ++i) points2->data.db[i] = points2_->data.fl[i]; break;
 333     case 4: for (int i = 0; i < 2*points2_->cols; ++i) points2->data.db[i] = points2_->data.i[i]; break;
 334     case 3:
 335     case 2: for (int i = 0; i < 2*points2_->cols; ++i) points2->data.db[i] = points2_->data.s[i]; break;
 336     case 1:
 337     case 0: for (int i = 0; i < 2*points2_->cols; ++i) points2->data.db[i] = points2_->data.ptr[i]; break;
 338     }
 339   }
 340
 341   tmp33 = cvCreateMat(3,3,CV_64FC1);
 342   tmp31 = cvCreateMat(3,1,CV_64FC1), tmp31_2 = cvCreateMat(3,1,CV_64FC1);
 343   T1i = cvCreateMat(3,3,CV_64FC1), T2i = cvCreateMat(3,3,CV_64FC1);
 344   R1 = cvCreateMat(3,3,CV_64FC1), R2 = cvCreateMat(3,3,CV_64FC1);
 345   TFT = cvCreateMat(3,3,CV_64FC1), TFTt = cvCreateMat(3,3,CV_64FC1), RTFTR = cvCreateMat(3,3,CV_64FC1);
 346   U = cvCreateMat(3,3,CV_64FC1);
 347   S = cvCreateMat(3,3,CV_64FC1);
 348   V = cvCreateMat(3,3,CV_64FC1);
 349   e1 = cvCreateMat(3,1,CV_64FC1), e2 = cvCreateMat(3,1,CV_64FC1);
 350   if (new_points1 != NULL) np1_type = ((new_points1->type & CV_MAT_TYPE_MASK) & 0x07); // 0b111
 351   if (new_points2 != NULL) np2_type = ((new_points2->type & CV_MAT_TYPE_MASK) & 0x07); // 0b111
 352
 353   double x1, y1, x2, y2;
 354   double scale;
 355   double f1, f2, a, b, c, d;
 356   polynomial = cvCreateMat(1,7,CV_64FC1);
 357   result = cvCreateMat(1,6,CV_64FC2);
 358   double t_min, s_val, t, s;
 359   for (int p = 0; p < points1->cols; ++p) {
 360     // Replace F by T2-t * F * T1-t
 361     x1 = points1->data.db[p*2];
 362     y1 = points1->data.db[p*2+1];
 363     x2 = points2->data.db[p*2];
 364     y2 = points2->data.db[p*2+1];
 365
 366     cvSetZero(T1i);
 367     cvSetReal2D(T1i,0,0,1);
 368     cvSetReal2D(T1i,1,1,1);
 369     cvSetReal2D(T1i,2,2,1);
 370     cvSetReal2D(T1i,0,2,x1);
 371     cvSetReal2D(T1i,1,2,y1);
 372     cvSetZero(T2i);
 373     cvSetReal2D(T2i,0,0,1);
 374     cvSetReal2D(T2i,1,1,1);
 375     cvSetReal2D(T2i,2,2,1);
 376     cvSetReal2D(T2i,0,2,x2);
 377     cvSetReal2D(T2i,1,2,y2);
 378     cvGEMM(T2i,F,1,0,0,tmp33,CV_GEMM_A_T);
 379     cvSetZero(TFT);
 380     cvGEMM(tmp33,T1i,1,0,0,TFT);
 381
 382     // Compute the right epipole e1 from F * e1 = 0
 383     cvSetZero(U);
 384     cvSetZero(S);
 385     cvSetZero(V);
 386     cvSVD(TFT,S,U,V);
 387     scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
 388     cvSetReal2D(e1,0,0,cvGetReal2D(V,0,2)/scale);
 389     cvSetReal2D(e1,1,0,cvGetReal2D(V,1,2)/scale);
 390     cvSetReal2D(e1,2,0,cvGetReal2D(V,2,2)/scale);
 391     if (cvGetReal2D(e1,2,0) < 0) {
 392       cvSetReal2D(e1,0,0,-cvGetReal2D(e1,0,0));
 393       cvSetReal2D(e1,1,0,-cvGetReal2D(e1,1,0));
 394       cvSetReal2D(e1,2,0,-cvGetReal2D(e1,2,0));
 395     }
 396
 397     // Compute the left epipole e2 from e2' * F = 0  =>  F' * e2 = 0
 398     cvSetZero(TFTt);
 399     cvTranspose(TFT, TFTt);
 400     cvSetZero(U);
 401     cvSetZero(S);
 402     cvSetZero(V);
 403     cvSVD(TFTt,S,U,V);
 404     cvSetZero(e2);
 405     scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
 406     cvSetReal2D(e2,0,0,cvGetReal2D(V,0,2)/scale);
 407     cvSetReal2D(e2,1,0,cvGetReal2D(V,1,2)/scale);
 408     cvSetReal2D(e2,2,0,cvGetReal2D(V,2,2)/scale);
 409     if (cvGetReal2D(e2,2,0) < 0) {
 410       cvSetReal2D(e2,0,0,-cvGetReal2D(e2,0,0));
 411       cvSetReal2D(e2,1,0,-cvGetReal2D(e2,1,0));
 412       cvSetReal2D(e2,2,0,-cvGetReal2D(e2,2,0));
 413     }
 414
 415     // Replace F by R2 * F * R1'
 416     cvSetZero(R1);
 417     cvSetReal2D(R1,0,0,cvGetReal2D(e1,0,0));
 418     cvSetReal2D(R1,0,1,cvGetReal2D(e1,1,0));
 419     cvSetReal2D(R1,1,0,-cvGetReal2D(e1,1,0));
 420     cvSetReal2D(R1,1,1,cvGetReal2D(e1,0,0));
 421     cvSetReal2D(R1,2,2,1);
 422     cvSetZero(R2);
 423     cvSetReal2D(R2,0,0,cvGetReal2D(e2,0,0));
 424     cvSetReal2D(R2,0,1,cvGetReal2D(e2,1,0));
 425     cvSetReal2D(R2,1,0,-cvGetReal2D(e2,1,0));
 426     cvSetReal2D(R2,1,1,cvGetReal2D(e2,0,0));
 427     cvSetReal2D(R2,2,2,1);
 428     cvGEMM(R2,TFT,1,0,0,tmp33);
 429     cvGEMM(tmp33,R1,1,0,0,RTFTR,CV_GEMM_B_T);
 430
 431     // Set f1 = e1(3), f2 = e2(3), a = F22, b = F23, c = F32, d = F33
 432     f1 = cvGetReal2D(e1,2,0);
 433     f2 = cvGetReal2D(e2,2,0);
 434     a = cvGetReal2D(RTFTR,1,1);
 435     b = cvGetReal2D(RTFTR,1,2);
 436     c = cvGetReal2D(RTFTR,2,1);
 437     d = cvGetReal2D(RTFTR,2,2);
 438
 439     // Form the polynomial g(t) = k6*t⁶ + k5*t⁵ + k4*t⁴ + k3*t³ + k2*t² + k1*t + k0
 440     // from f1, f2, a, b, c and d
 441     cvSetReal2D(polynomial,0,6,( +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c ));
 442     cvSetReal2D(polynomial,0,5,( +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a ));
 443     cvSetReal2D(polynomial,0,4,( +4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d ));
 444     cvSetReal2D(polynomial,0,3,( +6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d ));
 445     cvSetReal2D(polynomial,0,2,( +4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d ));
 446     cvSetReal2D(polynomial,0,1,( +f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c ));
 447     cvSetReal2D(polynomial,0,0,( -a*d*d*b+b*b*c*d ));
 448
 449     // Solve g(t) for t to get 6 roots
 450     cvSetZero(result);
 451     cvSolvePoly(polynomial, result, 100, 20);
 452
 453     // Evaluate the cost function s(t) at the real part of the 6 roots
 454     t_min = DBL_MAX;
 455     s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c);
 456     for (int ti = 0; ti < 6; ++ti) {
 457       t = result->data.db[2*ti];
 458       s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d));
 459       if (s < s_val) {
 460         s_val = s;
 461         t_min = t;
 462       }
 463     }
 464
 465     // find the optimal x1 and y1 as the points on l1 and l2 closest to the origin
 466     tmp31->data.db[0] = t_min*t_min*f1;
 467     tmp31->data.db[1] = t_min;
 468     tmp31->data.db[2] = t_min*t_min*f1*f1+1;
 469     tmp31->data.db[0] /= tmp31->data.db[2];
 470     tmp31->data.db[1] /= tmp31->data.db[2];
 471     tmp31->data.db[2] /= tmp31->data.db[2];
 472     cvGEMM(T1i,R1,1,0,0,tmp33,CV_GEMM_B_T);
 473     cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
 474     x1 = tmp31_2->data.db[0];
 475     y1 = tmp31_2->data.db[1];
 476
 477     tmp31->data.db[0] = f2*pow(c*t_min+d,2);
 478     tmp31->data.db[1] = -(a*t_min+b)*(c*t_min+d);
 479     tmp31->data.db[2] = f2*f2*pow(c*t_min+d,2) + pow(a*t_min+b,2);
 480     tmp31->data.db[0] /= tmp31->data.db[2];
 481     tmp31->data.db[1] /= tmp31->data.db[2];
 482     tmp31->data.db[2] /= tmp31->data.db[2];
 483     cvGEMM(T2i,R2,1,0,0,tmp33,CV_GEMM_B_T);
 484     cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
 485     x2 = tmp31_2->data.db[0];
 486     y2 = tmp31_2->data.db[1];
 487
 488     // Return the points in the matrix format that the user wants
 489     writePoint(x1, y1, new_points1 ? new_points1 : points1_, p);
 490     writePoint(x2, y2, new_points2 ? new_points2 : points2_, p);
 491   }
 492
 493   cvReleaseMat(&e1);
 494   cvReleaseMat(&e2);
 495   cvReleaseMat(&T1i);
 496   cvReleaseMat(&T2i);
 497   cvReleaseMat(&R1);
 498   cvReleaseMat(&R2);
 499   cvReleaseMat(&TFT);
 500   cvReleaseMat(&TFTt);
 501   cvReleaseMat(&RTFTR);
 502   cvReleaseMat(&U);
 503   cvReleaseMat(&S);
 504   cvReleaseMat(&V);
 505   cvReleaseMat(&tmp33);
 506   cvReleaseMat(&tmp31);
 507   cvReleaseMat(&tmp31_2);
 508
 509   // release only if we created new ones with higher precision
 510   if (F_ != F) cvReleaseMat(&F);
 511   if (points1 != points1_) cvReleaseMat(&points1);
 512   if (points2 != points2_) cvReleaseMat(&points2);
 513
 514   __END__;
 515 }