41 #ifndef PCL_REGISTRATION_NDT_IMPL_H_ 42 #define PCL_REGISTRATION_NDT_IMPL_H_ 48 template<
typename Po
intSource,
typename Po
intTarget>
53 , outlier_ratio_ (0.55)
56 , trans_probability_ ()
58 reg_name_ =
"NormalDistributionsTransform";
60 double gauss_c1, gauss_c2, gauss_d3;
65 gauss_d3 = -std::log (gauss_c2);
66 gauss_d1_ = -std::log ( gauss_c1 + gauss_c2 ) - gauss_d3;
67 gauss_d2_ = -2 * std::log ((-std::log ( gauss_c1 * std::exp ( -0.5 ) + gauss_c2 ) - gauss_d3) /
gauss_d1_);
74 template<
typename Po
intSource,
typename Po
intTarget>
void 80 double gauss_c1, gauss_c2, gauss_d3;
83 gauss_c1 = 10 * (1 - outlier_ratio_);
84 gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
85 gauss_d3 = -std::log (gauss_c2);
86 gauss_d1_ = -std::log ( gauss_c1 + gauss_c2 ) - gauss_d3;
87 gauss_d2_ = -2 * std::log ((-std::log ( gauss_c1 * std::exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_);
89 if (guess != Eigen::Matrix4f::Identity ())
92 final_transformation_ = guess;
98 point_gradient_.setZero ();
99 point_gradient_.block<3, 3>(0, 0).setIdentity ();
100 point_hessian_.setZero ();
102 Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
103 eig_transformation.matrix () = final_transformation_;
106 Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
107 Eigen::Vector3f init_translation = eig_transformation.translation ();
108 Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
109 p << init_translation (0), init_translation (1), init_translation (2),
110 init_rotation (0), init_rotation (1), init_rotation (2);
112 Eigen::Matrix<double, 6, 6> hessian;
117 score = computeDerivatives (score_gradient, hessian, output, p);
122 previous_transformation_ = transformation_;
125 Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
127 delta_p = sv.solve (-score_gradient);
130 double delta_p_norm = delta_p.norm ();
132 if (delta_p_norm == 0 || std::isnan(delta_p_norm))
134 trans_probability_ = score /
static_cast<double> (input_->size ());
135 converged_ = delta_p_norm == delta_p_norm;
139 delta_p.normalize ();
140 delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output);
141 delta_p *= delta_p_norm;
144 transformation_ = (Eigen::Translation<float, 3> (
static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
145 Eigen::AngleAxis<float> (
static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
146 Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
147 Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
153 if (update_visualizer_)
154 update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
156 double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1);
157 double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) +
158 transformation_.coeff (1, 3) * transformation_.coeff (1, 3) +
159 transformation_.coeff (2, 3) * transformation_.coeff (2, 3);
163 if (nr_iterations_ >= max_iterations_ ||
164 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
165 ((transformation_epsilon_ <= 0) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
166 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0)))
174 trans_probability_ = score /
static_cast<double> (input_->size ());
178 template<
typename Po
intSource,
typename Po
intTarget>
double 180 Eigen::Matrix<double, 6, 6> &hessian,
182 Eigen::Matrix<double, 6, 1> &p,
183 bool compute_hessian)
186 PointSource x_pt, x_trans_pt;
188 Eigen::Vector3d x, x_trans;
192 Eigen::Matrix3d c_inv;
194 score_gradient.setZero ();
199 computeAngleDerivatives (p);
202 for (std::size_t idx = 0; idx < input_->size (); idx++)
204 x_trans_pt = trans_cloud[idx];
207 std::vector<TargetGridLeafConstPtr> neighborhood;
208 std::vector<float> distances;
209 target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);
211 for (
typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it)
213 cell = *neighborhood_it;
214 x_pt = (*input_)[idx];
215 x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
217 x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
220 x_trans -= cell->getMean ();
222 c_inv = cell->getInverseCov ();
225 computePointDerivatives (x);
227 score += updateDerivatives (score_gradient, hessian, x_trans, c_inv, compute_hessian);
235 template<
typename Po
intSource,
typename Po
intTarget>
void 239 double cx, cy, cz, sx, sy, sz;
240 if (std::abs (p (3)) < 10e-5)
248 cx = std::cos (p (3));
251 if (std::abs (p (4)) < 10e-5)
259 cy = std::cos (p (4));
263 if (std::abs (p (5)) < 10e-5)
271 cz = std::cos (p (5));
276 j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy);
277 j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy);
278 j_ang_c_ << (-sy * cz), sy * sz, cy;
279 j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy;
280 j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy);
281 j_ang_f_ << (-cy * sz), (-cy * cz), 0;
282 j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0;
283 j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0;
288 h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy;
289 h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy);
291 h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy);
292 h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy);
294 h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0;
295 h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0;
297 h_ang_d1_ << (-cy * cz), (cy * sz), (sy);
298 h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy);
299 h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy);
301 h_ang_e1_ << (sy * sz), (sy * cz), 0;
302 h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0;
303 h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0;
305 h_ang_f1_ << (-cy * cz), (cy * sz), 0;
306 h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0;
307 h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0;
312 template<
typename Po
intSource,
typename Po
intTarget>
void 317 point_gradient_ (1, 3) = x.dot (j_ang_a_);
318 point_gradient_ (2, 3) = x.dot (j_ang_b_);
319 point_gradient_ (0, 4) = x.dot (j_ang_c_);
320 point_gradient_ (1, 4) = x.dot (j_ang_d_);
321 point_gradient_ (2, 4) = x.dot (j_ang_e_);
322 point_gradient_ (0, 5) = x.dot (j_ang_f_);
323 point_gradient_ (1, 5) = x.dot (j_ang_g_);
324 point_gradient_ (2, 5) = x.dot (j_ang_h_);
329 Eigen::Vector3d a, b, c, d, e, f;
331 a << 0, x.dot (h_ang_a2_), x.dot (h_ang_a3_);
332 b << 0, x.dot (h_ang_b2_), x.dot (h_ang_b3_);
333 c << 0, x.dot (h_ang_c2_), x.dot (h_ang_c3_);
334 d << x.dot (h_ang_d1_), x.dot (h_ang_d2_), x.dot (h_ang_d3_);
335 e << x.dot (h_ang_e1_), x.dot (h_ang_e2_), x.dot (h_ang_e3_);
336 f << x.dot (h_ang_f1_), x.dot (h_ang_f2_), x.dot (h_ang_f3_);
340 point_hessian_.block<3, 1>(9, 3) = a;
341 point_hessian_.block<3, 1>(12, 3) = b;
342 point_hessian_.block<3, 1>(15, 3) = c;
343 point_hessian_.block<3, 1>(9, 4) = b;
344 point_hessian_.block<3, 1>(12, 4) = d;
345 point_hessian_.block<3, 1>(15, 4) = e;
346 point_hessian_.block<3, 1>(9, 5) = c;
347 point_hessian_.block<3, 1>(12, 5) = e;
348 point_hessian_.block<3, 1>(15, 5) = f;
353 template<
typename Po
intSource,
typename Po
intTarget>
double 355 Eigen::Matrix<double, 6, 6> &hessian,
356 Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv,
357 bool compute_hessian)
359 Eigen::Vector3d cov_dxd_pi;
361 double e_x_cov_x = std::exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2);
363 double score_inc = -gauss_d1_ * e_x_cov_x;
365 e_x_cov_x = gauss_d2_ * e_x_cov_x;
368 if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x))
372 e_x_cov_x *= gauss_d1_;
375 for (
int i = 0; i < 6; i++)
378 cov_dxd_pi = c_inv * point_gradient_.col (i);
381 score_gradient (i) += x_trans.dot (cov_dxd_pi) * e_x_cov_x;
385 for (Eigen::Index j = 0; j < hessian.cols (); j++)
388 hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) +
389 x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
390 point_gradient_.col (j).dot (cov_dxd_pi) );
399 template<
typename Po
intSource,
typename Po
intTarget>
void 404 PointSource x_pt, x_trans_pt;
406 Eigen::Vector3d x, x_trans;
410 Eigen::Matrix3d c_inv;
417 for (std::size_t idx = 0; idx < input_->size (); idx++)
419 x_trans_pt = trans_cloud[idx];
422 std::vector<TargetGridLeafConstPtr> neighborhood;
423 std::vector<float> distances;
424 target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);
426 for (
typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it)
428 cell = *neighborhood_it;
431 x_pt = (*input_)[idx];
432 x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
434 x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
437 x_trans -= cell->getMean ();
439 c_inv = cell->getInverseCov ();
442 computePointDerivatives (x);
444 updateHessian (hessian, x_trans, c_inv);
451 template<
typename Po
intSource,
typename Po
intTarget>
void 454 Eigen::Vector3d cov_dxd_pi;
456 double e_x_cov_x = gauss_d2_ * std::exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2);
459 if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x))
463 e_x_cov_x *= gauss_d1_;
465 for (
int i = 0; i < 6; i++)
468 cov_dxd_pi = c_inv * point_gradient_.col (i);
470 for (Eigen::Index j = 0; j < hessian.cols (); j++)
473 hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) +
474 x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
475 point_gradient_.col (j).dot (cov_dxd_pi) );
482 template<
typename Po
intSource,
typename Po
intTarget>
bool 484 double &a_u,
double &f_u,
double &g_u,
485 double a_t,
double f_t,
double g_t)
496 if (g_t * (a_l - a_t) > 0)
504 if (g_t * (a_l - a_t) < 0)
520 template<
typename Po
intSource,
typename Po
intTarget>
double 522 double a_u,
double f_u,
double g_u,
523 double a_t,
double f_t,
double g_t)
530 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
531 double w = std::sqrt (z * z - g_t * g_l);
533 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
537 double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
539 if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l))
541 return (0.5 * (a_q + a_c));
548 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
549 double w = std::sqrt (z * z - g_t * g_l);
551 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
555 double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
557 if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t))
562 if (std::fabs (g_t) <= std::fabs (g_l))
566 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
567 double w = std::sqrt (z * z - g_t * g_l);
568 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
572 double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
576 if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t))
582 return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next));
583 return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next));
588 double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
589 double w = std::sqrt (z * z - g_t * g_u);
591 return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w));
595 template<
typename Po
intSource,
typename Po
intTarget>
double 597 double step_min,
double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
601 double phi_0 = -score;
603 double d_phi_0 = -(score_gradient.dot (step_dir));
605 Eigen::Matrix<double, 6, 1> x_t;
620 int max_step_iterations = 10;
621 int step_iterations = 0;
629 double a_l = 0, a_u = 0;
632 double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
633 double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
635 double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
636 double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
639 bool interval_converged = (step_max - step_min) < 0, open_interval =
true;
641 double a_t = step_init;
642 a_t = std::min (a_t, step_max);
643 a_t = std::max (a_t, step_min);
645 x_t = x + step_dir * a_t;
647 final_transformation_ = (Eigen::Translation<float, 3>(
static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
648 Eigen::AngleAxis<float> (
static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
649 Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
650 Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
657 score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t,
true);
660 double phi_t = -score;
662 double d_phi_t = -(score_gradient.dot (step_dir));
665 double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
667 double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
670 while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 && d_phi_t <= -nu * d_phi_0 ))
675 a_t = trialValueSelectionMT (a_l, f_l, g_l,
677 a_t, psi_t, d_psi_t);
681 a_t = trialValueSelectionMT (a_l, f_l, g_l,
683 a_t, phi_t, d_phi_t);
686 a_t = std::min (a_t, step_max);
687 a_t = std::max (a_t, step_min);
689 x_t = x + step_dir * a_t;
691 final_transformation_ = (Eigen::Translation<float, 3> (
static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
692 Eigen::AngleAxis<float> (
static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
693 Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
694 Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
701 score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t,
false);
706 d_phi_t = -(score_gradient.dot (step_dir));
709 psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
711 d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
714 if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
716 open_interval =
false;
719 f_l += phi_0 - mu * d_phi_0 * a_l;
723 f_u += phi_0 - mu * d_phi_0 * a_u;
730 interval_converged = updateIntervalMT (a_l, f_l, g_l,
732 a_t, psi_t, d_psi_t);
737 interval_converged = updateIntervalMT (a_l, f_l, g_l,
739 a_t, phi_t, d_phi_t);
749 computeHessian (hessian, trans_cloud, x_t);
756 #endif // PCL_REGISTRATION_NDT_IMPL_H_
int max_iterations_
The maximum number of iterations the internal optimization should run for.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
std::string reg_name_
The registration method name.