/** performs a single alignment of the measurement to the model.
* The model needs to be added using addToModel before this call.
*
* @param measurement - the mesh that needs to be matched
* @param max_iter - maximum number of iterations
* @param min_mse - minimum average square distance between points after which to stop
* @param min_mse_diff - minimum difference in average square distance between points after which to stop
* @param overlap - percentage of overlap, range between [0..1]. A value of
* 0.95 will discard 5% of the pairs with the worst matches
*/
Result _align( _Adapter measurement, size_t max_iter, double min_mse, double min_mse_diff, double overlap )
{
Result result;
Pairs pairs;
result.C_global2globalnew = Eigen::Affine3d::Identity();
result.iter = 0;
result.overlap = overlap;
result.d_box = std::numeric_limits<double>::infinity();
result.mse_diff = result.mse = std::numeric_limits<double>::infinity();
double old_mse = result.mse;
while( result.iter < max_iter && result.mse > min_mse && result.mse_diff > min_mse_diff )
{
old_mse = result.mse;
pairs.clear();
findPairs.findPairs( measurement, pairs, result.d_box );
const size_t n_po = measurement.size() * result.overlap;
result.d_box = pairs.trim( n_po ) * 2.0;
result.pairs = pairs.size();
if( result.pairs < Pairs::MIN_PAIRS )
return result;
Eigen::Affine3d C_globalprev2globalnew = pairs.getTransform();
result.C_global2globalnew = C_globalprev2globalnew * result.C_global2globalnew;
measurement.setOffsetTransform( result.C_global2globalnew );
result.mse = pairs.getMeanSquareError();
result.mse_diff = old_mse - result.mse;
result.iter++;
// std::cout
// << "points: " << measurement.size()
// << "\titer: " << result.iter
// << "\tpairs: " << pairs.size()
// << "\tmse: " << result.mse
// << "\tmse_diff: " << result.mse_diff
// << "\td_box: " << result.d_box
// << "\toverlap: " << result.overlap
// << std::endl;
}
// std::cout
// << std::endl;
std::vector<double> pairs_distance;
for( size_t i = 0; i < pairs.size(); i++ ) {
pairs_distance.push_back( pairs.pairs[i].distance );
}
result.pairs_distance = pairs_distance;
return result;
}
