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 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000, Intel Corporation, all rights reserved. 14 // Copyright (C) 2013, OpenCV Foundation, all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 #include "precomp.hpp" 44 #include "rho.h" 45 #include <iostream> 46 47 namespace cv 48 { 49 50 static bool haveCollinearPoints( const Mat& m, int count ) 51 { 52 int j, k, i = count-1; 53 const Point2f* ptr = m.ptr<Point2f>(); 54 55 // check that the i-th selected point does not belong 56 // to a line connecting some previously selected points 57 for( j = 0; j < i; j++ ) 58 { 59 double dx1 = ptr[j].x - ptr[i].x; 60 double dy1 = ptr[j].y - ptr[i].y; 61 for( k = 0; k < j; k++ ) 62 { 63 double dx2 = ptr[k].x - ptr[i].x; 64 double dy2 = ptr[k].y - ptr[i].y; 65 if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2))) 66 return true; 67 } 68 } 69 return false; 70 } 71 72 73 class HomographyEstimatorCallback : public PointSetRegistrator::Callback 74 { 75 public: 76 bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const 77 { 78 Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat(); 79 if( haveCollinearPoints(ms1, count) || haveCollinearPoints(ms2, count) ) 80 return false; 81 82 // We check whether the minimal set of points for the homography estimation 83 // are geometrically consistent. We check if every 3 correspondences sets 84 // fulfills the constraint. 85 // 86 // The usefullness of this constraint is explained in the paper: 87 // 88 // "Speeding-up homography estimation in mobile devices" 89 // Journal of Real-Time Image Processing. 2013. DOI: 10.1007/s11554-012-0314-1 90 // Pablo Marquez-Neila, Javier Lopez-Alberca, Jose M. Buenaposada, Luis Baumela 91 if( count == 4 ) 92 { 93 static const int tt[][3] = {{0, 1, 2}, {1, 2, 3}, {0, 2, 3}, {0, 1, 3}}; 94 const Point2f* src = ms1.ptr<Point2f>(); 95 const Point2f* dst = ms2.ptr<Point2f>(); 96 int negative = 0; 97 98 for( int i = 0; i < 4; i++ ) 99 { 100 const int* t = tt[i]; 101 Matx33d A(src[t[0]].x, src[t[0]].y, 1., src[t[1]].x, src[t[1]].y, 1., src[t[2]].x, src[t[2]].y, 1.); 102 Matx33d B(dst[t[0]].x, dst[t[0]].y, 1., dst[t[1]].x, dst[t[1]].y, 1., dst[t[2]].x, dst[t[2]].y, 1.); 103 104 negative += determinant(A)*determinant(B) < 0; 105 } 106 if( negative != 0 && negative != 4 ) 107 return false; 108 } 109 110 return true; 111 } 112 113 int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const 114 { 115 Mat m1 = _m1.getMat(), m2 = _m2.getMat(); 116 int i, count = m1.checkVector(2); 117 const Point2f* M = m1.ptr<Point2f>(); 118 const Point2f* m = m2.ptr<Point2f>(); 119 120 double LtL[9][9], W[9][1], V[9][9]; 121 Mat _LtL( 9, 9, CV_64F, &LtL[0][0] ); 122 Mat matW( 9, 1, CV_64F, W ); 123 Mat matV( 9, 9, CV_64F, V ); 124 Mat _H0( 3, 3, CV_64F, V[8] ); 125 Mat _Htemp( 3, 3, CV_64F, V[7] ); 126 Point2d cM(0,0), cm(0,0), sM(0,0), sm(0,0); 127 128 for( i = 0; i < count; i++ ) 129 { 130 cm.x += m[i].x; cm.y += m[i].y; 131 cM.x += M[i].x; cM.y += M[i].y; 132 } 133 134 cm.x /= count; 135 cm.y /= count; 136 cM.x /= count; 137 cM.y /= count; 138 139 for( i = 0; i < count; i++ ) 140 { 141 sm.x += fabs(m[i].x - cm.x); 142 sm.y += fabs(m[i].y - cm.y); 143 sM.x += fabs(M[i].x - cM.x); 144 sM.y += fabs(M[i].y - cM.y); 145 } 146 147 if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON || 148 fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON ) 149 return 0; 150 sm.x = count/sm.x; sm.y = count/sm.y; 151 sM.x = count/sM.x; sM.y = count/sM.y; 152 153 double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 }; 154 double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 }; 155 Mat _invHnorm( 3, 3, CV_64FC1, invHnorm ); 156 Mat _Hnorm2( 3, 3, CV_64FC1, Hnorm2 ); 157 158 _LtL.setTo(Scalar::all(0)); 159 for( i = 0; i < count; i++ ) 160 { 161 double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y; 162 double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y; 163 double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x }; 164 double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y }; 165 int j, k; 166 for( j = 0; j < 9; j++ ) 167 for( k = j; k < 9; k++ ) 168 LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k]; 169 } 170 completeSymm( _LtL ); 171 172 eigen( _LtL, matW, matV ); 173 _Htemp = _invHnorm*_H0; 174 _H0 = _Htemp*_Hnorm2; 175 _H0.convertTo(_model, _H0.type(), 1./_H0.at<double>(2,2) ); 176 177 return 1; 178 } 179 180 void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const 181 { 182 Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat(); 183 int i, count = m1.checkVector(2); 184 const Point2f* M = m1.ptr<Point2f>(); 185 const Point2f* m = m2.ptr<Point2f>(); 186 const double* H = model.ptr<double>(); 187 float Hf[] = { (float)H[0], (float)H[1], (float)H[2], (float)H[3], (float)H[4], (float)H[5], (float)H[6], (float)H[7] }; 188 189 _err.create(count, 1, CV_32F); 190 float* err = _err.getMat().ptr<float>(); 191 192 for( i = 0; i < count; i++ ) 193 { 194 float ww = 1.f/(Hf[6]*M[i].x + Hf[7]*M[i].y + 1.f); 195 float dx = (Hf[0]*M[i].x + Hf[1]*M[i].y + Hf[2])*ww - m[i].x; 196 float dy = (Hf[3]*M[i].x + Hf[4]*M[i].y + Hf[5])*ww - m[i].y; 197 err[i] = (float)(dx*dx + dy*dy); 198 } 199 } 200 }; 201 202 203 class HomographyRefineCallback : public LMSolver::Callback 204 { 205 public: 206 HomographyRefineCallback(InputArray _src, InputArray _dst) 207 { 208 src = _src.getMat(); 209 dst = _dst.getMat(); 210 } 211 212 bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const 213 { 214 int i, count = src.checkVector(2); 215 Mat param = _param.getMat(); 216 _err.create(count*2, 1, CV_64F); 217 Mat err = _err.getMat(), J; 218 if( _Jac.needed()) 219 { 220 _Jac.create(count*2, param.rows, CV_64F); 221 J = _Jac.getMat(); 222 CV_Assert( J.isContinuous() && J.cols == 8 ); 223 } 224 225 const Point2f* M = src.ptr<Point2f>(); 226 const Point2f* m = dst.ptr<Point2f>(); 227 const double* h = param.ptr<double>(); 228 double* errptr = err.ptr<double>(); 229 double* Jptr = J.data ? J.ptr<double>() : 0; 230 231 for( i = 0; i < count; i++ ) 232 { 233 double Mx = M[i].x, My = M[i].y; 234 double ww = h[6]*Mx + h[7]*My + 1.; 235 ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0; 236 double xi = (h[0]*Mx + h[1]*My + h[2])*ww; 237 double yi = (h[3]*Mx + h[4]*My + h[5])*ww; 238 errptr[i*2] = xi - m[i].x; 239 errptr[i*2+1] = yi - m[i].y; 240 241 if( Jptr ) 242 { 243 Jptr[0] = Mx*ww; Jptr[1] = My*ww; Jptr[2] = ww; 244 Jptr[3] = Jptr[4] = Jptr[5] = 0.; 245 Jptr[6] = -Mx*ww*xi; Jptr[7] = -My*ww*xi; 246 Jptr[8] = Jptr[9] = Jptr[10] = 0.; 247 Jptr[11] = Mx*ww; Jptr[12] = My*ww; Jptr[13] = ww; 248 Jptr[14] = -Mx*ww*yi; Jptr[15] = -My*ww*yi; 249 250 Jptr += 16; 251 } 252 } 253 254 return true; 255 } 256 257 Mat src, dst; 258 }; 259 260 } 261 262 263 264 namespace cv{ 265 static bool createAndRunRHORegistrator(double confidence, 266 int maxIters, 267 double ransacReprojThreshold, 268 int npoints, 269 InputArray _src, 270 InputArray _dst, 271 OutputArray _H, 272 OutputArray _tempMask){ 273 Mat src = _src.getMat(); 274 Mat dst = _dst.getMat(); 275 Mat tempMask; 276 bool result; 277 double beta = 0.35;/* 0.35 is a value that often works. */ 278 279 /* Create temporary output matrix (RHO outputs a single-precision H only). */ 280 Mat tmpH = Mat(3, 3, CV_32FC1); 281 282 /* Create output mask. */ 283 tempMask = Mat(npoints, 1, CV_8U); 284 285 /** 286 * Make use of the RHO estimator API. 287 * 288 * This is where the math happens. A homography estimation context is 289 * initialized, used, then finalized. 290 */ 291 292 Ptr<RHO_HEST> p = rhoInit(); 293 294 /** 295 * Optional. Ideally, the context would survive across calls to 296 * findHomography(), but no clean way appears to exit to do so. The price 297 * to pay is marginally more computational work than strictly needed. 298 */ 299 300 rhoEnsureCapacity(p, npoints, beta); 301 302 /** 303 * The critical call. All parameters are heavily documented in rhorefc.h. 304 * 305 * Currently, NR (Non-Randomness criterion) and Final Refinement (with 306 * internal, optimized Levenberg-Marquardt method) are enabled. However, 307 * while refinement seems to correctly smooth jitter most of the time, when 308 * refinement fails it tends to make the estimate visually very much worse. 309 * It may be necessary to remove the refinement flags in a future commit if 310 * this behaviour is too problematic. 311 */ 312 313 result = !!rhoHest(p, 314 (const float*)src.data, 315 (const float*)dst.data, 316 (char*) tempMask.data, 317 (unsigned) npoints, 318 (float) ransacReprojThreshold, 319 (unsigned) maxIters, 320 (unsigned) maxIters, 321 confidence, 322 4U, 323 beta, 324 RHO_FLAG_ENABLE_NR | RHO_FLAG_ENABLE_FINAL_REFINEMENT, 325 NULL, 326 (float*)tmpH.data); 327 328 /* Convert float homography to double precision. */ 329 tmpH.convertTo(_H, CV_64FC1); 330 331 /* Maps non-zero mask elems to 1, for the sake of the testcase. */ 332 for(int k=0;k<npoints;k++){ 333 tempMask.data[k] = !!tempMask.data[k]; 334 } 335 tempMask.copyTo(_tempMask); 336 337 return result; 338 } 339 } 340 341 342 cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, 343 int method, double ransacReprojThreshold, OutputArray _mask, 344 const int maxIters, const double confidence) 345 { 346 const double defaultRANSACReprojThreshold = 3; 347 bool result = false; 348 349 Mat points1 = _points1.getMat(), points2 = _points2.getMat(); 350 Mat src, dst, H, tempMask; 351 int npoints = -1; 352 353 for( int i = 1; i <= 2; i++ ) 354 { 355 Mat& p = i == 1 ? points1 : points2; 356 Mat& m = i == 1 ? src : dst; 357 npoints = p.checkVector(2, -1, false); 358 if( npoints < 0 ) 359 { 360 npoints = p.checkVector(3, -1, false); 361 if( npoints < 0 ) 362 CV_Error(Error::StsBadArg, "The input arrays should be 2D or 3D point sets"); 363 if( npoints == 0 ) 364 return Mat(); 365 convertPointsFromHomogeneous(p, p); 366 } 367 p.reshape(2, npoints).convertTo(m, CV_32F); 368 } 369 370 CV_Assert( src.checkVector(2) == dst.checkVector(2) ); 371 372 if( ransacReprojThreshold <= 0 ) 373 ransacReprojThreshold = defaultRANSACReprojThreshold; 374 375 Ptr<PointSetRegistrator::Callback> cb = makePtr<HomographyEstimatorCallback>(); 376 377 if( method == 0 || npoints == 4 ) 378 { 379 tempMask = Mat::ones(npoints, 1, CV_8U); 380 result = cb->runKernel(src, dst, H) > 0; 381 } 382 else if( method == RANSAC ) 383 result = createRANSACPointSetRegistrator(cb, 4, ransacReprojThreshold, confidence, maxIters)->run(src, dst, H, tempMask); 384 else if( method == LMEDS ) 385 result = createLMeDSPointSetRegistrator(cb, 4, confidence, maxIters)->run(src, dst, H, tempMask); 386 else if( method == RHO ) 387 result = createAndRunRHORegistrator(confidence, maxIters, ransacReprojThreshold, npoints, src, dst, H, tempMask); 388 else 389 CV_Error(Error::StsBadArg, "Unknown estimation method"); 390 391 if( result && npoints > 4 && method != RHO) 392 { 393 compressElems( src.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints ); 394 npoints = compressElems( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints ); 395 if( npoints > 0 ) 396 { 397 Mat src1 = src.rowRange(0, npoints); 398 Mat dst1 = dst.rowRange(0, npoints); 399 src = src1; 400 dst = dst1; 401 if( method == RANSAC || method == LMEDS ) 402 cb->runKernel( src, dst, H ); 403 Mat H8(8, 1, CV_64F, H.ptr<double>()); 404 createLMSolver(makePtr<HomographyRefineCallback>(src, dst), 10)->run(H8); 405 } 406 } 407 408 if( result ) 409 { 410 if( _mask.needed() ) 411 tempMask.copyTo(_mask); 412 } 413 else 414 H.release(); 415 416 return H; 417 } 418 419 cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, 420 OutputArray _mask, int method, double ransacReprojThreshold ) 421 { 422 return cv::findHomography(_points1, _points2, method, ransacReprojThreshold, _mask); 423 } 424 425 426 427 /* Estimation of Fundamental Matrix from point correspondences. 428 The original code has been written by Valery Mosyagin */ 429 430 /* The algorithms (except for RANSAC) and the notation have been taken from 431 Zhengyou Zhang's research report 432 "Determining the Epipolar Geometry and its Uncertainty: A Review" 433 that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */ 434 435 /************************************** 7-point algorithm *******************************/ 436 namespace cv 437 { 438 439 static int run7Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix ) 440 { 441 double a[7*9], w[7], u[9*9], v[9*9], c[4], r[3]; 442 double* f1, *f2; 443 double t0, t1, t2; 444 Mat A( 7, 9, CV_64F, a ); 445 Mat U( 7, 9, CV_64F, u ); 446 Mat Vt( 9, 9, CV_64F, v ); 447 Mat W( 7, 1, CV_64F, w ); 448 Mat coeffs( 1, 4, CV_64F, c ); 449 Mat roots( 1, 3, CV_64F, r ); 450 const Point2f* m1 = _m1.ptr<Point2f>(); 451 const Point2f* m2 = _m2.ptr<Point2f>(); 452 double* fmatrix = _fmatrix.ptr<double>(); 453 int i, k, n; 454 455 // form a linear system: i-th row of A(=a) represents 456 // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0 457 for( i = 0; i < 7; i++ ) 458 { 459 double x0 = m1[i].x, y0 = m1[i].y; 460 double x1 = m2[i].x, y1 = m2[i].y; 461 462 a[i*9+0] = x1*x0; 463 a[i*9+1] = x1*y0; 464 a[i*9+2] = x1; 465 a[i*9+3] = y1*x0; 466 a[i*9+4] = y1*y0; 467 a[i*9+5] = y1; 468 a[i*9+6] = x0; 469 a[i*9+7] = y0; 470 a[i*9+8] = 1; 471 } 472 473 // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so 474 // the solution is linear subspace of dimensionality 2. 475 // => use the last two singular vectors as a basis of the space 476 // (according to SVD properties) 477 SVDecomp( A, W, U, Vt, SVD::MODIFY_A + SVD::FULL_UV ); 478 f1 = v + 7*9; 479 f2 = v + 8*9; 480 481 // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix. 482 // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1), 483 // so f ~ lambda*f1 + (1 - lambda)*f2. 484 // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda. 485 // it will be a cubic equation. 486 // find c - polynomial coefficients. 487 for( i = 0; i < 9; i++ ) 488 f1[i] -= f2[i]; 489 490 t0 = f2[4]*f2[8] - f2[5]*f2[7]; 491 t1 = f2[3]*f2[8] - f2[5]*f2[6]; 492 t2 = f2[3]*f2[7] - f2[4]*f2[6]; 493 494 c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2; 495 496 c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 - 497 f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) + 498 f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) - 499 f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) + 500 f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) - 501 f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) + 502 f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]); 503 504 t0 = f1[4]*f1[8] - f1[5]*f1[7]; 505 t1 = f1[3]*f1[8] - f1[5]*f1[6]; 506 t2 = f1[3]*f1[7] - f1[4]*f1[6]; 507 508 c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 - 509 f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) + 510 f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) - 511 f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) + 512 f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) - 513 f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) + 514 f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]); 515 516 c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2; 517 518 // solve the cubic equation; there can be 1 to 3 roots ... 519 n = solveCubic( coeffs, roots ); 520 521 if( n < 1 || n > 3 ) 522 return n; 523 524 for( k = 0; k < n; k++, fmatrix += 9 ) 525 { 526 // for each root form the fundamental matrix 527 double lambda = r[k], mu = 1.; 528 double s = f1[8]*r[k] + f2[8]; 529 530 // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1 531 if( fabs(s) > DBL_EPSILON ) 532 { 533 mu = 1./s; 534 lambda *= mu; 535 fmatrix[8] = 1.; 536 } 537 else 538 fmatrix[8] = 0.; 539 540 for( i = 0; i < 8; i++ ) 541 fmatrix[i] = f1[i]*lambda + f2[i]*mu; 542 } 543 544 return n; 545 } 546 547 548 static int run8Point( const Mat& _m1, const Mat& _m2, Mat& _fmatrix ) 549 { 550 double a[9*9], w[9], v[9*9]; 551 Mat W( 9, 1, CV_64F, w ); 552 Mat V( 9, 9, CV_64F, v ); 553 Mat A( 9, 9, CV_64F, a ); 554 Mat U, F0, TF; 555 556 Point2d m1c(0,0), m2c(0,0); 557 double t, scale1 = 0, scale2 = 0; 558 559 const Point2f* m1 = _m1.ptr<Point2f>(); 560 const Point2f* m2 = _m2.ptr<Point2f>(); 561 double* fmatrix = _fmatrix.ptr<double>(); 562 CV_Assert( (_m1.cols == 1 || _m1.rows == 1) && _m1.size() == _m2.size()); 563 int i, j, k, count = _m1.checkVector(2); 564 565 // compute centers and average distances for each of the two point sets 566 for( i = 0; i < count; i++ ) 567 { 568 double x = m1[i].x, y = m1[i].y; 569 m1c.x += x; m1c.y += y; 570 571 x = m2[i].x, y = m2[i].y; 572 m2c.x += x; m2c.y += y; 573 } 574 575 // calculate the normalizing transformations for each of the point sets: 576 // after the transformation each set will have the mass center at the coordinate origin 577 // and the average distance from the origin will be ~sqrt(2). 578 t = 1./count; 579 m1c.x *= t; m1c.y *= t; 580 m2c.x *= t; m2c.y *= t; 581 582 for( i = 0; i < count; i++ ) 583 { 584 double x = m1[i].x - m1c.x, y = m1[i].y - m1c.y; 585 scale1 += std::sqrt(x*x + y*y); 586 587 x = m2[i].x - m2c.x, y = m2[i].y - m2c.y; 588 scale2 += std::sqrt(x*x + y*y); 589 } 590 591 scale1 *= t; 592 scale2 *= t; 593 594 if( scale1 < FLT_EPSILON || scale2 < FLT_EPSILON ) 595 return 0; 596 597 scale1 = std::sqrt(2.)/scale1; 598 scale2 = std::sqrt(2.)/scale2; 599 600 A.setTo(Scalar::all(0)); 601 602 // form a linear system Ax=0: for each selected pair of points m1 & m2, 603 // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0 604 // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0. 605 for( i = 0; i < count; i++ ) 606 { 607 double x1 = (m1[i].x - m1c.x)*scale1; 608 double y1 = (m1[i].y - m1c.y)*scale1; 609 double x2 = (m2[i].x - m2c.x)*scale2; 610 double y2 = (m2[i].y - m2c.y)*scale2; 611 double r[9] = { x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1 }; 612 for( j = 0; j < 9; j++ ) 613 for( k = 0; k < 9; k++ ) 614 a[j*9+k] += r[j]*r[k]; 615 } 616 617 eigen(A, W, V); 618 619 for( i = 0; i < 9; i++ ) 620 { 621 if( fabs(w[i]) < DBL_EPSILON ) 622 break; 623 } 624 625 if( i < 8 ) 626 return 0; 627 628 F0 = Mat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0 629 630 // make F0 singular (of rank 2) by decomposing it with SVD, 631 // zeroing the last diagonal element of W and then composing the matrices back. 632 633 // use v as a temporary storage for different 3x3 matrices 634 W = U = V = TF = F0; 635 W = Mat(3, 1, CV_64F, v); 636 U = Mat(3, 3, CV_64F, v + 9); 637 V = Mat(3, 3, CV_64F, v + 18); 638 TF = Mat(3, 3, CV_64F, v + 27); 639 640 SVDecomp( F0, W, U, V, SVD::MODIFY_A ); 641 W.at<double>(2) = 0.; 642 643 // F0 <- U*diag([W(1), W(2), 0])*V' 644 gemm( U, Mat::diag(W), 1., 0, 0., TF, 0 ); 645 gemm( TF, V, 1., 0, 0., F0, 0/*CV_GEMM_B_T*/ ); 646 647 // apply the transformation that is inverse 648 // to what we used to normalize the point coordinates 649 double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 }; 650 double tt2[] = { scale2, 0, -scale2*m2c.x, 0, scale2, -scale2*m2c.y, 0, 0, 1 }; 651 Mat T1(3, 3, CV_64F, tt1), T2(3, 3, CV_64F, tt2); 652 653 // F0 <- T2'*F0*T1 654 gemm( T2, F0, 1., 0, 0., TF, GEMM_1_T ); 655 F0 = Mat(3, 3, CV_64F, fmatrix); 656 gemm( TF, T1, 1., 0, 0., F0, 0 ); 657 658 // make F(3,3) = 1 659 if( fabs(F0.at<double>(2,2)) > FLT_EPSILON ) 660 F0 *= 1./F0.at<double>(2,2); 661 662 return 1; 663 } 664 665 666 class FMEstimatorCallback : public PointSetRegistrator::Callback 667 { 668 public: 669 bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const 670 { 671 Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat(); 672 return !haveCollinearPoints(ms1, count) && !haveCollinearPoints(ms2, count); 673 } 674 675 int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const 676 { 677 double f[9*3]; 678 Mat m1 = _m1.getMat(), m2 = _m2.getMat(); 679 int count = m1.checkVector(2); 680 Mat F(count == 7 ? 9 : 3, 3, CV_64F, f); 681 int n = count == 7 ? run7Point(m1, m2, F) : run8Point(m1, m2, F); 682 683 if( n == 0 ) 684 _model.release(); 685 else 686 F.rowRange(0, n*3).copyTo(_model); 687 688 return n; 689 } 690 691 void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const 692 { 693 Mat __m1 = _m1.getMat(), __m2 = _m2.getMat(), __model = _model.getMat(); 694 int i, count = __m1.checkVector(2); 695 const Point2f* m1 = __m1.ptr<Point2f>(); 696 const Point2f* m2 = __m2.ptr<Point2f>(); 697 const double* F = __model.ptr<double>(); 698 _err.create(count, 1, CV_32F); 699 float* err = _err.getMat().ptr<float>(); 700 701 for( i = 0; i < count; i++ ) 702 { 703 double a, b, c, d1, d2, s1, s2; 704 705 a = F[0]*m1[i].x + F[1]*m1[i].y + F[2]; 706 b = F[3]*m1[i].x + F[4]*m1[i].y + F[5]; 707 c = F[6]*m1[i].x + F[7]*m1[i].y + F[8]; 708 709 s2 = 1./(a*a + b*b); 710 d2 = m2[i].x*a + m2[i].y*b + c; 711 712 a = F[0]*m2[i].x + F[3]*m2[i].y + F[6]; 713 b = F[1]*m2[i].x + F[4]*m2[i].y + F[7]; 714 c = F[2]*m2[i].x + F[5]*m2[i].y + F[8]; 715 716 s1 = 1./(a*a + b*b); 717 d1 = m1[i].x*a + m1[i].y*b + c; 718 719 err[i] = (float)std::max(d1*d1*s1, d2*d2*s2); 720 } 721 } 722 }; 723 724 } 725 726 cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2, 727 int method, double param1, double param2, 728 OutputArray _mask ) 729 { 730 Mat points1 = _points1.getMat(), points2 = _points2.getMat(); 731 Mat m1, m2, F; 732 int npoints = -1; 733 734 for( int i = 1; i <= 2; i++ ) 735 { 736 Mat& p = i == 1 ? points1 : points2; 737 Mat& m = i == 1 ? m1 : m2; 738 npoints = p.checkVector(2, -1, false); 739 if( npoints < 0 ) 740 { 741 npoints = p.checkVector(3, -1, false); 742 if( npoints < 0 ) 743 CV_Error(Error::StsBadArg, "The input arrays should be 2D or 3D point sets"); 744 if( npoints == 0 ) 745 return Mat(); 746 convertPointsFromHomogeneous(p, p); 747 } 748 p.reshape(2, npoints).convertTo(m, CV_32F); 749 } 750 751 CV_Assert( m1.checkVector(2) == m2.checkVector(2) ); 752 753 if( npoints < 7 ) 754 return Mat(); 755 756 Ptr<PointSetRegistrator::Callback> cb = makePtr<FMEstimatorCallback>(); 757 int result; 758 759 if( npoints == 7 || method == FM_8POINT ) 760 { 761 result = cb->runKernel(m1, m2, F); 762 if( _mask.needed() ) 763 { 764 _mask.create(npoints, 1, CV_8U, -1, true); 765 Mat mask = _mask.getMat(); 766 CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == npoints ); 767 mask.setTo(Scalar::all(1)); 768 } 769 } 770 else 771 { 772 if( param1 <= 0 ) 773 param1 = 3; 774 if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON ) 775 param2 = 0.99; 776 777 if( (method & ~3) == FM_RANSAC && npoints >= 15 ) 778 result = createRANSACPointSetRegistrator(cb, 7, param1, param2)->run(m1, m2, F, _mask); 779 else 780 result = createLMeDSPointSetRegistrator(cb, 7, param2)->run(m1, m2, F, _mask); 781 } 782 783 if( result <= 0 ) 784 return Mat(); 785 786 return F; 787 } 788 789 cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2, 790 OutputArray _mask, int method, double param1, double param2 ) 791 { 792 return cv::findFundamentalMat(_points1, _points2, method, param1, param2, _mask); 793 } 794 795 796 void cv::computeCorrespondEpilines( InputArray _points, int whichImage, 797 InputArray _Fmat, OutputArray _lines ) 798 { 799 double f[9]; 800 Mat tempF(3, 3, CV_64F, f); 801 Mat points = _points.getMat(), F = _Fmat.getMat(); 802 803 if( !points.isContinuous() ) 804 points = points.clone(); 805 806 int npoints = points.checkVector(2); 807 if( npoints < 0 ) 808 { 809 npoints = points.checkVector(3); 810 if( npoints < 0 ) 811 CV_Error( Error::StsBadArg, "The input should be a 2D or 3D point set"); 812 Mat temp; 813 convertPointsFromHomogeneous(points, temp); 814 points = temp; 815 } 816 int depth = points.depth(); 817 CV_Assert( depth == CV_32F || depth == CV_32S || depth == CV_64F ); 818 819 CV_Assert(F.size() == Size(3,3)); 820 F.convertTo(tempF, CV_64F); 821 if( whichImage == 2 ) 822 transpose(tempF, tempF); 823 824 int ltype = CV_MAKETYPE(MAX(depth, CV_32F), 3); 825 _lines.create(npoints, 1, ltype); 826 Mat lines = _lines.getMat(); 827 if( !lines.isContinuous() ) 828 { 829 _lines.release(); 830 _lines.create(npoints, 1, ltype); 831 lines = _lines.getMat(); 832 } 833 CV_Assert( lines.isContinuous()); 834 835 if( depth == CV_32S || depth == CV_32F ) 836 { 837 const Point* ptsi = points.ptr<Point>(); 838 const Point2f* ptsf = points.ptr<Point2f>(); 839 Point3f* dstf = lines.ptr<Point3f>(); 840 for( int i = 0; i < npoints; i++ ) 841 { 842 Point2f pt = depth == CV_32F ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y); 843 double a = f[0]*pt.x + f[1]*pt.y + f[2]; 844 double b = f[3]*pt.x + f[4]*pt.y + f[5]; 845 double c = f[6]*pt.x + f[7]*pt.y + f[8]; 846 double nu = a*a + b*b; 847 nu = nu ? 1./std::sqrt(nu) : 1.; 848 a *= nu; b *= nu; c *= nu; 849 dstf[i] = Point3f((float)a, (float)b, (float)c); 850 } 851 } 852 else 853 { 854 const Point2d* ptsd = points.ptr<Point2d>(); 855 Point3d* dstd = lines.ptr<Point3d>(); 856 for( int i = 0; i < npoints; i++ ) 857 { 858 Point2d pt = ptsd[i]; 859 double a = f[0]*pt.x + f[1]*pt.y + f[2]; 860 double b = f[3]*pt.x + f[4]*pt.y + f[5]; 861 double c = f[6]*pt.x + f[7]*pt.y + f[8]; 862 double nu = a*a + b*b; 863 nu = nu ? 1./std::sqrt(nu) : 1.; 864 a *= nu; b *= nu; c *= nu; 865 dstd[i] = Point3d(a, b, c); 866 } 867 } 868 } 869 870 void cv::convertPointsFromHomogeneous( InputArray _src, OutputArray _dst ) 871 { 872 Mat src = _src.getMat(); 873 if( !src.isContinuous() ) 874 src = src.clone(); 875 int i, npoints = src.checkVector(3), depth = src.depth(), cn = 3; 876 if( npoints < 0 ) 877 { 878 npoints = src.checkVector(4); 879 CV_Assert(npoints >= 0); 880 cn = 4; 881 } 882 CV_Assert( npoints >= 0 && (depth == CV_32S || depth == CV_32F || depth == CV_64F)); 883 884 int dtype = CV_MAKETYPE(depth <= CV_32F ? CV_32F : CV_64F, cn-1); 885 _dst.create(npoints, 1, dtype); 886 Mat dst = _dst.getMat(); 887 if( !dst.isContinuous() ) 888 { 889 _dst.release(); 890 _dst.create(npoints, 1, dtype); 891 dst = _dst.getMat(); 892 } 893 CV_Assert( dst.isContinuous() ); 894 895 if( depth == CV_32S ) 896 { 897 if( cn == 3 ) 898 { 899 const Point3i* sptr = src.ptr<Point3i>(); 900 Point2f* dptr = dst.ptr<Point2f>(); 901 for( i = 0; i < npoints; i++ ) 902 { 903 float scale = sptr[i].z != 0 ? 1.f/sptr[i].z : 1.f; 904 dptr[i] = Point2f(sptr[i].x*scale, sptr[i].y*scale); 905 } 906 } 907 else 908 { 909 const Vec4i* sptr = src.ptr<Vec4i>(); 910 Point3f* dptr = dst.ptr<Point3f>(); 911 for( i = 0; i < npoints; i++ ) 912 { 913 float scale = sptr[i][3] != 0 ? 1.f/sptr[i][3] : 1.f; 914 dptr[i] = Point3f(sptr[i][0]*scale, sptr[i][1]*scale, sptr[i][2]*scale); 915 } 916 } 917 } 918 else if( depth == CV_32F ) 919 { 920 if( cn == 3 ) 921 { 922 const Point3f* sptr = src.ptr<Point3f>(); 923 Point2f* dptr = dst.ptr<Point2f>(); 924 for( i = 0; i < npoints; i++ ) 925 { 926 float scale = sptr[i].z != 0.f ? 1.f/sptr[i].z : 1.f; 927 dptr[i] = Point2f(sptr[i].x*scale, sptr[i].y*scale); 928 } 929 } 930 else 931 { 932 const Vec4f* sptr = src.ptr<Vec4f>(); 933 Point3f* dptr = dst.ptr<Point3f>(); 934 for( i = 0; i < npoints; i++ ) 935 { 936 float scale = sptr[i][3] != 0.f ? 1.f/sptr[i][3] : 1.f; 937 dptr[i] = Point3f(sptr[i][0]*scale, sptr[i][1]*scale, sptr[i][2]*scale); 938 } 939 } 940 } 941 else if( depth == CV_64F ) 942 { 943 if( cn == 3 ) 944 { 945 const Point3d* sptr = src.ptr<Point3d>(); 946 Point2d* dptr = dst.ptr<Point2d>(); 947 for( i = 0; i < npoints; i++ ) 948 { 949 double scale = sptr[i].z != 0. ? 1./sptr[i].z : 1.; 950 dptr[i] = Point2d(sptr[i].x*scale, sptr[i].y*scale); 951 } 952 } 953 else 954 { 955 const Vec4d* sptr = src.ptr<Vec4d>(); 956 Point3d* dptr = dst.ptr<Point3d>(); 957 for( i = 0; i < npoints; i++ ) 958 { 959 double scale = sptr[i][3] != 0.f ? 1./sptr[i][3] : 1.; 960 dptr[i] = Point3d(sptr[i][0]*scale, sptr[i][1]*scale, sptr[i][2]*scale); 961 } 962 } 963 } 964 else 965 CV_Error(Error::StsUnsupportedFormat, ""); 966 } 967 968 969 void cv::convertPointsToHomogeneous( InputArray _src, OutputArray _dst ) 970 { 971 Mat src = _src.getMat(); 972 if( !src.isContinuous() ) 973 src = src.clone(); 974 int i, npoints = src.checkVector(2), depth = src.depth(), cn = 2; 975 if( npoints < 0 ) 976 { 977 npoints = src.checkVector(3); 978 CV_Assert(npoints >= 0); 979 cn = 3; 980 } 981 CV_Assert( npoints >= 0 && (depth == CV_32S || depth == CV_32F || depth == CV_64F)); 982 983 int dtype = CV_MAKETYPE(depth <= CV_32F ? CV_32F : CV_64F, cn+1); 984 _dst.create(npoints, 1, dtype); 985 Mat dst = _dst.getMat(); 986 if( !dst.isContinuous() ) 987 { 988 _dst.release(); 989 _dst.create(npoints, 1, dtype); 990 dst = _dst.getMat(); 991 } 992 CV_Assert( dst.isContinuous() ); 993 994 if( depth == CV_32S ) 995 { 996 if( cn == 2 ) 997 { 998 const Point2i* sptr = src.ptr<Point2i>(); 999 Point3i* dptr = dst.ptr<Point3i>(); 1000 for( i = 0; i < npoints; i++ ) 1001 dptr[i] = Point3i(sptr[i].x, sptr[i].y, 1); 1002 } 1003 else 1004 { 1005 const Point3i* sptr = src.ptr<Point3i>(); 1006 Vec4i* dptr = dst.ptr<Vec4i>(); 1007 for( i = 0; i < npoints; i++ ) 1008 dptr[i] = Vec4i(sptr[i].x, sptr[i].y, sptr[i].z, 1); 1009 } 1010 } 1011 else if( depth == CV_32F ) 1012 { 1013 if( cn == 2 ) 1014 { 1015 const Point2f* sptr = src.ptr<Point2f>(); 1016 Point3f* dptr = dst.ptr<Point3f>(); 1017 for( i = 0; i < npoints; i++ ) 1018 dptr[i] = Point3f(sptr[i].x, sptr[i].y, 1.f); 1019 } 1020 else 1021 { 1022 const Point3f* sptr = src.ptr<Point3f>(); 1023 Vec4f* dptr = dst.ptr<Vec4f>(); 1024 for( i = 0; i < npoints; i++ ) 1025 dptr[i] = Vec4f(sptr[i].x, sptr[i].y, sptr[i].z, 1.f); 1026 } 1027 } 1028 else if( depth == CV_64F ) 1029 { 1030 if( cn == 2 ) 1031 { 1032 const Point2d* sptr = src.ptr<Point2d>(); 1033 Point3d* dptr = dst.ptr<Point3d>(); 1034 for( i = 0; i < npoints; i++ ) 1035 dptr[i] = Point3d(sptr[i].x, sptr[i].y, 1.); 1036 } 1037 else 1038 { 1039 const Point3d* sptr = src.ptr<Point3d>(); 1040 Vec4d* dptr = dst.ptr<Vec4d>(); 1041 for( i = 0; i < npoints; i++ ) 1042 dptr[i] = Vec4d(sptr[i].x, sptr[i].y, sptr[i].z, 1.); 1043 } 1044 } 1045 else 1046 CV_Error(Error::StsUnsupportedFormat, ""); 1047 } 1048 1049 1050 void cv::convertPointsHomogeneous( InputArray _src, OutputArray _dst ) 1051 { 1052 int stype = _src.type(), dtype = _dst.type(); 1053 CV_Assert( _dst.fixedType() ); 1054 1055 if( CV_MAT_CN(stype) > CV_MAT_CN(dtype) ) 1056 convertPointsFromHomogeneous(_src, _dst); 1057 else 1058 convertPointsToHomogeneous(_src, _dst); 1059 } 1060 1061 /* End of file. */ 1062