451 PointSource query = output[i];
452 query.getVector4fMap() =
453 transformation_.template cast<float>() * query.getVector4fMap();
455 if (!searchForNeighbors(query, nn_indices, nn_dists)) {
456 PCL_ERROR(
"[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
457 "in the target dataset for point %d in the source!\n",
458 getClassName().c_str(),
465 if (nn_dists[0] < dist_threshold) {
466 Eigen::Matrix3d& C1 = (*input_covariances_)[i];
467 Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
468 Eigen::Matrix3d& M = mahalanobis_[i];
472 Eigen::Matrix3d temp = M * R.transpose();
476 source_indices[cnt] =
static_cast<int>(i);
477 target_indices[cnt] = nn_indices[0];
482 source_indices.resize(cnt);
483 target_indices.resize(cnt);
485 previous_transformation_ = transformation_;
488 rigid_transformation_estimation_(
489 output, source_indices, *target_, target_indices, transformation_);
492 for (
int k = 0; k < 4; k++) {
493 for (
int l = 0; l < 4; l++) {
496 ratio = 1. / rotation_epsilon_;
498 ratio = 1. / transformation_epsilon_;
500 ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
505 }
catch (PCLException& e) {
506 PCL_DEBUG(
"[pcl::%s::computeTransformation] Optimization issue %s\n",
507 getClassName().c_str(),
513 if (update_visualizer_ !=
nullptr) {
514 PointCloudSourcePtr input_transformed(
new PointCloudSource);
516 update_visualizer_(*input_transformed, source_indices, *target_, target_indices);
520 if (nr_iterations_ >= max_iterations_ || delta < 1) {
522 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence reached. Number of "
523 "iterations: %d out of %d. Transformation difference: %f\n",
524 getClassName().c_str(),
527 (transformation_ - previous_transformation_).array().abs().sum());
528 previous_transformation_ = transformation_;
531 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence failed\n",
532 getClassName().c_str());
534 final_transformation_ = previous_transformation_ * guess;
536 PCL_DEBUG(
"Transformation "
537 "is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%"
538 "5f\t%5f\t%5f\t%5f\n",
539 final_transformation_(0, 0),
540 final_transformation_(0, 1),
541 final_transformation_(0, 2),
542 final_transformation_(0, 3),
543 final_transformation_(1, 0),
544 final_transformation_(1, 1),
545 final_transformation_(1, 2),
546 final_transformation_(1, 3),
547 final_transformation_(2, 0),
548 final_transformation_(2, 1),
549 final_transformation_(2, 2),
550 final_transformation_(2, 3),
551 final_transformation_(3, 0),
552 final_transformation_(3, 1),
553 final_transformation_(3, 2),
554 final_transformation_(3, 3));
560template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
567 AngleAxis(
static_cast<Scalar
>(x[4]), Vector3::UnitY()) *
568 AngleAxis(
static_cast<Scalar
>(x[3]), Vector3::UnitX()))
570 Matrix4 T = Matrix4::Identity();
571 T.template block<3, 3>(0, 0) = R;
572 T.template block<3, 1>(0, 3) =
Vector3(
573 static_cast<Scalar
>(x[0]),
static_cast<Scalar
>(x[1]),
static_cast<Scalar
>(x[2]));