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 42 #include "_cv.h" 43 44 template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count ) 45 { 46 int i, j; 47 for( i = j = 0; i < count; i++ ) 48 if( mask[i*mstep] ) 49 { 50 if( i > j ) 51 ptr[j] = ptr[i]; 52 j++; 53 } 54 return j; 55 } 56 57 class CvModelEstimator2 58 { 59 public: 60 CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions); 61 virtual ~CvModelEstimator2(); 62 63 virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0; 64 virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model, 65 CvMat* mask, double confidence=0.99, int maxIters=1000 ); 66 virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model, 67 CvMat* mask, double threshold, 68 double confidence=0.99, int maxIters=1000 ); 69 virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; } 70 virtual void setSeed( int64 seed ); 71 72 protected: 73 virtual void computeReprojError( const CvMat* m1, const CvMat* m2, 74 const CvMat* model, CvMat* error ) = 0; 75 virtual int findInliers( const CvMat* m1, const CvMat* m2, 76 const CvMat* model, CvMat* error, 77 CvMat* mask, double threshold ); 78 virtual bool getSubset( const CvMat* m1, const CvMat* m2, 79 CvMat* ms1, CvMat* ms2, int maxAttempts=1000 ); 80 virtual bool checkSubset( const CvMat* ms1, int count ); 81 82 CvRNG rng; 83 int modelPoints; 84 CvSize modelSize; 85 int maxBasicSolutions; 86 bool checkPartialSubsets; 87 }; 88 89 90 CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions) 91 { 92 modelPoints = _modelPoints; 93 modelSize = _modelSize; 94 maxBasicSolutions = _maxBasicSolutions; 95 checkPartialSubsets = true; 96 rng = cvRNG(-1); 97 } 98 99 CvModelEstimator2::~CvModelEstimator2() 100 { 101 } 102 103 void CvModelEstimator2::setSeed( int64 seed ) 104 { 105 rng = cvRNG(seed); 106 } 107 108 109 int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2, 110 const CvMat* model, CvMat* _err, 111 CvMat* _mask, double threshold ) 112 { 113 int i, count = _err->rows*_err->cols, goodCount = 0; 114 const float* err = _err->data.fl; 115 uchar* mask = _mask->data.ptr; 116 117 computeReprojError( m1, m2, model, _err ); 118 threshold *= threshold; 119 for( i = 0; i < count; i++ ) 120 goodCount += mask[i] = err[i] <= threshold; 121 return goodCount; 122 } 123 124 125 CV_IMPL int 126 cvRANSACUpdateNumIters( double p, double ep, 127 int model_points, int max_iters ) 128 { 129 int result = 0; 130 131 CV_FUNCNAME( "cvRANSACUpdateNumIters" ); 132 133 __BEGIN__; 134 135 double num, denom; 136 137 if( model_points <= 0 ) 138 CV_ERROR( CV_StsOutOfRange, "the number of model points should be positive" ); 139 140 p = MAX(p, 0.); 141 p = MIN(p, 1.); 142 ep = MAX(ep, 0.); 143 ep = MIN(ep, 1.); 144 145 // avoid inf's & nan's 146 num = MAX(1. - p, DBL_MIN); 147 denom = 1. - pow(1. - ep,model_points); 148 if( denom < DBL_MIN ) 149 EXIT; 150 151 num = log(num); 152 denom = log(denom); 153 154 result = denom >= 0 || -num >= max_iters*(-denom) ? 155 max_iters : cvRound(num/denom); 156 157 __END__; 158 159 return result; 160 } 161 162 bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model, 163 CvMat* mask, double reprojThreshold, 164 double confidence, int maxIters ) 165 { 166 bool result = false; 167 CvMat* mask0 = mask, *tmask = 0, *t; 168 CvMat* models = 0, *err = 0; 169 CvMat *ms1 = 0, *ms2 = 0; 170 171 CV_FUNCNAME( "CvModelEstimator2::estimateRansac" ); 172 173 __BEGIN__; 174 175 int iter, niters = maxIters; 176 int count = m1->rows*m1->cols, maxGoodCount = 0; 177 CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) ); 178 179 if( count < modelPoints ) 180 EXIT; 181 182 models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 ); 183 err = cvCreateMat( 1, count, CV_32FC1 ); 184 tmask = cvCreateMat( 1, count, CV_8UC1 ); 185 186 if( count > modelPoints ) 187 { 188 ms1 = cvCreateMat( 1, modelPoints, m1->type ); 189 ms2 = cvCreateMat( 1, modelPoints, m2->type ); 190 } 191 else 192 { 193 niters = 1; 194 ms1 = (CvMat*)m1; 195 ms2 = (CvMat*)m2; 196 } 197 198 for( iter = 0; iter < niters; iter++ ) 199 { 200 int i, goodCount, nmodels; 201 if( count > modelPoints ) 202 { 203 bool found = getSubset( m1, m2, ms1, ms2, modelPoints ); 204 if( !found ) 205 { 206 if( iter == 0 ) 207 EXIT; 208 break; 209 } 210 } 211 212 nmodels = runKernel( ms1, ms2, models ); 213 if( nmodels <= 0 ) 214 continue; 215 for( i = 0; i < nmodels; i++ ) 216 { 217 CvMat model_i; 218 cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height ); 219 goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold ); 220 221 if( goodCount > MAX(maxGoodCount, modelPoints-1) ) 222 { 223 CV_SWAP( tmask, mask, t ); 224 cvCopy( &model_i, model ); 225 maxGoodCount = goodCount; 226 niters = cvRANSACUpdateNumIters( confidence, 227 (double)(count - goodCount)/count, modelPoints, niters ); 228 } 229 } 230 } 231 232 if( maxGoodCount > 0 ) 233 { 234 if( mask != mask0 ) 235 { 236 CV_SWAP( tmask, mask, t ); 237 cvCopy( tmask, mask ); 238 } 239 result = true; 240 } 241 242 __END__; 243 244 if( ms1 != m1 ) 245 cvReleaseMat( &ms1 ); 246 if( ms2 != m2 ) 247 cvReleaseMat( &ms2 ); 248 cvReleaseMat( &models ); 249 cvReleaseMat( &err ); 250 cvReleaseMat( &tmask ); 251 return result; 252 } 253 254 255 static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT ) 256 257 bool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model, 258 CvMat* mask, double confidence, int maxIters ) 259 { 260 const double outlierRatio = 0.45; 261 bool result = false; 262 CvMat* models = 0; 263 CvMat *ms1 = 0, *ms2 = 0; 264 CvMat* err = 0; 265 266 CV_FUNCNAME( "CvModelEstimator2::estimateLMeDS" ); 267 268 __BEGIN__; 269 270 int iter, niters = maxIters; 271 int count = m1->rows*m1->cols; 272 double minMedian = DBL_MAX, sigma; 273 274 CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) ); 275 276 if( count < modelPoints ) 277 EXIT; 278 279 models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 ); 280 err = cvCreateMat( 1, count, CV_32FC1 ); 281 282 if( count > modelPoints ) 283 { 284 ms1 = cvCreateMat( 1, modelPoints, m1->type ); 285 ms2 = cvCreateMat( 1, modelPoints, m2->type ); 286 } 287 else 288 { 289 niters = 1; 290 ms1 = (CvMat*)m1; 291 ms2 = (CvMat*)m2; 292 } 293 294 niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints))); 295 niters = MIN( MAX(niters, 3), maxIters ); 296 297 for( iter = 0; iter < niters; iter++ ) 298 { 299 int i, nmodels; 300 if( count > modelPoints ) 301 { 302 bool found = getSubset( m1, m2, ms1, ms2, 300 ); 303 if( !found ) 304 { 305 if( iter == 0 ) 306 EXIT; 307 break; 308 } 309 } 310 311 nmodels = runKernel( ms1, ms2, models ); 312 if( nmodels <= 0 ) 313 continue; 314 for( i = 0; i < nmodels; i++ ) 315 { 316 CvMat model_i; 317 cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height ); 318 computeReprojError( m1, m2, &model_i, err ); 319 icvSortDistances( err->data.i, count, 0 ); 320 321 double median = count % 2 != 0 ? 322 err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5; 323 324 if( median < minMedian ) 325 { 326 minMedian = median; 327 cvCopy( &model_i, model ); 328 } 329 } 330 } 331 332 if( minMedian < DBL_MAX ) 333 { 334 sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*sqrt(minMedian); 335 sigma = MAX( sigma, FLT_EPSILON*100 ); 336 337 count = findInliers( m1, m2, model, err, mask, sigma ); 338 result = count >= modelPoints; 339 } 340 341 __END__; 342 343 if( ms1 != m1 ) 344 cvReleaseMat( &ms1 ); 345 if( ms2 != m2 ) 346 cvReleaseMat( &ms2 ); 347 cvReleaseMat( &models ); 348 cvReleaseMat( &err ); 349 return result; 350 } 351 352 353 bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2, 354 CvMat* ms1, CvMat* ms2, int maxAttempts ) 355 { 356 int* idx = (int*)cvStackAlloc( modelPoints*sizeof(idx[0]) ); 357 int i, j, k, idx_i, iters = 0; 358 int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type); 359 const int *m1ptr = m1->data.i, *m2ptr = m2->data.i; 360 int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i; 361 int count = m1->cols*m1->rows; 362 363 assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) ); 364 elemSize /= sizeof(int); 365 366 for(;;) 367 { 368 for( i = 0; i < modelPoints && iters < maxAttempts; iters++ ) 369 { 370 idx[i] = idx_i = cvRandInt(&rng) % count; 371 for( j = 0; j < i; j++ ) 372 if( idx_i == idx[j] ) 373 break; 374 if( j < i ) 375 continue; 376 for( k = 0; k < elemSize; k++ ) 377 { 378 ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k]; 379 ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k]; 380 } 381 if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 ))) 382 continue; 383 i++; 384 iters = 0; 385 } 386 if( !checkPartialSubsets && i == modelPoints && 387 (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 ))) 388 continue; 389 break; 390 } 391 392 return i == modelPoints; 393 } 394 395 396 bool CvModelEstimator2::checkSubset( const CvMat* m, int count ) 397 { 398 int j, k, i = count-1; 399 CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr; 400 401 assert( CV_MAT_TYPE(m->type) == CV_64FC2 ); 402 403 // check that the i-th selected point does not belong 404 // to a line connecting some previously selected points 405 for( j = 0; j < i; j++ ) 406 { 407 double dx1 = ptr[j].x - ptr[i].x; 408 double dy1 = ptr[j].y - ptr[i].y; 409 for( k = 0; k < j; k++ ) 410 { 411 double dx2 = ptr[k].x - ptr[i].x; 412 double dy2 = ptr[k].y - ptr[i].y; 413 if( fabs(dx2*dy1 - dy2*dx1) < FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2))) 414 break; 415 } 416 if( k < j ) 417 break; 418 } 419 420 return j == i; 421 } 422 423 424 class CvHomographyEstimator : public CvModelEstimator2 425 { 426 public: 427 CvHomographyEstimator( int modelPoints ); 428 429 virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model ); 430 virtual bool refine( const CvMat* m1, const CvMat* m2, 431 CvMat* model, int maxIters ); 432 protected: 433 virtual void computeReprojError( const CvMat* m1, const CvMat* m2, 434 const CvMat* model, CvMat* error ); 435 }; 436 437 438 CvHomographyEstimator::CvHomographyEstimator(int _modelPoints) 439 : CvModelEstimator2(_modelPoints, cvSize(3,3), 1) 440 { 441 assert( _modelPoints == 4 || _modelPoints == 5 ); 442 } 443 444 int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H ) 445 { 446 int i, count = m1->rows*m1->cols; 447 const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr; 448 const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr; 449 450 double LtL[9][9], W[9][9], V[9][9]; 451 CvMat _LtL = cvMat( 9, 9, CV_64F, LtL ); 452 CvMat _W = cvMat( 9, 9, CV_64F, W ); 453 CvMat _V = cvMat( 9, 9, CV_64F, V ); 454 CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] ); 455 CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] ); 456 CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0}; 457 458 for( i = 0; i < count; i++ ) 459 { 460 cm.x += m[i].x; cm.y += m[i].y; 461 cM.x += M[i].x; cM.y += M[i].y; 462 } 463 464 cm.x /= count; cm.y /= count; 465 cM.x /= count; cM.y /= count; 466 467 for( i = 0; i < count; i++ ) 468 { 469 sm.x += fabs(m[i].x - cm.x); 470 sm.y += fabs(m[i].y - cm.y); 471 sM.x += fabs(M[i].x - cM.x); 472 sM.y += fabs(M[i].y - cM.y); 473 } 474 475 sm.x = count/sm.x; sm.y = count/sm.y; 476 sM.x = count/sM.x; sM.y = count/sM.y; 477 478 double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 }; 479 double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 }; 480 CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm ); 481 CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 ); 482 483 cvZero( &_LtL ); 484 for( i = 0; i < count; i++ ) 485 { 486 double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y; 487 double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y; 488 double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x }; 489 double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y }; 490 int j, k; 491 for( j = 0; j < 9; j++ ) 492 for( k = j; k < 9; k++ ) 493 LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k]; 494 } 495 cvCompleteSymm( &_LtL ); 496 497 cvSVD( &_LtL, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 498 cvMatMul( &_invHnorm, &_H0, &_Htemp ); 499 cvMatMul( &_Htemp, &_Hnorm2, &_H0 ); 500 cvConvertScale( &_H0, H, 1./_H0.data.db[8] ); 501 502 return 1; 503 } 504 505 506 void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2, 507 const CvMat* model, CvMat* _err ) 508 { 509 int i, count = m1->rows*m1->cols; 510 const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr; 511 const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr; 512 const double* H = model->data.db; 513 float* err = _err->data.fl; 514 515 for( i = 0; i < count; i++ ) 516 { 517 double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.); 518 double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x; 519 double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y; 520 err[i] = (float)(dx*dx + dy*dy); 521 } 522 } 523 524 bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters ) 525 { 526 CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON)); 527 int i, j, k, count = m1->rows*m1->cols; 528 const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr; 529 const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr; 530 CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr ); 531 cvCopy( &modelPart, solver.param ); 532 533 for(;;) 534 { 535 const CvMat* _param = 0; 536 CvMat *_JtJ = 0, *_JtErr = 0; 537 double* _errNorm = 0; 538 539 if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm )) 540 break; 541 542 for( i = 0; i < count; i++ ) 543 { 544 const double* h = _param->data.db; 545 double Mx = M[i].x, My = M[i].y; 546 double ww = 1./(h[6]*Mx + h[7]*My + 1.); 547 double _xi = (h[0]*Mx + h[1]*My + h[2])*ww; 548 double _yi = (h[3]*Mx + h[4]*My + h[5])*ww; 549 double err[] = { _xi - m[i].x, _yi - m[i].y }; 550 if( _JtJ || _JtErr ) 551 { 552 double J[][8] = 553 { 554 { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi }, 555 { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi } 556 }; 557 558 for( j = 0; j < 8; j++ ) 559 { 560 for( k = j; k < 8; k++ ) 561 _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k]; 562 _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1]; 563 } 564 } 565 if( _errNorm ) 566 *_errNorm += err[0]*err[0] + err[1]*err[1]; 567 } 568 } 569 570 cvCopy( solver.param, &modelPart ); 571 return true; 572 } 573 574 575 CV_IMPL int 576 cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints, 577 CvMat* __H, int method, double ransacReprojThreshold, 578 CvMat* mask ) 579 { 580 const double confidence = 0.99; 581 bool result = false; 582 CvMat *m = 0, *M = 0, *tempMask = 0; 583 584 CV_FUNCNAME( "cvFindHomography" ); 585 586 __BEGIN__; 587 588 double H[9]; 589 CvMat _H = cvMat( 3, 3, CV_64FC1, H ); 590 int count; 591 592 CV_ASSERT( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) ); 593 594 count = MAX(imagePoints->cols, imagePoints->rows); 595 CV_ASSERT( count >= 4 ); 596 597 m = cvCreateMat( 1, count, CV_64FC2 ); 598 cvConvertPointsHomogeneous( imagePoints, m ); 599 600 M = cvCreateMat( 1, count, CV_64FC2 ); 601 cvConvertPointsHomogeneous( objectPoints, M ); 602 603 if( mask ) 604 { 605 CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) && 606 (mask->rows == 1 || mask->cols == 1) && 607 mask->rows*mask->cols == count ); 608 tempMask = mask; 609 } 610 else if( count > 4 ) 611 tempMask = cvCreateMat( 1, count, CV_8U ); 612 if( tempMask ) 613 cvSet( tempMask, cvScalarAll(1.) ); 614 615 { 616 CvHomographyEstimator estimator( MIN(count, 5) ); 617 if( count == 4 ) 618 method = 0; 619 if( method == CV_LMEDS ) 620 result = estimator.runLMeDS( M, m, &_H, tempMask, confidence ); 621 else if( method == CV_RANSAC ) 622 result = estimator.runRANSAC( M, m, &_H, tempMask, ransacReprojThreshold, confidence ); 623 else 624 result = estimator.runKernel( M, m, &_H ) > 0; 625 626 if( result && count > 4 ) 627 { 628 icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count ); 629 count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count ); 630 M->cols = m->cols = count; 631 estimator.refine( M, m, &_H, 10 ); 632 } 633 } 634 635 if( result ) 636 cvConvert( &_H, __H ); 637 638 __END__; 639 640 cvReleaseMat( &m ); 641 cvReleaseMat( &M ); 642 if( tempMask != mask ) 643 cvReleaseMat( &tempMask ); 644 645 return (int)result; 646 } 647 648 649 /* Evaluation of Fundamental Matrix from point correspondences. 650 The original code has been written by Valery Mosyagin */ 651 652 /* The algorithms (except for RANSAC) and the notation have been taken from 653 Zhengyou Zhang's research report 654 "Determining the Epipolar Geometry and its Uncertainty: A Review" 655 that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */ 656 657 /************************************** 7-point algorithm *******************************/ 658 class CvFMEstimator : public CvModelEstimator2 659 { 660 public: 661 CvFMEstimator( int _modelPoints ); 662 663 virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model ); 664 virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model ); 665 virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model ); 666 protected: 667 virtual void computeReprojError( const CvMat* m1, const CvMat* m2, 668 const CvMat* model, CvMat* error ); 669 }; 670 671 CvFMEstimator::CvFMEstimator( int _modelPoints ) 672 : CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 ) 673 { 674 assert( _modelPoints == 7 || _modelPoints == 8 ); 675 } 676 677 678 int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model ) 679 { 680 return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model ); 681 } 682 683 int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix ) 684 { 685 double a[7*9], w[7], v[9*9], c[4], r[3]; 686 double* f1, *f2; 687 double t0, t1, t2; 688 CvMat A = cvMat( 7, 9, CV_64F, a ); 689 CvMat V = cvMat( 9, 9, CV_64F, v ); 690 CvMat W = cvMat( 7, 1, CV_64F, w ); 691 CvMat coeffs = cvMat( 1, 4, CV_64F, c ); 692 CvMat roots = cvMat( 1, 3, CV_64F, r ); 693 const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr; 694 const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr; 695 double* fmatrix = _fmatrix->data.db; 696 int i, k, n; 697 698 // form a linear system: i-th row of A(=a) represents 699 // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0 700 for( i = 0; i < 7; i++ ) 701 { 702 double x0 = m1[i].x, y0 = m1[i].y; 703 double x1 = m2[i].x, y1 = m2[i].y; 704 705 a[i*9+0] = x1*x0; 706 a[i*9+1] = x1*y0; 707 a[i*9+2] = x1; 708 a[i*9+3] = y1*x0; 709 a[i*9+4] = y1*y0; 710 a[i*9+5] = y1; 711 a[i*9+6] = x0; 712 a[i*9+7] = y0; 713 a[i*9+8] = 1; 714 } 715 716 // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so 717 // the solution is linear subspace of dimensionality 2. 718 // => use the last two singular vectors as a basis of the space 719 // (according to SVD properties) 720 cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 721 f1 = v + 7*9; 722 f2 = v + 8*9; 723 724 // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix. 725 // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1), 726 // so f ~ lambda*f1 + (1 - lambda)*f2. 727 // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda. 728 // it will be a cubic equation. 729 // find c - polynomial coefficients. 730 for( i = 0; i < 9; i++ ) 731 f1[i] -= f2[i]; 732 733 t0 = f2[4]*f2[8] - f2[5]*f2[7]; 734 t1 = f2[3]*f2[8] - f2[5]*f2[6]; 735 t2 = f2[3]*f2[7] - f2[4]*f2[6]; 736 737 c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2; 738 739 c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 - 740 f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) + 741 f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) - 742 f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) + 743 f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) - 744 f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) + 745 f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]); 746 747 t0 = f1[4]*f1[8] - f1[5]*f1[7]; 748 t1 = f1[3]*f1[8] - f1[5]*f1[6]; 749 t2 = f1[3]*f1[7] - f1[4]*f1[6]; 750 751 c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 - 752 f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) + 753 f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) - 754 f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) + 755 f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) - 756 f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) + 757 f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]); 758 759 c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2; 760 761 // solve the cubic equation; there can be 1 to 3 roots ... 762 n = cvSolveCubic( &coeffs, &roots ); 763 764 if( n < 1 || n > 3 ) 765 return n; 766 767 for( k = 0; k < n; k++, fmatrix += 9 ) 768 { 769 // for each root form the fundamental matrix 770 double lambda = r[k], mu = 1.; 771 double s = f1[8]*r[k] + f2[8]; 772 773 // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1 774 if( fabs(s) > DBL_EPSILON ) 775 { 776 mu = 1./s; 777 lambda *= mu; 778 fmatrix[8] = 1.; 779 } 780 else 781 fmatrix[8] = 0.; 782 783 for( i = 0; i < 8; i++ ) 784 fmatrix[i] = f1[i]*lambda + f2[i]*mu; 785 } 786 787 return n; 788 } 789 790 791 int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix ) 792 { 793 double a[9*9], w[9], v[9*9]; 794 CvMat W = cvMat( 1, 9, CV_64F, w ); 795 CvMat V = cvMat( 9, 9, CV_64F, v ); 796 CvMat A = cvMat( 9, 9, CV_64F, a ); 797 CvMat U, F0, TF; 798 799 CvPoint2D64f m0c = {0,0}, m1c = {0,0}; 800 double t, scale0 = 0, scale1 = 0; 801 802 const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr; 803 const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr; 804 double* fmatrix = _fmatrix->data.db; 805 int i, j, k, count = _m1->cols*_m1->rows; 806 807 // compute centers and average distances for each of the two point sets 808 for( i = 0; i < count; i++ ) 809 { 810 double x = m1[i].x, y = m1[i].y; 811 m0c.x += x; m0c.y += y; 812 813 x = m2[i].x, y = m2[i].y; 814 m1c.x += x; m1c.y += y; 815 } 816 817 // calculate the normalizing transformations for each of the point sets: 818 // after the transformation each set will have the mass center at the coordinate origin 819 // and the average distance from the origin will be ~sqrt(2). 820 t = 1./count; 821 m0c.x *= t; m0c.y *= t; 822 m1c.x *= t; m1c.y *= t; 823 824 for( i = 0; i < count; i++ ) 825 { 826 double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y; 827 scale0 += sqrt(x*x + y*y); 828 829 x = fabs(m2[i].x - m1c.x), y = fabs(m2[i].y - m1c.y); 830 scale1 += sqrt(x*x + y*y); 831 } 832 833 scale0 *= t; 834 scale1 *= t; 835 836 if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON ) 837 return 0; 838 839 scale0 = sqrt(2.)/scale0; 840 scale1 = sqrt(2.)/scale1; 841 842 cvZero( &A ); 843 844 // form a linear system Ax=0: for each selected pair of points m1 & m2, 845 // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0 846 // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0. 847 for( i = 0; i < count; i++ ) 848 { 849 double x0 = (m1[i].x - m0c.x)*scale0; 850 double y0 = (m1[i].y - m0c.y)*scale0; 851 double x1 = (m2[i].x - m1c.x)*scale1; 852 double y1 = (m2[i].y - m1c.y)*scale1; 853 double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 }; 854 for( j = 0; j < 9; j++ ) 855 for( k = 0; k < 9; k++ ) 856 a[j*9+k] += r[j]*r[k]; 857 } 858 859 cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 860 861 for( i = 0; i < 8; i++ ) 862 { 863 if( fabs(w[i]) < DBL_EPSILON ) 864 break; 865 } 866 867 if( i < 7 ) 868 return 0; 869 870 F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0 871 872 // make F0 singular (of rank 2) by decomposing it with SVD, 873 // zeroing the last diagonal element of W and then composing the matrices back. 874 875 // use v as a temporary storage for different 3x3 matrices 876 W = U = V = TF = F0; 877 W.data.db = v; 878 U.data.db = v + 9; 879 V.data.db = v + 18; 880 TF.data.db = v + 27; 881 882 cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 883 W.data.db[8] = 0.; 884 885 // F0 <- U*diag([W(1), W(2), 0])*V' 886 cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T ); 887 cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ ); 888 889 // apply the transformation that is inverse 890 // to what we used to normalize the point coordinates 891 { 892 double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 }; 893 double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 }; 894 CvMat T0, T1; 895 T0 = T1 = F0; 896 T0.data.db = tt0; 897 T1.data.db = tt1; 898 899 // F0 <- T1'*F0*T0 900 cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T ); 901 F0.data.db = fmatrix; 902 cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 ); 903 904 // make F(3,3) = 1 905 if( fabs(F0.data.db[8]) > FLT_EPSILON ) 906 cvScale( &F0, &F0, 1./F0.data.db[8] ); 907 } 908 909 return 1; 910 } 911 912 913 void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2, 914 const CvMat* model, CvMat* _err ) 915 { 916 int i, count = _m1->rows*_m1->cols; 917 const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr; 918 const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr; 919 const double* F = model->data.db; 920 float* err = _err->data.fl; 921 922 for( i = 0; i < count; i++ ) 923 { 924 double a, b, c, d1, d2, s1, s2; 925 926 a = F[0]*m1[i].x + F[1]*m1[i].y + F[2]; 927 b = F[3]*m1[i].x + F[4]*m1[i].y + F[5]; 928 c = F[6]*m1[i].x + F[7]*m1[i].y + F[8]; 929 930 s2 = 1./(a*a + b*b); 931 d2 = m2[i].x*a + m2[i].y*b + c; 932 933 a = F[0]*m2[i].x + F[3]*m2[i].y + F[6]; 934 b = F[1]*m2[i].x + F[4]*m2[i].y + F[7]; 935 c = F[2]*m2[i].x + F[5]*m2[i].y + F[8]; 936 937 s1 = 1./(a*a + b*b); 938 d1 = m1[i].x*a + m1[i].y*b + c; 939 940 err[i] = (float)(d1*d1*s1 + d2*d2*s2); 941 } 942 } 943 944 945 CV_IMPL int 946 cvFindFundamentalMat( const CvMat* points1, const CvMat* points2, 947 CvMat* fmatrix, int method, 948 double param1, double param2, CvMat* mask ) 949 { 950 int result = 0; 951 CvMat *m1 = 0, *m2 = 0, *tempMask = 0; 952 953 CV_FUNCNAME( "cvFindFundamentalMat" ); 954 955 __BEGIN__; 956 957 double F[3*9]; 958 CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F ); 959 int count; 960 961 CV_ASSERT( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) ); 962 CV_ASSERT( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 && 963 (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) ); 964 965 count = MAX(points1->cols, points1->rows); 966 if( count < 7 ) 967 EXIT; 968 969 m1 = cvCreateMat( 1, count, CV_64FC2 ); 970 cvConvertPointsHomogeneous( points1, m1 ); 971 972 m2 = cvCreateMat( 1, count, CV_64FC2 ); 973 cvConvertPointsHomogeneous( points2, m2 ); 974 975 if( mask ) 976 { 977 CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) && 978 (mask->rows == 1 || mask->cols == 1) && 979 mask->rows*mask->cols == count ); 980 tempMask = mask; 981 } 982 else if( count > 8 ) 983 tempMask = cvCreateMat( 1, count, CV_8U ); 984 if( tempMask ) 985 cvSet( tempMask, cvScalarAll(1.) ); 986 987 { 988 CvFMEstimator estimator( MIN(count, (method & 3) == CV_FM_7POINT ? 7 : 8) ); 989 if( count == 7 ) 990 result = estimator.run7Point(m1, m2, &_F9x3); 991 else if( count == 8 || method == CV_FM_8POINT ) 992 result = estimator.run8Point(m1, m2, &_F3x3); 993 else if( count > 8 ) 994 { 995 if( param1 <= 0 ) 996 param1 = 3; 997 if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON ) 998 param2 = 0.99; 999 1000 if( (method & ~3) == CV_RANSAC ) 1001 result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 ); 1002 else 1003 result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 ); 1004 if( result <= 0 ) 1005 EXIT; 1006 icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count ); 1007 count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count ); 1008 assert( count >= 8 ); 1009 m1->cols = m2->cols = count; 1010 estimator.run8Point(m1, m2, &_F3x3); 1011 } 1012 } 1013 1014 if( result ) 1015 cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix ); 1016 1017 __END__; 1018 1019 cvReleaseMat( &m1 ); 1020 cvReleaseMat( &m2 ); 1021 if( tempMask != mask ) 1022 cvReleaseMat( &tempMask ); 1023 1024 return result; 1025 } 1026 1027 1028 CV_IMPL void 1029 cvComputeCorrespondEpilines( const CvMat* points, int pointImageID, 1030 const CvMat* fmatrix, CvMat* lines ) 1031 { 1032 CV_FUNCNAME( "cvComputeCorrespondEpilines" ); 1033 1034 __BEGIN__; 1035 1036 int abc_stride, abc_plane_stride, abc_elem_size; 1037 int plane_stride, stride, elem_size; 1038 int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn; 1039 uchar *ap, *bp, *cp; 1040 const uchar *xp, *yp, *zp; 1041 double f[9]; 1042 CvMat F = cvMat( 3, 3, CV_64F, f ); 1043 1044 if( !CV_IS_MAT(points) ) 1045 CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" ); 1046 1047 depth = CV_MAT_DEPTH(points->type); 1048 cn = CV_MAT_CN(points->type); 1049 if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) ) 1050 CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" ); 1051 1052 if( points->rows > points->cols ) 1053 { 1054 dims = cn*points->cols; 1055 count = points->rows; 1056 } 1057 else 1058 { 1059 if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) ) 1060 CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" ); 1061 dims = cn * points->rows; 1062 count = points->cols; 1063 } 1064 1065 if( dims != 2 && dims != 3 ) 1066 CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" ); 1067 1068 if( !CV_IS_MAT(fmatrix) ) 1069 CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" ); 1070 1071 if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 ) 1072 CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" ); 1073 1074 if( fmatrix->cols != 3 || fmatrix->rows != 3 ) 1075 CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" ); 1076 1077 if( !CV_IS_MAT(lines) ) 1078 CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" ); 1079 1080 abc_depth = CV_MAT_DEPTH(lines->type); 1081 abc_cn = CV_MAT_CN(lines->type); 1082 if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) ) 1083 CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" ); 1084 1085 if( lines->rows > lines->cols ) 1086 { 1087 abc_dims = abc_cn*lines->cols; 1088 abc_count = lines->rows; 1089 } 1090 else 1091 { 1092 if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) ) 1093 CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" ); 1094 abc_dims = abc_cn * lines->rows; 1095 abc_count = lines->cols; 1096 } 1097 1098 if( abc_dims != 3 ) 1099 CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" ); 1100 1101 if( abc_count != count ) 1102 CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" ); 1103 1104 elem_size = CV_ELEM_SIZE(depth); 1105 abc_elem_size = CV_ELEM_SIZE(abc_depth); 1106 1107 if( points->rows == dims ) 1108 { 1109 plane_stride = points->step; 1110 stride = elem_size; 1111 } 1112 else 1113 { 1114 plane_stride = elem_size; 1115 stride = points->rows == 1 ? dims*elem_size : points->step; 1116 } 1117 1118 if( lines->rows == 3 ) 1119 { 1120 abc_plane_stride = lines->step; 1121 abc_stride = abc_elem_size; 1122 } 1123 else 1124 { 1125 abc_plane_stride = abc_elem_size; 1126 abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step; 1127 } 1128 1129 CV_CALL( cvConvert( fmatrix, &F )); 1130 if( pointImageID == 2 ) 1131 cvTranspose( &F, &F ); 1132 1133 xp = points->data.ptr; 1134 yp = xp + plane_stride; 1135 zp = dims == 3 ? yp + plane_stride : 0; 1136 1137 ap = lines->data.ptr; 1138 bp = ap + abc_plane_stride; 1139 cp = bp + abc_plane_stride; 1140 1141 for( i = 0; i < count; i++ ) 1142 { 1143 double x, y, z = 1.; 1144 double a, b, c, nu; 1145 1146 if( depth == CV_32F ) 1147 { 1148 x = *(float*)xp; y = *(float*)yp; 1149 if( zp ) 1150 z = *(float*)zp, zp += stride; 1151 } 1152 else 1153 { 1154 x = *(double*)xp; y = *(double*)yp; 1155 if( zp ) 1156 z = *(double*)zp, zp += stride; 1157 } 1158 1159 xp += stride; yp += stride; 1160 1161 a = f[0]*x + f[1]*y + f[2]*z; 1162 b = f[3]*x + f[4]*y + f[5]*z; 1163 c = f[6]*x + f[7]*y + f[8]*z; 1164 nu = a*a + b*b; 1165 nu = nu ? 1./sqrt(nu) : 1.; 1166 a *= nu; b *= nu; c *= nu; 1167 1168 if( abc_depth == CV_32F ) 1169 { 1170 *(float*)ap = (float)a; 1171 *(float*)bp = (float)b; 1172 *(float*)cp = (float)c; 1173 } 1174 else 1175 { 1176 *(double*)ap = a; 1177 *(double*)bp = b; 1178 *(double*)cp = c; 1179 } 1180 1181 ap += abc_stride; 1182 bp += abc_stride; 1183 cp += abc_stride; 1184 } 1185 1186 __END__; 1187 } 1188 1189 1190 CV_IMPL void 1191 cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst ) 1192 { 1193 CvMat* temp = 0; 1194 CvMat* denom = 0; 1195 1196 CV_FUNCNAME( "cvConvertPointsHomogeneous" ); 1197 1198 __BEGIN__; 1199 1200 int i, s_count, s_dims, d_count, d_dims; 1201 CvMat _src, _dst, _ones; 1202 CvMat* ones = 0; 1203 1204 if( !CV_IS_MAT(src) ) 1205 CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg, 1206 "The input parameter is not a valid matrix" ); 1207 1208 if( !CV_IS_MAT(dst) ) 1209 CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg, 1210 "The output parameter is not a valid matrix" ); 1211 1212 if( src == dst || src->data.ptr == dst->data.ptr ) 1213 { 1214 if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) ) 1215 CV_ERROR( CV_StsBadArg, "Invalid inplace operation" ); 1216 EXIT; 1217 } 1218 1219 if( src->rows > src->cols ) 1220 { 1221 if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) ) 1222 CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" ); 1223 1224 s_dims = CV_MAT_CN(src->type)*src->cols; 1225 s_count = src->rows; 1226 } 1227 else 1228 { 1229 if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) ) 1230 CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" ); 1231 1232 s_dims = CV_MAT_CN(src->type)*src->rows; 1233 s_count = src->cols; 1234 } 1235 1236 if( src->rows == 1 || src->cols == 1 ) 1237 src = cvReshape( src, &_src, 1, s_count ); 1238 1239 if( dst->rows > dst->cols ) 1240 { 1241 if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) ) 1242 CV_ERROR( CV_StsBadSize, 1243 "Either the number of channels or columns or rows in the input matrix must be =1" ); 1244 1245 d_dims = CV_MAT_CN(dst->type)*dst->cols; 1246 d_count = dst->rows; 1247 } 1248 else 1249 { 1250 if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) ) 1251 CV_ERROR( CV_StsBadSize, 1252 "Either the number of channels or columns or rows in the output matrix must be =1" ); 1253 1254 d_dims = CV_MAT_CN(dst->type)*dst->rows; 1255 d_count = dst->cols; 1256 } 1257 1258 if( dst->rows == 1 || dst->cols == 1 ) 1259 dst = cvReshape( dst, &_dst, 1, d_count ); 1260 1261 if( s_count != d_count ) 1262 CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" ); 1263 1264 if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F ) 1265 CV_ERROR( CV_StsUnsupportedFormat, 1266 "Both matrices must be floating-point (single or double precision)" ); 1267 1268 if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 ) 1269 CV_ERROR( CV_StsOutOfRange, 1270 "Both input and output point dimensionality must be 2, 3 or 4" ); 1271 1272 if( s_dims < d_dims - 1 || s_dims > d_dims + 1 ) 1273 CV_ERROR( CV_StsUnmatchedSizes, 1274 "The dimensionalities of input and output point sets differ too much" ); 1275 1276 if( s_dims == d_dims - 1 ) 1277 { 1278 if( d_count == dst->rows ) 1279 { 1280 ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count )); 1281 dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count )); 1282 } 1283 else 1284 { 1285 ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 )); 1286 dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims )); 1287 } 1288 } 1289 1290 if( s_dims <= d_dims ) 1291 { 1292 if( src->rows == dst->rows && src->cols == dst->cols ) 1293 { 1294 if( CV_ARE_TYPES_EQ( src, dst ) ) 1295 cvCopy( src, dst ); 1296 else 1297 cvConvert( src, dst ); 1298 } 1299 else 1300 { 1301 if( !CV_ARE_TYPES_EQ( src, dst )) 1302 { 1303 CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type )); 1304 cvConvert( src, temp ); 1305 src = temp; 1306 } 1307 cvTranspose( src, dst ); 1308 } 1309 1310 if( ones ) 1311 cvSet( ones, cvRealScalar(1.) ); 1312 } 1313 else 1314 { 1315 int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size; 1316 1317 if( !CV_ARE_TYPES_EQ( src, dst )) 1318 { 1319 CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type )); 1320 cvConvert( src, temp ); 1321 src = temp; 1322 } 1323 1324 elem_size = CV_ELEM_SIZE(src->type); 1325 1326 if( s_count == src->cols ) 1327 s_plane_stride = src->step / elem_size, s_stride = 1; 1328 else 1329 s_stride = src->step / elem_size, s_plane_stride = 1; 1330 1331 if( d_count == dst->cols ) 1332 d_plane_stride = dst->step / elem_size, d_stride = 1; 1333 else 1334 d_stride = dst->step / elem_size, d_plane_stride = 1; 1335 1336 CV_CALL( denom = cvCreateMat( 1, d_count, dst->type )); 1337 1338 if( CV_MAT_DEPTH(dst->type) == CV_32F ) 1339 { 1340 const float* xs = src->data.fl; 1341 const float* ys = xs + s_plane_stride; 1342 const float* zs = 0; 1343 const float* ws = xs + (s_dims - 1)*s_plane_stride; 1344 1345 float* iw = denom->data.fl; 1346 1347 float* xd = dst->data.fl; 1348 float* yd = xd + d_plane_stride; 1349 float* zd = 0; 1350 1351 if( d_dims == 3 ) 1352 { 1353 zs = ys + s_plane_stride; 1354 zd = yd + d_plane_stride; 1355 } 1356 1357 for( i = 0; i < d_count; i++, ws += s_stride ) 1358 { 1359 float t = *ws; 1360 iw[i] = t ? t : 1.f; 1361 } 1362 1363 cvDiv( 0, denom, denom ); 1364 1365 if( d_dims == 3 ) 1366 for( i = 0; i < d_count; i++ ) 1367 { 1368 float w = iw[i]; 1369 float x = *xs * w, y = *ys * w, z = *zs * w; 1370 xs += s_stride; ys += s_stride; zs += s_stride; 1371 *xd = x; *yd = y; *zd = z; 1372 xd += d_stride; yd += d_stride; zd += d_stride; 1373 } 1374 else 1375 for( i = 0; i < d_count; i++ ) 1376 { 1377 float w = iw[i]; 1378 float x = *xs * w, y = *ys * w; 1379 xs += s_stride; ys += s_stride; 1380 *xd = x; *yd = y; 1381 xd += d_stride; yd += d_stride; 1382 } 1383 } 1384 else 1385 { 1386 const double* xs = src->data.db; 1387 const double* ys = xs + s_plane_stride; 1388 const double* zs = 0; 1389 const double* ws = xs + (s_dims - 1)*s_plane_stride; 1390 1391 double* iw = denom->data.db; 1392 1393 double* xd = dst->data.db; 1394 double* yd = xd + d_plane_stride; 1395 double* zd = 0; 1396 1397 if( d_dims == 3 ) 1398 { 1399 zs = ys + s_plane_stride; 1400 zd = yd + d_plane_stride; 1401 } 1402 1403 for( i = 0; i < d_count; i++, ws += s_stride ) 1404 { 1405 double t = *ws; 1406 iw[i] = t ? t : 1.; 1407 } 1408 1409 cvDiv( 0, denom, denom ); 1410 1411 if( d_dims == 3 ) 1412 for( i = 0; i < d_count; i++ ) 1413 { 1414 double w = iw[i]; 1415 double x = *xs * w, y = *ys * w, z = *zs * w; 1416 xs += s_stride; ys += s_stride; zs += s_stride; 1417 *xd = x; *yd = y; *zd = z; 1418 xd += d_stride; yd += d_stride; zd += d_stride; 1419 } 1420 else 1421 for( i = 0; i < d_count; i++ ) 1422 { 1423 double w = iw[i]; 1424 double x = *xs * w, y = *ys * w; 1425 xs += s_stride; ys += s_stride; 1426 *xd = x; *yd = y; 1427 xd += d_stride; yd += d_stride; 1428 } 1429 } 1430 } 1431 1432 __END__; 1433 1434 cvReleaseMat( &denom ); 1435 cvReleaseMat( &temp ); 1436 } 1437 1438 /* End of file. */ 1439
