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-2008, Intel Corporation, all rights reserved. 14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved. 15 // Copyright (C) 2013, OpenCV Foundation, all rights reserved. 16 // Third party copyrights are property of their respective owners. 17 // 18 // Redistribution and use in source and binary forms, with or without modification, 19 // are permitted provided that the following conditions are met: 20 // 21 // * Redistribution's of source code must retain the above copyright notice, 22 // this list of conditions and the following disclaimer. 23 // 24 // * Redistribution's in binary form must reproduce the above copyright notice, 25 // this list of conditions and the following disclaimer in the documentation 26 // and/or other materials provided with the distribution. 27 // 28 // * The name of the copyright holders may not be used to endorse or promote products 29 // derived from this software without specific prior written permission. 30 // 31 // This software is provided by the copyright holders and contributors "as is" and 32 // any express or implied warranties, including, but not limited to, the implied 33 // warranties of merchantability and fitness for a particular purpose are disclaimed. 34 // In no event shall the Intel Corporation or contributors be liable for any direct, 35 // indirect, incidental, special, exemplary, or consequential damages 36 // (including, but not limited to, procurement of substitute goods or services; 37 // loss of use, data, or profits; or business interruption) however caused 38 // and on any theory of liability, whether in contract, strict liability, 39 // or tort (including negligence or otherwise) arising in any way out of 40 // the use of this software, even if advised of the possibility of such damage. 41 // 42 //M*/ 43 44 #include "precomp.hpp" 45 46 #include <algorithm> 47 #include <iterator> 48 #include <limits> 49 50 namespace cv 51 { 52 53 int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters ) 54 { 55 if( modelPoints <= 0 ) 56 CV_Error( Error::StsOutOfRange, "the number of model points should be positive" ); 57 58 p = MAX(p, 0.); 59 p = MIN(p, 1.); 60 ep = MAX(ep, 0.); 61 ep = MIN(ep, 1.); 62 63 // avoid inf's & nan's 64 double num = MAX(1. - p, DBL_MIN); 65 double denom = 1. - std::pow(1. - ep, modelPoints); 66 if( denom < DBL_MIN ) 67 return 0; 68 69 num = std::log(num); 70 denom = std::log(denom); 71 72 return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom); 73 } 74 75 76 class RANSACPointSetRegistrator : public PointSetRegistrator 77 { 78 public: 79 RANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(), 80 int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000) 81 : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters) 82 { 83 checkPartialSubsets = false; 84 } 85 86 int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const 87 { 88 cb->computeError( m1, m2, model, err ); 89 mask.create(err.size(), CV_8U); 90 91 CV_Assert( err.isContinuous() && err.type() == CV_32F && mask.isContinuous() && mask.type() == CV_8U); 92 const float* errptr = err.ptr<float>(); 93 uchar* maskptr = mask.ptr<uchar>(); 94 float t = (float)(thresh*thresh); 95 int i, n = (int)err.total(), nz = 0; 96 for( i = 0; i < n; i++ ) 97 { 98 int f = errptr[i] <= t; 99 maskptr[i] = (uchar)f; 100 nz += f; 101 } 102 return nz; 103 } 104 105 bool getSubset( const Mat& m1, const Mat& m2, 106 Mat& ms1, Mat& ms2, RNG& rng, 107 int maxAttempts=1000 ) const 108 { 109 cv::AutoBuffer<int> _idx(modelPoints); 110 int* idx = _idx; 111 int i = 0, j, k, iters = 0; 112 int esz1 = (int)m1.elemSize(), esz2 = (int)m2.elemSize(); 113 int d1 = m1.channels() > 1 ? m1.channels() : m1.cols; 114 int d2 = m2.channels() > 1 ? m2.channels() : m2.cols; 115 int count = m1.checkVector(d1), count2 = m2.checkVector(d2); 116 const int *m1ptr = m1.ptr<int>(), *m2ptr = m2.ptr<int>(); 117 118 ms1.create(modelPoints, 1, CV_MAKETYPE(m1.depth(), d1)); 119 ms2.create(modelPoints, 1, CV_MAKETYPE(m2.depth(), d2)); 120 121 int *ms1ptr = ms1.ptr<int>(), *ms2ptr = ms2.ptr<int>(); 122 123 CV_Assert( count >= modelPoints && count == count2 ); 124 CV_Assert( (esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0 ); 125 esz1 /= sizeof(int); 126 esz2 /= sizeof(int); 127 128 for(; iters < maxAttempts; iters++) 129 { 130 for( i = 0; i < modelPoints && iters < maxAttempts; ) 131 { 132 int idx_i = 0; 133 for(;;) 134 { 135 idx_i = idx[i] = rng.uniform(0, count); 136 for( j = 0; j < i; j++ ) 137 if( idx_i == idx[j] ) 138 break; 139 if( j == i ) 140 break; 141 } 142 for( k = 0; k < esz1; k++ ) 143 ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k]; 144 for( k = 0; k < esz2; k++ ) 145 ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k]; 146 if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 )) 147 { 148 // we may have selected some bad points; 149 // so, let's remove some of them randomly 150 i = rng.uniform(0, i+1); 151 iters++; 152 continue; 153 } 154 i++; 155 } 156 if( !checkPartialSubsets && i == modelPoints && !cb->checkSubset(ms1, ms2, i)) 157 continue; 158 break; 159 } 160 161 return i == modelPoints && iters < maxAttempts; 162 } 163 164 bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const 165 { 166 bool result = false; 167 Mat m1 = _m1.getMat(), m2 = _m2.getMat(); 168 Mat err, mask, model, bestModel, ms1, ms2; 169 170 int iter, niters = MAX(maxIters, 1); 171 int d1 = m1.channels() > 1 ? m1.channels() : m1.cols; 172 int d2 = m2.channels() > 1 ? m2.channels() : m2.cols; 173 int count = m1.checkVector(d1), count2 = m2.checkVector(d2), maxGoodCount = 0; 174 175 RNG rng((uint64)-1); 176 177 CV_Assert( cb ); 178 CV_Assert( confidence > 0 && confidence < 1 ); 179 180 CV_Assert( count >= 0 && count2 == count ); 181 if( count < modelPoints ) 182 return false; 183 184 Mat bestMask0, bestMask; 185 186 if( _mask.needed() ) 187 { 188 _mask.create(count, 1, CV_8U, -1, true); 189 bestMask0 = bestMask = _mask.getMat(); 190 CV_Assert( (bestMask.cols == 1 || bestMask.rows == 1) && (int)bestMask.total() == count ); 191 } 192 else 193 { 194 bestMask.create(count, 1, CV_8U); 195 bestMask0 = bestMask; 196 } 197 198 if( count == modelPoints ) 199 { 200 if( cb->runKernel(m1, m2, bestModel) <= 0 ) 201 return false; 202 bestModel.copyTo(_model); 203 bestMask.setTo(Scalar::all(1)); 204 return true; 205 } 206 207 for( iter = 0; iter < niters; iter++ ) 208 { 209 int i, goodCount, nmodels; 210 if( count > modelPoints ) 211 { 212 bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 ); 213 if( !found ) 214 { 215 if( iter == 0 ) 216 return false; 217 break; 218 } 219 } 220 221 nmodels = cb->runKernel( ms1, ms2, model ); 222 if( nmodels <= 0 ) 223 continue; 224 CV_Assert( model.rows % nmodels == 0 ); 225 Size modelSize(model.cols, model.rows/nmodels); 226 227 for( i = 0; i < nmodels; i++ ) 228 { 229 Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height ); 230 goodCount = findInliers( m1, m2, model_i, err, mask, threshold ); 231 232 if( goodCount > MAX(maxGoodCount, modelPoints-1) ) 233 { 234 std::swap(mask, bestMask); 235 model_i.copyTo(bestModel); 236 maxGoodCount = goodCount; 237 niters = RANSACUpdateNumIters( confidence, (double)(count - goodCount)/count, modelPoints, niters ); 238 } 239 } 240 } 241 242 if( maxGoodCount > 0 ) 243 { 244 if( bestMask.data != bestMask0.data ) 245 { 246 if( bestMask.size() == bestMask0.size() ) 247 bestMask.copyTo(bestMask0); 248 else 249 transpose(bestMask, bestMask0); 250 } 251 bestModel.copyTo(_model); 252 result = true; 253 } 254 else 255 _model.release(); 256 257 return result; 258 } 259 260 void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) { cb = _cb; } 261 262 Ptr<PointSetRegistrator::Callback> cb; 263 int modelPoints; 264 bool checkPartialSubsets; 265 double threshold; 266 double confidence; 267 int maxIters; 268 }; 269 270 class LMeDSPointSetRegistrator : public RANSACPointSetRegistrator 271 { 272 public: 273 LMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(), 274 int _modelPoints=0, double _confidence=0.99, int _maxIters=1000) 275 : RANSACPointSetRegistrator(_cb, _modelPoints, 0, _confidence, _maxIters) {} 276 277 bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const 278 { 279 const double outlierRatio = 0.45; 280 bool result = false; 281 Mat m1 = _m1.getMat(), m2 = _m2.getMat(); 282 Mat ms1, ms2, err, errf, model, bestModel, mask, mask0; 283 284 int d1 = m1.channels() > 1 ? m1.channels() : m1.cols; 285 int d2 = m2.channels() > 1 ? m2.channels() : m2.cols; 286 int count = m1.checkVector(d1), count2 = m2.checkVector(d2); 287 double minMedian = DBL_MAX, sigma; 288 289 RNG rng((uint64)-1); 290 291 CV_Assert( cb ); 292 CV_Assert( confidence > 0 && confidence < 1 ); 293 294 CV_Assert( count >= 0 && count2 == count ); 295 if( count < modelPoints ) 296 return false; 297 298 if( _mask.needed() ) 299 { 300 _mask.create(count, 1, CV_8U, -1, true); 301 mask0 = mask = _mask.getMat(); 302 CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == count ); 303 } 304 305 if( count == modelPoints ) 306 { 307 if( cb->runKernel(m1, m2, bestModel) <= 0 ) 308 return false; 309 bestModel.copyTo(_model); 310 mask.setTo(Scalar::all(1)); 311 return true; 312 } 313 314 int iter, niters = RANSACUpdateNumIters(confidence, outlierRatio, modelPoints, maxIters); 315 niters = MAX(niters, 3); 316 317 for( iter = 0; iter < niters; iter++ ) 318 { 319 int i, nmodels; 320 if( count > modelPoints ) 321 { 322 bool found = getSubset( m1, m2, ms1, ms2, rng ); 323 if( !found ) 324 { 325 if( iter == 0 ) 326 return false; 327 break; 328 } 329 } 330 331 nmodels = cb->runKernel( ms1, ms2, model ); 332 if( nmodels <= 0 ) 333 continue; 334 335 CV_Assert( model.rows % nmodels == 0 ); 336 Size modelSize(model.cols, model.rows/nmodels); 337 338 for( i = 0; i < nmodels; i++ ) 339 { 340 Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height ); 341 cb->computeError( m1, m2, model_i, err ); 342 if( err.depth() != CV_32F ) 343 err.convertTo(errf, CV_32F); 344 else 345 errf = err; 346 CV_Assert( errf.isContinuous() && errf.type() == CV_32F && (int)errf.total() == count ); 347 std::sort(errf.ptr<int>(), errf.ptr<int>() + count); 348 349 double median = count % 2 != 0 ? 350 errf.at<float>(count/2) : (errf.at<float>(count/2-1) + errf.at<float>(count/2))*0.5; 351 352 if( median < minMedian ) 353 { 354 minMedian = median; 355 model_i.copyTo(bestModel); 356 } 357 } 358 } 359 360 if( minMedian < DBL_MAX ) 361 { 362 sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian); 363 sigma = MAX( sigma, 0.001 ); 364 365 count = findInliers( m1, m2, bestModel, err, mask, sigma ); 366 if( _mask.needed() && mask0.data != mask.data ) 367 { 368 if( mask0.size() == mask.size() ) 369 mask.copyTo(mask0); 370 else 371 transpose(mask, mask0); 372 } 373 bestModel.copyTo(_model); 374 result = count >= modelPoints; 375 } 376 else 377 _model.release(); 378 379 return result; 380 } 381 382 }; 383 384 Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb, 385 int _modelPoints, double _threshold, 386 double _confidence, int _maxIters) 387 { 388 return Ptr<PointSetRegistrator>( 389 new RANSACPointSetRegistrator(_cb, _modelPoints, _threshold, _confidence, _maxIters)); 390 } 391 392 393 Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb, 394 int _modelPoints, double _confidence, int _maxIters) 395 { 396 return Ptr<PointSetRegistrator>( 397 new LMeDSPointSetRegistrator(_cb, _modelPoints, _confidence, _maxIters)); 398 } 399 400 401 class Affine3DEstimatorCallback : public PointSetRegistrator::Callback 402 { 403 public: 404 int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const 405 { 406 Mat m1 = _m1.getMat(), m2 = _m2.getMat(); 407 const Point3f* from = m1.ptr<Point3f>(); 408 const Point3f* to = m2.ptr<Point3f>(); 409 410 const int N = 12; 411 double buf[N*N + N + N]; 412 Mat A(N, N, CV_64F, &buf[0]); 413 Mat B(N, 1, CV_64F, &buf[0] + N*N); 414 Mat X(N, 1, CV_64F, &buf[0] + N*N + N); 415 double* Adata = A.ptr<double>(); 416 double* Bdata = B.ptr<double>(); 417 A = Scalar::all(0); 418 419 for( int i = 0; i < (N/3); i++ ) 420 { 421 Bdata[i*3] = to[i].x; 422 Bdata[i*3+1] = to[i].y; 423 Bdata[i*3+2] = to[i].z; 424 425 double *aptr = Adata + i*3*N; 426 for(int k = 0; k < 3; ++k) 427 { 428 aptr[0] = from[i].x; 429 aptr[1] = from[i].y; 430 aptr[2] = from[i].z; 431 aptr[3] = 1.0; 432 aptr += 16; 433 } 434 } 435 436 solve(A, B, X, DECOMP_SVD); 437 X.reshape(1, 3).copyTo(_model); 438 439 return 1; 440 } 441 442 void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const 443 { 444 Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat(); 445 const Point3f* from = m1.ptr<Point3f>(); 446 const Point3f* to = m2.ptr<Point3f>(); 447 const double* F = model.ptr<double>(); 448 449 int count = m1.checkVector(3); 450 CV_Assert( count > 0 ); 451 452 _err.create(count, 1, CV_32F); 453 Mat err = _err.getMat(); 454 float* errptr = err.ptr<float>(); 455 456 for(int i = 0; i < count; i++ ) 457 { 458 const Point3f& f = from[i]; 459 const Point3f& t = to[i]; 460 461 double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x; 462 double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y; 463 double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z; 464 465 errptr[i] = (float)std::sqrt(a*a + b*b + c*c); 466 } 467 } 468 469 bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const 470 { 471 const float threshold = 0.996f; 472 Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat(); 473 474 for( int inp = 1; inp <= 2; inp++ ) 475 { 476 int j, k, i = count - 1; 477 const Mat* msi = inp == 1 ? &ms1 : &ms2; 478 const Point3f* ptr = msi->ptr<Point3f>(); 479 480 CV_Assert( count <= msi->rows ); 481 482 // check that the i-th selected point does not belong 483 // to a line connecting some previously selected points 484 for(j = 0; j < i; ++j) 485 { 486 Point3f d1 = ptr[j] - ptr[i]; 487 float n1 = d1.x*d1.x + d1.y*d1.y; 488 489 for(k = 0; k < j; ++k) 490 { 491 Point3f d2 = ptr[k] - ptr[i]; 492 float denom = (d2.x*d2.x + d2.y*d2.y)*n1; 493 float num = d1.x*d2.x + d1.y*d2.y; 494 495 if( num*num > threshold*threshold*denom ) 496 return false; 497 } 498 } 499 } 500 return true; 501 } 502 }; 503 504 } 505 506 int cv::estimateAffine3D(InputArray _from, InputArray _to, 507 OutputArray _out, OutputArray _inliers, 508 double param1, double param2) 509 { 510 Mat from = _from.getMat(), to = _to.getMat(); 511 int count = from.checkVector(3); 512 513 CV_Assert( count >= 0 && to.checkVector(3) == count ); 514 515 Mat dFrom, dTo; 516 from.convertTo(dFrom, CV_32F); 517 to.convertTo(dTo, CV_32F); 518 dFrom = dFrom.reshape(3, count); 519 dTo = dTo.reshape(3, count); 520 521 const double epsilon = DBL_EPSILON; 522 param1 = param1 <= 0 ? 3 : param1; 523 param2 = (param2 < epsilon) ? 0.99 : (param2 > 1 - epsilon) ? 0.99 : param2; 524 525 return createRANSACPointSetRegistrator(makePtr<Affine3DEstimatorCallback>(), 4, param1, param2)->run(dFrom, dTo, _out, _inliers); 526 } 527
