449 PointSource query = output[i];
450 query.getVector4fMap() =
451 transformation_.template cast<float>() * query.getVector4fMap();
453 if (!searchForNeighbors(query, nn_indices, nn_dists)) {
454 PCL_ERROR(
"[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
455 "in the target dataset for point %d in the source!\n",
456 getClassName().c_str(),
463 if (nn_dists[0] < dist_threshold) {
464 Eigen::Matrix3d& C1 = (*input_covariances_)[i];
465 Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
466 Eigen::Matrix3d& M = mahalanobis_[i];
470 Eigen::Matrix3d temp = M * R.transpose();
474 source_indices[cnt] =
static_cast<int>(i);
475 target_indices[cnt] = nn_indices[0];
480 source_indices.resize(cnt);
481 target_indices.resize(cnt);
483 previous_transformation_ = transformation_;
486 rigid_transformation_estimation_(
487 output, source_indices, *target_, target_indices, transformation_);
490 for (
int k = 0; k < 4; k++) {
491 for (
int l = 0; l < 4; l++) {
494 ratio = 1. / rotation_epsilon_;
496 ratio = 1. / transformation_epsilon_;
498 ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
503 }
catch (PCLException& e) {
504 PCL_DEBUG(
"[pcl::%s::computeTransformation] Optimization issue %s\n",
505 getClassName().c_str(),
511 if (update_visualizer_ !=
nullptr) {
512 PointCloudSourcePtr input_transformed(
new PointCloudSource);
514 update_visualizer_(*input_transformed, source_indices, *target_, target_indices);
518 if (nr_iterations_ >= max_iterations_ || delta < 1) {
520 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence reached. Number of "
521 "iterations: %d out of %d. Transformation difference: %f\n",
522 getClassName().c_str(),
525 (transformation_ - previous_transformation_).array().abs().sum());
526 previous_transformation_ = transformation_;
529 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence failed\n",
530 getClassName().c_str());
532 final_transformation_ = previous_transformation_ * guess;
534 PCL_DEBUG(
"Transformation "
535 "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%"
536 "5f\t%5f\t%5f\t%5f\n",
537 final_transformation_(0, 0),
538 final_transformation_(0, 1),
539 final_transformation_(0, 2),
540 final_transformation_(0, 3),
541 final_transformation_(1, 0),
542 final_transformation_(1, 1),
543 final_transformation_(1, 2),
544 final_transformation_(1, 3),
545 final_transformation_(2, 0),
546 final_transformation_(2, 1),
547 final_transformation_(2, 2),
548 final_transformation_(2, 3),
549 final_transformation_(3, 0),
550 final_transformation_(3, 1),
551 final_transformation_(3, 2),
552 final_transformation_(3, 3));
558template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
565 AngleAxis(
static_cast<Scalar
>(x[4]), Vector3::UnitY()) *
566 AngleAxis(
static_cast<Scalar
>(x[3]), Vector3::UnitX()))
568 Matrix4 T = Matrix4::Identity();
569 T.template block<3, 3>(0, 0) = R;
570 T.template block<3, 1>(0, 3) =
Vector3(
571 static_cast<Scalar
>(x[0]),
static_cast<Scalar
>(x[1]),
static_cast<Scalar
>(x[2]));