| 379 | } |
| 380 | |
| 381 | void ICP::calcGen_( float (ICP::*dist)() const, bool (ICP::*iter)() ) |
| 382 | { |
| 383 | updatePointPairs(); |
| 384 | float minDist = (this->*dist)(); |
| 385 | |
| 386 | int badIterCount = 0; |
| 387 | resultType_ = ICPExitType::MaxIterations; |
| 388 | AffineXf3f resXf = flt_.xf; |
| 389 | for ( iter_ = 1; iter_ <= prop_.iterLimit; ++iter_ ) |
| 390 | { |
| 391 | if ( !(this->*iter)() ) |
| 392 | { |
| 393 | resultType_ = ICPExitType::NotFoundSolution; |
| 394 | break; |
| 395 | } |
| 396 | // without this call, getMeanSqDistToPoint()/getMeanSqDistToPlane() will ignore xf changed in p2ptIter_()/p2plIter_() |
| 397 | updatePointPairs(); |
| 398 | |
| 399 | const float curDist = (this->*dist)(); |
| 400 | |
| 401 | // exit if several(3) iterations didn't decrease minimization parameter |
| 402 | if ( curDist < minDist ) |
| 403 | { |
| 404 | resXf = flt_.xf; |
| 405 | minDist = curDist; |
| 406 | badIterCount = 0; |
| 407 | |
| 408 | if ( prop_.exitVal > curDist ) |
| 409 | { |
| 410 | resultType_ = ICPExitType::StopMsdReached; |
| 411 | break; |
| 412 | } |
| 413 | } |
| 414 | else |
| 415 | { |
| 416 | if ( badIterCount >= prop_.badIterStopCount ) |
| 417 | { |
| 418 | resultType_ = ICPExitType::MaxBadIterations; |
| 419 | break; |
| 420 | } |
| 421 | badIterCount++; |
| 422 | } |
| 423 | } |
| 424 | flt_.xf = resXf; |
| 425 | } |
| 426 | |
| 427 | void ICP::calcP2Pt_() |
| 428 | { |
nothing calls this directly
no test coverage detected