modules/calib3d/src/triangulate.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 "precomp.hpp"
  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     if( projMatr1 == 0 || projMatr2 == 0 ||
  56       projPoints1 == 0 || projPoints2 == 0 ||
  57       points4D == 0)
  58       CV_Error( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
  59
  60     if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) ||
  61       !CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) ||
  62       !CV_IS_MAT(points4D) )
  63       CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
  64
  65     int numPoints;
  66     numPoints = projPoints1->cols;
  67
  68     if( numPoints < 1 )
  69         CV_Error( CV_StsOutOfRange, "Number of points must be more than zero" );
  70
  71     if( projPoints2->cols != numPoints || points4D->cols != numPoints )
  72         CV_Error( CV_StsUnmatchedSizes, "Number of points must be the same" );
  73
  74     if( projPoints1->rows != 2 || projPoints2->rows != 2)
  75         CV_Error( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
  76
  77     if( points4D->rows != 4 )
  78         CV_Error( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
  79
  80     if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
  81        projMatr2->cols != 4 || projMatr2->rows != 3)
  82         CV_Error( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
  83
  84     CvMat matrA;
  85     double matrA_dat[24];
  86     matrA = cvMat(6,4,CV_64F,matrA_dat);
  87
  88     //CvMat matrU;
  89     CvMat matrW;
  90     CvMat matrV;
  91     //double matrU_dat[9*9];
  92     double matrW_dat[6*4];
  93     double matrV_dat[4*4];
  94
  95     //matrU = cvMat(6,6,CV_64F,matrU_dat);
  96     matrW = cvMat(6,4,CV_64F,matrW_dat);
  97     matrV = cvMat(4,4,CV_64F,matrV_dat);
  98
  99     CvMat* projPoints[2];
 100     CvMat* projMatrs[2];
 101
 102     projPoints[0] = projPoints1;
 103     projPoints[1] = projPoints2;
 104
 105     projMatrs[0] = projMatr1;
 106     projMatrs[1] = projMatr2;
 107
 108     /* Solve system for each point */
 109     int i,j;
 110     for( i = 0; i < numPoints; i++ )/* For each point */
 111     {
 112         /* Fill matrix for current point */
 113         for( j = 0; j < 2; j++ )/* For each view */
 114         {
 115             double x,y;
 116             x = cvmGet(projPoints[j],0,i);
 117             y = cvmGet(projPoints[j],1,i);
 118             for( int k = 0; k < 4; k++ )
 119             {
 120                 cvmSet(&matrA, j*3+0, k, x * cvmGet(projMatrs[j],2,k) -     cvmGet(projMatrs[j],0,k) );
 121                 cvmSet(&matrA, j*3+1, k, y * cvmGet(projMatrs[j],2,k) -     cvmGet(projMatrs[j],1,k) );
 122                 cvmSet(&matrA, j*3+2, k, x * cvmGet(projMatrs[j],1,k) - y * cvmGet(projMatrs[j],0,k) );
 123             }
 124         }
 125         /* Solve system for current point */
 126         {
 127             cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
 128
 129             /* Copy computed point */
 130             cvmSet(points4D,0,i,cvmGet(&matrV,3,0));/* X */
 131             cvmSet(points4D,1,i,cvmGet(&matrV,3,1));/* Y */
 132             cvmSet(points4D,2,i,cvmGet(&matrV,3,2));/* Z */
 133             cvmSet(points4D,3,i,cvmGet(&matrV,3,3));/* W */
 134         }
 135     }
 136
 137 #if 0
 138     double err = 0;
 139     /* Points was reconstructed. Try to reproject points */
 140     /* We can compute reprojection error if need */
 141     {
 142         int i;
 143         CvMat point3D;
 144         double point3D_dat[4];
 145         point3D = cvMat(4,1,CV_64F,point3D_dat);
 146
 147         CvMat point2D;
 148         double point2D_dat[3];
 149         point2D = cvMat(3,1,CV_64F,point2D_dat);
 150
 151         for( i = 0; i < numPoints; i++ )
 152         {
 153             double W = cvmGet(points4D,3,i);
 154
 155             point3D_dat[0] = cvmGet(points4D,0,i)/W;
 156             point3D_dat[1] = cvmGet(points4D,1,i)/W;
 157             point3D_dat[2] = cvmGet(points4D,2,i)/W;
 158             point3D_dat[3] = 1;
 159
 160             /* !!! Project this point for each camera */
 161             for( int currCamera = 0; currCamera < 2; currCamera++ )
 162             {
 163                 cvMatMul(projMatrs[currCamera], &point3D, &point2D);
 164
 165                 float x,y;
 166                 float xr,yr,wr;
 167                 x = (float)cvmGet(projPoints[currCamera],0,i);
 168                 y = (float)cvmGet(projPoints[currCamera],1,i);
 169
 170                 wr = (float)point2D_dat[2];
 171                 xr = (float)(point2D_dat[0]/wr);
 172                 yr = (float)(point2D_dat[1]/wr);
 173
 174                 float deltaX,deltaY;
 175                 deltaX = (float)fabs(x-xr);
 176                 deltaY = (float)fabs(y-yr);
 177                 err += deltaX*deltaX + deltaY*deltaY;
 178             }
 179         }
 180     }
 181 #endif
 182 }
 183
 184
 185 /*
 186  *      The Optimal Triangulation Method (see HZ for details)
 187  *              For each given point correspondence points1[i] <-> points2[i], and a fundamental matrix F,
 188  *              computes the corrected correspondences new_points1[i] <-> new_points2[i] that minimize the
 189  *              geometric error d(points1[i],new_points1[i])^2 + d(points2[i],new_points2[i])^2 (where d(a,b)
 190  *              is the geometric distance between points a and b) subject to the epipolar constraint
 191  *              new_points2' * F * new_points1 = 0.
 192  *
 193  *              F_                      :       3x3 fundamental matrix
 194  *              points1_        :       2xN matrix containing the first set of points
 195  *              points2_        :       2xN matrix containing the second set of points
 196  *              new_points1     :       the optimized points1_. if this is NULL, the corrected points are placed back in points1_
 197  *              new_points2     :       the optimized points2_. if this is NULL, the corrected points are placed back in points2_
 198  */
 199 CV_IMPL void
 200 cvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points1, CvMat *new_points2)
 201 {
 202     cv::Ptr<CvMat> tmp33;
 203     cv::Ptr<CvMat> tmp31, tmp31_2;
 204     cv::Ptr<CvMat> T1i, T2i;
 205     cv::Ptr<CvMat> R1, R2;
 206     cv::Ptr<CvMat> TFT, TFTt, RTFTR;
 207     cv::Ptr<CvMat> U, S, V;
 208     cv::Ptr<CvMat> e1, e2;
 209     cv::Ptr<CvMat> polynomial;
 210     cv::Ptr<CvMat> result;
 211     cv::Ptr<CvMat> points1, points2;
 212     cv::Ptr<CvMat> F;
 213
 214     if (!CV_IS_MAT(F_) || !CV_IS_MAT(points1_) || !CV_IS_MAT(points2_) )
 215         CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
 216     if (!( F_->cols == 3 && F_->rows == 3))
 217         CV_Error( CV_StsUnmatchedSizes, "The fundamental matrix must be a 3x3 matrix");
 218     if (!(((F_->type & CV_MAT_TYPE_MASK) >> 3) == 0 ))
 219         CV_Error( CV_StsUnsupportedFormat, "The fundamental matrix must be a single-channel matrix" );
 220     if (!(points1_->rows == 1 && points2_->rows == 1 && points1_->cols == points2_->cols))
 221         CV_Error( CV_StsUnmatchedSizes, "The point-matrices must have two rows, and an equal number of columns" );
 222     if (((points1_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
 223         CV_Error( CV_StsUnmatchedSizes, "The first set of points must contain two channels; one for x and one for y" );
 224     if (((points2_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
 225         CV_Error( CV_StsUnmatchedSizes, "The second set of points must contain two channels; one for x and one for y" );
 226     if (new_points1 != NULL) {
 227         CV_Assert(CV_IS_MAT(new_points1));
 228         if (new_points1->cols != points1_->cols || new_points1->rows != 1)
 229             CV_Error( CV_StsUnmatchedSizes, "The first output matrix must have the same dimensions as the input matrices" );
 230         if (CV_MAT_CN(new_points1->type) != 2)
 231             CV_Error( CV_StsUnsupportedFormat, "The first output matrix must have two channels; one for x and one for y" );
 232     }
 233     if (new_points2 != NULL) {
 234         CV_Assert(CV_IS_MAT(new_points2));
 235         if (new_points2->cols != points2_->cols || new_points2->rows != 1)
 236             CV_Error( CV_StsUnmatchedSizes, "The second output matrix must have the same dimensions as the input matrices" );
 237         if (CV_MAT_CN(new_points2->type) != 2)
 238             CV_Error( CV_StsUnsupportedFormat, "The second output matrix must have two channels; one for x and one for y" );
 239     }
 240
 241     // Make sure F uses double precision
 242     F = cvCreateMat(3,3,CV_64FC1);
 243     cvConvert(F_, F);
 244
 245     // Make sure points1 uses double precision
 246     points1 = cvCreateMat(points1_->rows,points1_->cols,CV_64FC2);
 247     cvConvert(points1_, points1);
 248
 249     // Make sure points2 uses double precision
 250     points2 = cvCreateMat(points2_->rows,points2_->cols,CV_64FC2);
 251     cvConvert(points2_, points2);
 252
 253     tmp33 = cvCreateMat(3,3,CV_64FC1);
 254     tmp31 = cvCreateMat(3,1,CV_64FC1), tmp31_2 = cvCreateMat(3,1,CV_64FC1);
 255     T1i = cvCreateMat(3,3,CV_64FC1), T2i = cvCreateMat(3,3,CV_64FC1);
 256     R1 = cvCreateMat(3,3,CV_64FC1), R2 = cvCreateMat(3,3,CV_64FC1);
 257     TFT = cvCreateMat(3,3,CV_64FC1), TFTt = cvCreateMat(3,3,CV_64FC1), RTFTR = cvCreateMat(3,3,CV_64FC1);
 258     U = cvCreateMat(3,3,CV_64FC1);
 259     S = cvCreateMat(3,3,CV_64FC1);
 260     V = cvCreateMat(3,3,CV_64FC1);
 261     e1 = cvCreateMat(3,1,CV_64FC1), e2 = cvCreateMat(3,1,CV_64FC1);
 262
 263     double x1, y1, x2, y2;
 264     double scale;
 265     double f1, f2, a, b, c, d;
 266     polynomial = cvCreateMat(1,7,CV_64FC1);
 267     result = cvCreateMat(1,6,CV_64FC2);
 268     double t_min, s_val, t, s;
 269     for (int p = 0; p < points1->cols; ++p) {
 270         // Replace F by T2-t * F * T1-t
 271         x1 = points1->data.db[p*2];
 272         y1 = points1->data.db[p*2+1];
 273         x2 = points2->data.db[p*2];
 274         y2 = points2->data.db[p*2+1];
 275
 276         cvSetZero(T1i);
 277         cvSetReal2D(T1i,0,0,1);
 278         cvSetReal2D(T1i,1,1,1);
 279         cvSetReal2D(T1i,2,2,1);
 280         cvSetReal2D(T1i,0,2,x1);
 281         cvSetReal2D(T1i,1,2,y1);
 282         cvSetZero(T2i);
 283         cvSetReal2D(T2i,0,0,1);
 284         cvSetReal2D(T2i,1,1,1);
 285         cvSetReal2D(T2i,2,2,1);
 286         cvSetReal2D(T2i,0,2,x2);
 287         cvSetReal2D(T2i,1,2,y2);
 288         cvGEMM(T2i,F,1,0,0,tmp33,CV_GEMM_A_T);
 289         cvSetZero(TFT);
 290         cvGEMM(tmp33,T1i,1,0,0,TFT);
 291
 292         // Compute the right epipole e1 from F * e1 = 0
 293         cvSetZero(U);
 294         cvSetZero(S);
 295         cvSetZero(V);
 296         cvSVD(TFT,S,U,V);
 297         scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
 298         cvSetReal2D(e1,0,0,cvGetReal2D(V,0,2)/scale);
 299         cvSetReal2D(e1,1,0,cvGetReal2D(V,1,2)/scale);
 300         cvSetReal2D(e1,2,0,cvGetReal2D(V,2,2)/scale);
 301         if (cvGetReal2D(e1,2,0) < 0) {
 302             cvSetReal2D(e1,0,0,-cvGetReal2D(e1,0,0));
 303             cvSetReal2D(e1,1,0,-cvGetReal2D(e1,1,0));
 304             cvSetReal2D(e1,2,0,-cvGetReal2D(e1,2,0));
 305         }
 306
 307         // Compute the left epipole e2 from e2' * F = 0  =>  F' * e2 = 0
 308         cvSetZero(TFTt);
 309         cvTranspose(TFT, TFTt);
 310         cvSetZero(U);
 311         cvSetZero(S);
 312         cvSetZero(V);
 313         cvSVD(TFTt,S,U,V);
 314         cvSetZero(e2);
 315         scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
 316         cvSetReal2D(e2,0,0,cvGetReal2D(V,0,2)/scale);
 317         cvSetReal2D(e2,1,0,cvGetReal2D(V,1,2)/scale);
 318         cvSetReal2D(e2,2,0,cvGetReal2D(V,2,2)/scale);
 319         if (cvGetReal2D(e2,2,0) < 0) {
 320             cvSetReal2D(e2,0,0,-cvGetReal2D(e2,0,0));
 321             cvSetReal2D(e2,1,0,-cvGetReal2D(e2,1,0));
 322             cvSetReal2D(e2,2,0,-cvGetReal2D(e2,2,0));
 323         }
 324
 325         // Replace F by R2 * F * R1'
 326         cvSetZero(R1);
 327         cvSetReal2D(R1,0,0,cvGetReal2D(e1,0,0));
 328         cvSetReal2D(R1,0,1,cvGetReal2D(e1,1,0));
 329         cvSetReal2D(R1,1,0,-cvGetReal2D(e1,1,0));
 330         cvSetReal2D(R1,1,1,cvGetReal2D(e1,0,0));
 331         cvSetReal2D(R1,2,2,1);
 332         cvSetZero(R2);
 333         cvSetReal2D(R2,0,0,cvGetReal2D(e2,0,0));
 334         cvSetReal2D(R2,0,1,cvGetReal2D(e2,1,0));
 335         cvSetReal2D(R2,1,0,-cvGetReal2D(e2,1,0));
 336         cvSetReal2D(R2,1,1,cvGetReal2D(e2,0,0));
 337         cvSetReal2D(R2,2,2,1);
 338         cvGEMM(R2,TFT,1,0,0,tmp33);
 339         cvGEMM(tmp33,R1,1,0,0,RTFTR,CV_GEMM_B_T);
 340
 341         // Set f1 = e1(3), f2 = e2(3), a = F22, b = F23, c = F32, d = F33
 342         f1 = cvGetReal2D(e1,2,0);
 343         f2 = cvGetReal2D(e2,2,0);
 344         a = cvGetReal2D(RTFTR,1,1);
 345         b = cvGetReal2D(RTFTR,1,2);
 346         c = cvGetReal2D(RTFTR,2,1);
 347         d = cvGetReal2D(RTFTR,2,2);
 348
 349         // Form the polynomial g(t) = k6*t⁶ + k5*t⁵ + k4*t⁴ + k3*t³ + k2*t² + k1*t + k0
 350         // from f1, f2, a, b, c and d
 351         cvSetReal2D(polynomial,0,6,( +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c ));
 352         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 ));
 353         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 ));
 354         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 ));
 355         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 ));
 356         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 ));
 357         cvSetReal2D(polynomial,0,0,( -a*d*d*b+b*b*c*d ));
 358
 359         // Solve g(t) for t to get 6 roots
 360         cvSetZero(result);
 361         cvSolvePoly(polynomial, result, 100, 20);
 362
 363         // Evaluate the cost function s(t) at the real part of the 6 roots
 364         t_min = DBL_MAX;
 365         s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c);
 366         for (int ti = 0; ti < 6; ++ti) {
 367             t = result->data.db[2*ti];
 368             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));
 369             if (s < s_val) {
 370                 s_val = s;
 371                 t_min = t;
 372             }
 373         }
 374
 375         // find the optimal x1 and y1 as the points on l1 and l2 closest to the origin
 376         tmp31->data.db[0] = t_min*t_min*f1;
 377         tmp31->data.db[1] = t_min;
 378         tmp31->data.db[2] = t_min*t_min*f1*f1+1;
 379         tmp31->data.db[0] /= tmp31->data.db[2];
 380         tmp31->data.db[1] /= tmp31->data.db[2];
 381         tmp31->data.db[2] /= tmp31->data.db[2];
 382         cvGEMM(T1i,R1,1,0,0,tmp33,CV_GEMM_B_T);
 383         cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
 384         x1 = tmp31_2->data.db[0];
 385         y1 = tmp31_2->data.db[1];
 386
 387         tmp31->data.db[0] = f2*pow(c*t_min+d,2);
 388         tmp31->data.db[1] = -(a*t_min+b)*(c*t_min+d);
 389         tmp31->data.db[2] = f2*f2*pow(c*t_min+d,2) + pow(a*t_min+b,2);
 390         tmp31->data.db[0] /= tmp31->data.db[2];
 391         tmp31->data.db[1] /= tmp31->data.db[2];
 392         tmp31->data.db[2] /= tmp31->data.db[2];
 393         cvGEMM(T2i,R2,1,0,0,tmp33,CV_GEMM_B_T);
 394         cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
 395         x2 = tmp31_2->data.db[0];
 396         y2 = tmp31_2->data.db[1];
 397
 398         // Return the points in the matrix format that the user wants
 399         points1->data.db[p*2] = x1;
 400         points1->data.db[p*2+1] = y1;
 401         points2->data.db[p*2] = x2;
 402         points2->data.db[p*2+1] = y2;
 403     }
 404
 405     if( new_points1 )
 406         cvConvert( points1, new_points1 );
 407     if( new_points2 )
 408         cvConvert( points2, new_points2 );
 409 }