41 #ifndef PCL_REGISTRATION_NDT_IMPL_H_
42 #define PCL_REGISTRATION_NDT_IMPL_H_
46 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
51 reg_name_ =
"NormalDistributionsTransform";
56 const double gauss_d3 = -std::log(gauss_c2);
57 gauss_d1_ = -std::log(gauss_c1 + gauss_c2) - gauss_d3;
59 -2 * std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) /
66 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
73 if (target_cells_.getCentroids()->empty()) {
74 PCL_ERROR(
"[%s::computeTransformation] Voxel grid is not searchable!\n",
75 getClassName().c_str());
80 const double gauss_c1 = 10 * (1 - outlier_ratio_);
81 const double gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
82 const double gauss_d3 = -std::log(gauss_c2);
83 gauss_d1_ = -std::log(gauss_c1 + gauss_c2) - gauss_d3;
85 -2 * std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) /
88 if (guess != Matrix4::Identity()) {
90 final_transformation_ = guess;
96 point_jacobian_.setZero();
97 point_jacobian_.block<3, 3>(0, 0).setIdentity();
98 point_hessian_.setZero();
100 Eigen::Transform<Scalar, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
101 eig_transformation.matrix() = final_transformation_;
104 Eigen::Matrix<double, 6, 1> transform, score_gradient;
105 Vector3 init_translation = eig_transformation.translation();
106 Vector3 init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2);
107 transform << init_translation.template cast<double>(),
108 init_rotation.template cast<double>();
110 Eigen::Matrix<double, 6, 6> hessian;
114 double score = computeDerivatives(score_gradient, hessian, output, transform);
116 while (!converged_) {
118 previous_transformation_ = transformation_;
122 Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> sv(
123 hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
124 #if EIGEN_VERSION_AT_LEAST(3, 4, 0)
125 if (sv.info() != Eigen::ComputationInfo::Success) {
126 trans_likelihood_ = score /
static_cast<double>(input_->size());
128 PCL_ERROR(
"[%s::computeTransformation] JacobiSVD on hessian failed!\n",
129 getClassName().c_str());
134 Eigen::Matrix<double, 6, 1> delta = sv.solve(-score_gradient);
137 double delta_norm = delta.norm();
139 if (delta_norm == 0 || std::isnan(delta_norm)) {
140 trans_likelihood_ = score /
static_cast<double>(input_->size());
141 converged_ = delta_norm == 0;
146 delta_norm = computeStepLengthMT(transform,
150 transformation_epsilon_ / 2,
158 convertTransform(delta, transformation_);
163 if (update_visualizer_)
166 const double cos_angle =
167 0.5 * (transformation_.template block<3, 3>(0, 0).trace() - 1);
168 const double translation_sqr =
169 transformation_.template block<3, 1>(0, 3).squaredNorm();
173 if (nr_iterations_ >= max_iterations_ ||
174 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
175 (transformation_rotation_epsilon_ > 0 &&
176 cos_angle >= transformation_rotation_epsilon_)) ||
177 ((transformation_epsilon_ <= 0) &&
178 (transformation_rotation_epsilon_ > 0 &&
179 cos_angle >= transformation_rotation_epsilon_)) ||
180 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
181 (transformation_rotation_epsilon_ <= 0))) {
189 trans_likelihood_ = score /
static_cast<double>(input_->size());
192 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
195 Eigen::Matrix<double, 6, 1>& score_gradient,
196 Eigen::Matrix<double, 6, 6>& hessian,
198 const Eigen::Matrix<double, 6, 1>& transform,
199 bool compute_hessian)
201 score_gradient.setZero();
206 computeAngleDerivatives(transform);
209 for (std::size_t idx = 0; idx < input_->size(); idx++) {
211 const auto& x_trans_pt = trans_cloud[idx];
215 std::vector<TargetGridLeafConstPtr> neighborhood;
216 std::vector<float> distances;
217 target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
219 for (
const auto& cell : neighborhood) {
221 const auto& x_pt = (*input_)[idx];
222 const Eigen::Vector3d x = x_pt.getVector3fMap().template cast<double>();
225 const Eigen::Vector3d x_trans =
226 x_trans_pt.getVector3fMap().template cast<double>() - cell->getMean();
229 const Eigen::Matrix3d c_inv = cell->getInverseCov();
233 computePointDerivatives(x);
237 updateDerivatives(score_gradient, hessian, x_trans, c_inv, compute_hessian);
243 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
246 const Eigen::Matrix<double, 6, 1>& transform,
bool compute_hessian)
249 const auto calculate_cos_sin = [](
double angle,
double& c,
double& s) {
250 if (std::abs(angle) < 10e-5) {
260 double cx, cy, cz, sx, sy, sz;
261 calculate_cos_sin(transform(3), cx, sx);
262 calculate_cos_sin(transform(4), cy, sy);
263 calculate_cos_sin(transform(5), cz, sz);
267 angular_jacobian_.setZero();
268 angular_jacobian_.row(0).noalias() = Eigen::Vector4d(
269 (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 1.0);
270 angular_jacobian_.row(1).noalias() = Eigen::Vector4d(
271 (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 1.0);
272 angular_jacobian_.row(2).noalias() =
273 Eigen::Vector4d((-sy * cz), sy * sz, cy, 1.0);
274 angular_jacobian_.row(3).noalias() =
275 Eigen::Vector4d(sx * cy * cz, (-sx * cy * sz), sx * sy, 1.0);
276 angular_jacobian_.row(4).noalias() =
277 Eigen::Vector4d((-cx * cy * cz), cx * cy * sz, (-cx * sy), 1.0);
278 angular_jacobian_.row(5).noalias() =
279 Eigen::Vector4d((-cy * sz), (-cy * cz), 0, 1.0);
280 angular_jacobian_.row(6).noalias() =
281 Eigen::Vector4d((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 1.0);
282 angular_jacobian_.row(7).noalias() =
283 Eigen::Vector4d((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 1.0);
285 if (compute_hessian) {
288 angular_hessian_.setZero();
289 angular_hessian_.row(0).noalias() = Eigen::Vector4d(
290 (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f);
291 angular_hessian_.row(1).noalias() = Eigen::Vector4d(
292 (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f);
294 angular_hessian_.row(2).noalias() =
295 Eigen::Vector4d((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f);
296 angular_hessian_.row(3).noalias() =
297 Eigen::Vector4d((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f);
300 angular_hessian_.row(4).noalias() = Eigen::Vector4d(
301 (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f);
302 angular_hessian_.row(5).noalias() = Eigen::Vector4d(
303 (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f);
305 angular_hessian_.row(6).noalias() =
306 Eigen::Vector4d((-cy * cz), (cy * sz), (-sy), 0.0f);
307 angular_hessian_.row(7).noalias() =
308 Eigen::Vector4d((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f);
309 angular_hessian_.row(8).noalias() =
310 Eigen::Vector4d((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f);
312 angular_hessian_.row(9).noalias() =
313 Eigen::Vector4d((sy * sz), (sy * cz), 0, 0.0f);
314 angular_hessian_.row(10).noalias() =
315 Eigen::Vector4d((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f);
316 angular_hessian_.row(11).noalias() =
317 Eigen::Vector4d((cx * cy * sz), (cx * cy * cz), 0, 0.0f);
319 angular_hessian_.row(12).noalias() =
320 Eigen::Vector4d((-cy * cz), (cy * sz), 0, 0.0f);
321 angular_hessian_.row(13).noalias() = Eigen::Vector4d(
322 (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f);
323 angular_hessian_.row(14).noalias() = Eigen::Vector4d(
324 (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f);
328 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
331 const Eigen::Vector3d& x,
bool compute_hessian)
336 Eigen::Matrix<double, 8, 1> point_angular_jacobian =
337 angular_jacobian_ * Eigen::Vector4d(x[0], x[1], x[2], 0.0);
338 point_jacobian_(1, 3) = point_angular_jacobian[0];
339 point_jacobian_(2, 3) = point_angular_jacobian[1];
340 point_jacobian_(0, 4) = point_angular_jacobian[2];
341 point_jacobian_(1, 4) = point_angular_jacobian[3];
342 point_jacobian_(2, 4) = point_angular_jacobian[4];
343 point_jacobian_(0, 5) = point_angular_jacobian[5];
344 point_jacobian_(1, 5) = point_angular_jacobian[6];
345 point_jacobian_(2, 5) = point_angular_jacobian[7];
347 if (compute_hessian) {
348 Eigen::Matrix<double, 15, 1> point_angular_hessian =
349 angular_hessian_ * Eigen::Vector4d(x[0], x[1], x[2], 0.0);
352 const Eigen::Vector3d a(0, point_angular_hessian[0], point_angular_hessian[1]);
353 const Eigen::Vector3d b(0, point_angular_hessian[2], point_angular_hessian[3]);
354 const Eigen::Vector3d c(0, point_angular_hessian[4], point_angular_hessian[5]);
355 const Eigen::Vector3d d = point_angular_hessian.block<3, 1>(6, 0);
356 const Eigen::Vector3d e = point_angular_hessian.block<3, 1>(9, 0);
357 const Eigen::Vector3d f = point_angular_hessian.block<3, 1>(12, 0);
362 point_hessian_.block<3, 1>(9, 3) = a;
363 point_hessian_.block<3, 1>(12, 3) = b;
364 point_hessian_.block<3, 1>(15, 3) = c;
365 point_hessian_.block<3, 1>(9, 4) = b;
366 point_hessian_.block<3, 1>(12, 4) = d;
367 point_hessian_.block<3, 1>(15, 4) = e;
368 point_hessian_.block<3, 1>(9, 5) = c;
369 point_hessian_.block<3, 1>(12, 5) = e;
370 point_hessian_.block<3, 1>(15, 5) = f;
374 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
377 Eigen::Matrix<double, 6, 1>& score_gradient,
378 Eigen::Matrix<double, 6, 6>& hessian,
379 const Eigen::Vector3d& x_trans,
380 const Eigen::Matrix3d& c_inv,
381 bool compute_hessian)
const
384 double e_x_cov_x = std::exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
387 const double score_inc = -gauss_d1_ * e_x_cov_x;
389 e_x_cov_x = gauss_d2_ * e_x_cov_x;
392 if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x)) {
397 e_x_cov_x *= gauss_d1_;
399 for (
int i = 0; i < 6; i++) {
402 const Eigen::Vector3d cov_dxd_pi = c_inv * point_jacobian_.col(i);
405 score_gradient(i) += x_trans.dot(cov_dxd_pi) * e_x_cov_x;
407 if (compute_hessian) {
408 for (Eigen::Index j = 0; j < hessian.cols(); j++) {
411 e_x_cov_x * (-gauss_d2_ * x_trans.dot(cov_dxd_pi) *
412 x_trans.dot(c_inv * point_jacobian_.col(j)) +
413 x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
414 point_jacobian_.col(j).dot(cov_dxd_pi));
422 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
432 for (std::size_t idx = 0; idx < input_->size(); idx++) {
434 const auto& x_trans_pt = trans_cloud[idx];
438 std::vector<TargetGridLeafConstPtr> neighborhood;
439 std::vector<float> distances;
440 target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
442 for (
const auto& cell : neighborhood) {
444 const auto& x_pt = (*input_)[idx];
445 const Eigen::Vector3d x = x_pt.getVector3fMap().template cast<double>();
448 const Eigen::Vector3d x_trans =
449 x_trans_pt.getVector3fMap().template cast<double>() - cell->getMean();
452 const Eigen::Matrix3d c_inv = cell->getInverseCov();
456 computePointDerivatives(x);
459 updateHessian(hessian, x_trans, c_inv);
464 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
467 Eigen::Matrix<double, 6, 6>& hessian,
468 const Eigen::Vector3d& x_trans,
469 const Eigen::Matrix3d& c_inv)
const
473 gauss_d2_ * std::exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
476 if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x)) {
481 e_x_cov_x *= gauss_d1_;
483 for (
int i = 0; i < 6; i++) {
486 const Eigen::Vector3d cov_dxd_pi = c_inv * point_jacobian_.col(i);
488 for (Eigen::Index j = 0; j < hessian.cols(); j++) {
491 e_x_cov_x * (-gauss_d2_ * x_trans.dot(cov_dxd_pi) *
492 x_trans.dot(c_inv * point_jacobian_.col(j)) +
493 x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
494 point_jacobian_.col(j).dot(cov_dxd_pi));
499 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
522 if (g_t * (a_l - a_t) > 0) {
530 if (g_t * (a_l - a_t) < 0) {
544 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
557 if (a_t == a_l && a_t == a_u) {
562 enum class EndpointsCondition { Case1, Case2, Case3, Case4 };
563 EndpointsCondition condition;
566 condition = EndpointsCondition::Case4;
568 else if (f_t > f_l) {
569 condition = EndpointsCondition::Case1;
571 else if (g_t * g_l < 0) {
572 condition = EndpointsCondition::Case2;
574 else if (std::fabs(g_t) <= std::fabs(g_l)) {
575 condition = EndpointsCondition::Case3;
578 condition = EndpointsCondition::Case4;
582 case EndpointsCondition::Case1: {
585 const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
586 const double w = std::sqrt(z * z - g_t * g_l);
588 const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
593 a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
595 if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) {
598 return 0.5 * (a_q + a_c);
601 case EndpointsCondition::Case2: {
604 const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
605 const double w = std::sqrt(z * z - g_t * g_l);
607 const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
611 const double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
613 if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) {
619 case EndpointsCondition::Case3: {
622 const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
623 const double w = std::sqrt(z * z - g_t * g_l);
624 const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
628 const double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
632 if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) {
640 return std::min(a_t + 0.66 * (a_u - a_t), a_t_next);
642 return std::max(a_t + 0.66 * (a_u - a_t), a_t_next);
646 case EndpointsCondition::Case4: {
649 const double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
650 const double w = std::sqrt(z * z - g_t * g_u);
652 return a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w);
657 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
660 const Eigen::Matrix<double, 6, 1>& x,
661 Eigen::Matrix<double, 6, 1>& step_dir,
666 Eigen::Matrix<double, 6, 1>& score_gradient,
667 Eigen::Matrix<double, 6, 6>& hessian,
671 const double phi_0 = -score;
673 double d_phi_0 = -(score_gradient.dot(step_dir));
687 constexpr
int max_step_iterations = 10;
688 int step_iterations = 0;
691 constexpr
double mu = 1.e-4;
693 constexpr
double nu = 0.9;
696 double a_l = 0, a_u = 0;
700 double f_l = auxilaryFunction_PsiMT(a_l, phi_0, phi_0, d_phi_0, mu);
701 double g_l = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu);
703 double f_u = auxilaryFunction_PsiMT(a_u, phi_0, phi_0, d_phi_0, mu);
704 double g_u = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu);
708 bool interval_converged = (step_max - step_min) < 0, open_interval =
true;
710 double a_t = step_init;
711 a_t = std::min(a_t, step_max);
712 a_t = std::max(a_t, step_min);
714 Eigen::Matrix<double, 6, 1> x_t = x + step_dir * a_t;
717 convertTransform(x_t, final_transformation_);
726 score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t,
true);
729 double phi_t = -score;
731 double d_phi_t = -(score_gradient.dot(step_dir));
734 double psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
736 double d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);
741 while (!interval_converged && step_iterations < max_step_iterations &&
743 d_phi_t > -nu * d_phi_0 )) {
746 a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
749 a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
752 a_t = std::min(a_t, step_max);
753 a_t = std::max(a_t, step_min);
755 x_t = x + step_dir * a_t;
758 convertTransform(x_t, final_transformation_);
765 score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t,
false);
770 d_phi_t = -(score_gradient.dot(step_dir));
773 psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
775 d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);
778 if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) {
779 open_interval =
false;
782 f_l += phi_0 - mu * d_phi_0 * a_l;
786 f_u += phi_0 - mu * d_phi_0 * a_u;
793 updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
799 updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
808 if (step_iterations) {
809 computeHessian(hessian, trans_cloud);
std::string reg_name_
The registration method name.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
IndicesAllocator<> Indices
Type used for indices in PCL.