393 PointCloudSource& output,
const Eigen::Matrix4f& guess)
395 PointCloudSource intm_cloud = output;
400 if (guess != Eigen::Matrix4f::Identity()) {
401 transformation_ = guess;
410 Eigen::Matrix4f& transformation = transformation_;
414 const Eigen::Matrix3f initial_rot(transformation.block<3, 3>(0, 0));
415 const Eigen::Vector3f rot_x(initial_rot * Eigen::Vector3f::UnitX());
416 const double z_rotation = std::atan2(rot_x[1], rot_x[0]);
418 Eigen::Vector3d xytheta_transformation(
419 transformation(0, 3), transformation(1, 3), z_rotation);
421 while (!converged_) {
422 const double cos_theta = std::cos(xytheta_transformation[2]);
423 const double sin_theta = std::sin(xytheta_transformation[2]);
424 previous_transformation_ = transformation;
428 for (std::size_t i = 0; i < intm_cloud.size(); i++)
429 score += target_ndt.
test(intm_cloud[i], cos_theta, sin_theta);
431 PCL_DEBUG(
"[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score "
432 "%f (x=%f,y=%f,r=%f)\n",
434 xytheta_transformation[0],
435 xytheta_transformation[1],
436 xytheta_transformation[2]);
438 if (score.
value != 0) {
440 Eigen::EigenSolver<Eigen::Matrix3d> solver;
441 solver.compute(score.
hessian,
false);
442 double min_eigenvalue = 0;
443 for (
int i = 0; i < 3; i++)
444 if (solver.eigenvalues()[i].real() < min_eigenvalue)
445 min_eigenvalue = solver.eigenvalues()[i].real();
449 if (min_eigenvalue < 0) {
450 double lambda = 1.1 * min_eigenvalue - 1;
451 score.
hessian += Eigen::Vector3d(-lambda, -lambda, -lambda).asDiagonal();
452 solver.compute(score.
hessian,
false);
453 PCL_DEBUG(
"[pcl::NormalDistributionsTransform2D::computeTransformation] adjust "
454 "hessian: %f: new eigenvalues:%f %f %f\n",
456 solver.eigenvalues()[0].real(),
457 solver.eigenvalues()[1].real(),
458 solver.eigenvalues()[2].real());
460 assert(solver.eigenvalues()[0].real() >= 0 &&
461 solver.eigenvalues()[1].real() >= 0 &&
462 solver.eigenvalues()[2].real() >= 0);
464 Eigen::Vector3d delta_transformation(-score.
hessian.inverse() * score.
grad);
465 Eigen::Vector3d new_transformation =
466 xytheta_transformation + newton_lambda_.cwiseProduct(delta_transformation);
468 xytheta_transformation = new_transformation;
471 transformation.block<3, 3>(0, 0).matrix() = Eigen::Matrix3f(Eigen::AngleAxisf(
472 static_cast<float>(xytheta_transformation[2]), Eigen::Vector3f::UnitZ()));
473 transformation.block<3, 1>(0, 3).matrix() =
474 Eigen::Vector3f(
static_cast<float>(xytheta_transformation[0]),
475 static_cast<float>(xytheta_transformation[1]),
481 PCL_ERROR(
"[pcl::NormalDistributionsTransform2D::computeTransformation] no "
482 "overlap: try increasing the size or reducing the step of the grid\n");
490 if (update_visualizer_)
491 update_visualizer_(output, *indices_, *target_, *indices_);
496 Eigen::Matrix4f transformation_delta =
497 transformation.inverse() * previous_transformation_;
499 0.5 * (transformation_delta.coeff(0, 0) + transformation_delta.coeff(1, 1) +
500 transformation_delta.coeff(2, 2) - 1);
501 double translation_sqr =
502 transformation_delta.coeff(0, 3) * transformation_delta.coeff(0, 3) +
503 transformation_delta.coeff(1, 3) * transformation_delta.coeff(1, 3) +
504 transformation_delta.coeff(2, 3) * transformation_delta.coeff(2, 3);
506 if (nr_iterations_ >= max_iterations_ ||
507 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
508 (transformation_rotation_epsilon_ > 0 &&
509 cos_angle >= transformation_rotation_epsilon_)) ||
510 ((transformation_epsilon_ <= 0) &&
511 (transformation_rotation_epsilon_ > 0 &&
512 cos_angle >= transformation_rotation_epsilon_)) ||
513 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
514 (transformation_rotation_epsilon_ <= 0))) {
518 final_transformation_ = transformation;