41 #ifndef PCL_REGISTRATION_NDT_IMPL_H_ 42 #define PCL_REGISTRATION_NDT_IMPL_H_ 45 template<
typename Po
intSource,
typename Po
intTarget>
50 , outlier_ratio_ (0.55)
53 , trans_probability_ ()
54 , j_ang_a_ (), j_ang_b_ (), j_ang_c_ (), j_ang_d_ (), j_ang_e_ (), j_ang_f_ (), j_ang_g_ (), j_ang_h_ ()
55 , h_ang_a2_ (), h_ang_a3_ (), h_ang_b2_ (), h_ang_b3_ (), h_ang_c2_ (), h_ang_c3_ (), h_ang_d1_ (), h_ang_d2_ ()
56 , h_ang_d3_ (), h_ang_e1_ (), h_ang_e2_ (), h_ang_e3_ (), h_ang_f1_ (), h_ang_f2_ (), h_ang_f3_ ()
60 reg_name_ =
"NormalDistributionsTransform";
62 double gauss_c1, gauss_c2, gauss_d3;
67 gauss_d3 = -log (gauss_c2);
68 gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3;
69 gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) /
gauss_d1_);
76 template<
typename Po
intSource,
typename Po
intTarget>
void 82 double gauss_c1, gauss_c2, gauss_d3;
87 gauss_d3 = -log (gauss_c2);
88 gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3;
89 gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) /
gauss_d1_);
91 if (guess != Eigen::Matrix4f::Identity ())
104 Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
108 Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
109 Eigen::Vector3f init_translation = eig_transformation.translation ();
110 Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
111 p << init_translation (0), init_translation (1), init_translation (2),
112 init_rotation (0), init_rotation (1), init_rotation (2);
114 Eigen::Matrix<double, 6, 6> hessian;
128 Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
130 delta_p = sv.solve (-score_gradient);
133 delta_p_norm = delta_p.norm ();
135 if (delta_p_norm == 0 || delta_p_norm != delta_p_norm)
142 delta_p.normalize ();
144 delta_p *= delta_p_norm;
147 transformation_ = (Eigen::Translation<float, 3> (
static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
148 Eigen::AngleAxis<float> (
static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
149 Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
150 Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
181 template<
typename Po
intSource,
typename Po
intTarget>
double 183 Eigen::Matrix<double, 6, 6> &hessian,
185 Eigen::Matrix<double, 6, 1> &p,
186 bool compute_hessian)
189 PointSource x_pt, x_trans_pt;
191 Eigen::Vector3d x, x_trans;
195 Eigen::Matrix3d c_inv;
197 score_gradient.setZero ();
205 for (
size_t idx = 0; idx <
input_->points.size (); idx++)
207 x_trans_pt = trans_cloud.points[idx];
210 std::vector<TargetGridLeafConstPtr> neighborhood;
211 std::vector<float> distances;
214 for (
typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
216 cell = *neighborhood_it;
217 x_pt =
input_->points[idx];
218 x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
220 x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
223 x_trans -= cell->getMean ();
225 c_inv = cell->getInverseCov ();
230 score +=
updateDerivatives (score_gradient, hessian, x_trans, c_inv, compute_hessian);
238 template<
typename Po
intSource,
typename Po
intTarget>
void 242 double cx, cy, cz, sx, sy, sz;
243 if (fabs (p (3)) < 10e-5)
254 if (fabs (p (4)) < 10e-5)
266 if (fabs (p (5)) < 10e-5)
279 j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy);
280 j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy);
281 j_ang_c_ << (-sy * cz), sy * sz, cy;
282 j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy;
283 j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy);
284 j_ang_f_ << (-cy * sz), (-cy * cz), 0;
285 j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0;
286 j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0;
291 h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy;
292 h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy);
294 h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy);
295 h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy);
297 h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0;
298 h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0;
300 h_ang_d1_ << (-cy * cz), (cy * sz), (sy);
301 h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy);
302 h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy);
305 h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0;
306 h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0;
309 h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0;
310 h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0;
315 template<
typename Po
intSource,
typename Po
intTarget>
void 332 Eigen::Vector3d a, b, c, d, e, f;
356 template<
typename Po
intSource,
typename Po
intTarget>
double 358 Eigen::Matrix<double, 6, 6> &hessian,
359 Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv,
360 bool compute_hessian)
362 Eigen::Vector3d cov_dxd_pi;
364 double e_x_cov_x = exp (-
gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2);
366 double score_inc = -
gauss_d1_ * e_x_cov_x;
371 if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x)
378 for (
int i = 0; i < 6; i++)
384 score_gradient (i) += x_trans.dot (cov_dxd_pi) * e_x_cov_x;
388 for (
int j = 0; j < hessian.cols (); j++)
391 hessian (i, j) += e_x_cov_x * (-
gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv *
point_gradient_.col (j)) +
402 template<
typename Po
intSource,
typename Po
intTarget>
void 407 PointSource x_pt, x_trans_pt;
409 Eigen::Vector3d x, x_trans;
413 Eigen::Matrix3d c_inv;
420 for (
size_t idx = 0; idx <
input_->points.size (); idx++)
422 x_trans_pt = trans_cloud.points[idx];
425 std::vector<TargetGridLeafConstPtr> neighborhood;
426 std::vector<float> distances;
429 for (
typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
431 cell = *neighborhood_it;
434 x_pt =
input_->points[idx];
435 x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
437 x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
440 x_trans -= cell->getMean ();
442 c_inv = cell->getInverseCov ();
454 template<
typename Po
intSource,
typename Po
intTarget>
void 457 Eigen::Vector3d cov_dxd_pi;
462 if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x)
468 for (
int i = 0; i < 6; i++)
473 for (
int j = 0; j < hessian.cols (); j++)
476 hessian (i, j) += e_x_cov_x * (-
gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv *
point_gradient_.col (j)) +
485 template<
typename Po
intSource,
typename Po
intTarget>
bool 487 double &a_u,
double &f_u,
double &g_u,
488 double a_t,
double f_t,
double g_t)
500 if (g_t * (a_l - a_t) > 0)
509 if (g_t * (a_l - a_t) < 0)
526 template<
typename Po
intSource,
typename Po
intTarget>
double 528 double a_u,
double f_u,
double g_u,
529 double a_t,
double f_t,
double g_t)
536 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
537 double w = std::sqrt (z * z - g_t * g_l);
539 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
543 double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
545 if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l))
548 return (0.5 * (a_q + a_c));
556 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
557 double w = std::sqrt (z * z - g_t * g_l);
559 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
563 double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
565 if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t))
572 if (std::fabs (g_t) <= std::fabs (g_l))
576 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
577 double w = std::sqrt (z * z - g_t * g_l);
578 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
582 double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
586 if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t))
592 return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next));
594 return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next));
601 double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
602 double w = std::sqrt (z * z - g_t * g_u);
604 return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w));
609 template<
typename Po
intSource,
typename Po
intTarget>
double 611 double step_min,
double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
615 double phi_0 = -score;
617 double d_phi_0 = -(score_gradient.dot (step_dir));
619 Eigen::Matrix<double, 6, 1> x_t;
637 int max_step_iterations = 10;
638 int step_iterations = 0;
646 double a_l = 0, a_u = 0;
656 bool interval_converged = (step_max - step_min) > 0, open_interval =
true;
658 double a_t = step_init;
659 a_t = std::min (a_t, step_max);
660 a_t = std::max (a_t, step_min);
662 x_t = x + step_dir * a_t;
664 final_transformation_ = (Eigen::Translation<float, 3>(
static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
665 Eigen::AngleAxis<float> (
static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
666 Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
667 Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
677 double phi_t = -score;
679 double d_phi_t = -(score_gradient.dot (step_dir));
687 while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 && d_phi_t <= -nu * d_phi_0 ))
694 a_t, psi_t, d_psi_t);
700 a_t, phi_t, d_phi_t);
703 a_t = std::min (a_t, step_max);
704 a_t = std::max (a_t, step_min);
706 x_t = x + step_dir * a_t;
708 final_transformation_ = (Eigen::Translation<float, 3> (
static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
709 Eigen::AngleAxis<float> (
static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
710 Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
711 Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
723 d_phi_t = -(score_gradient.dot (step_dir));
731 if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
733 open_interval =
false;
736 f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
737 g_l = g_l + mu * d_phi_0;
740 f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
741 g_u = g_u + mu * d_phi_0;
749 a_t, psi_t, d_psi_t);
756 a_t, phi_t, d_phi_t);
771 #endif // PCL_REGISTRATION_NDT_IMPL_H_
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
Matrix4 transformation_
The transformation matrix estimated by the registration method.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
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=true)
Apply an affine transform defined by an Eigen Transform.
double transformation_rotation_epsilon_
The maximum rotation difference between two consecutive transformations in order to consider converge...
bool converged_
Holds internal convergence state, given user parameters.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
std::string reg_name_
The registration method name.
boost::function< void(const pcl::PointCloud< PointSource > &cloud_src, const std::vector< int > &indices_src, const pcl::PointCloud< PointTarget > &cloud_tgt, const std::vector< int > &indices_tgt)> update_visualizer_
Callback function to update intermediate source point cloud position during it's registration to the ...
PointCloudConstPtr input_
The input point cloud dataset.
int radiusSearch(const PointT &point, double radius, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all the nearest occupied voxels of the query point in a given radius.