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