41#ifndef PCL_REGISTRATION_NDT_IMPL_H_
42#define PCL_REGISTRATION_NDT_IMPL_H_
46template <
typename Po
intSource,
typename Po
intTarget>
56 reg_name_ =
"NormalDistributionsTransform";
71template <
typename Po
intSource,
typename Po
intTarget>
80 const double gauss_c1 = 10 * (1 - outlier_ratio_);
81 const double gauss_c2 = outlier_ratio_ /
pow(resolution_, 3);
88 if (
guess != Eigen::Matrix4f::Identity()) {
90 final_transformation_ =
guess;
96 point_jacobian_.setZero();
98 point_hessian_.setZero();
109 Eigen::Matrix<double, 6, 6> hessian;
115 while (!converged_) {
117 previous_transformation_ = transformation_;
121 Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>>
sv(
122 hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
130 trans_probability_ = score /
static_cast<double>(input_->size());
140 transformation_epsilon_ / 2,
148 convertTransform(
delta, transformation_);
153 if (update_visualizer_)
157 0.5 * transformation_.template
block<3, 3>(0, 0).trace() - 1;
159 transformation_.template
block<3, 1>(0, 3).squaredNorm();
163 if (nr_iterations_ >= max_iterations_ ||
164 ((transformation_epsilon_ > 0 &&
translation_sqr <= transformation_epsilon_) &&
165 (transformation_rotation_epsilon_ > 0 &&
166 cos_angle >= transformation_rotation_epsilon_)) ||
167 ((transformation_epsilon_ <= 0) &&
168 (transformation_rotation_epsilon_ > 0 &&
169 cos_angle >= transformation_rotation_epsilon_)) ||
170 ((transformation_epsilon_ > 0 &&
translation_sqr <= transformation_epsilon_) &&
171 (transformation_rotation_epsilon_ <= 0))) {
179 trans_probability_ = score /
static_cast<double>(input_->size());
182template <
typename Po
intSource,
typename Po
intTarget>
186 Eigen::Matrix<double, 6, 6>& hessian,
188 const Eigen::Matrix<double, 6, 1>& transform,
196 computeAngleDerivatives(transform);
199 for (std::size_t idx = 0; idx < input_->size(); idx++) {
206 std::vector<float> distances;
211 const auto&
x_pt = (*input_)[idx];
215 const Eigen::Vector3d
x_trans =
219 const Eigen::Matrix3d
c_inv =
cell->getInverseCov();
223 computePointDerivatives(x);
233template <
typename Po
intSource,
typename Po
intTarget>
240 if (std::abs(angle) < 10
e-5) {
257 angular_jacobian_.setZero();
258 angular_jacobian_.row(0).noalias() = Eigen::Vector4d(
260 angular_jacobian_.row(1).noalias() = Eigen::Vector4d(
262 angular_jacobian_.row(2).noalias() =
263 Eigen::Vector4d((-
sy *
cz),
sy *
sz, cy, 1.0);
264 angular_jacobian_.row(3).noalias() =
265 Eigen::Vector4d(
sx * cy *
cz, (-
sx * cy *
sz),
sx *
sy, 1.0);
266 angular_jacobian_.row(4).noalias() =
267 Eigen::Vector4d((-cx * cy *
cz), cx * cy *
sz, (-cx *
sy), 1.0);
268 angular_jacobian_.row(5).noalias() =
269 Eigen::Vector4d((-cy *
sz), (-cy *
cz), 0, 1.0);
270 angular_jacobian_.row(6).noalias() =
271 Eigen::Vector4d((cx *
cz -
sx *
sy *
sz), (-cx *
sz -
sx *
sy *
cz), 0, 1.0);
272 angular_jacobian_.row(7).noalias() =
273 Eigen::Vector4d((
sx *
cz + cx *
sy *
sz), (cx *
sy *
cz -
sx *
sz), 0, 1.0);
278 angular_hessian_.setZero();
279 angular_hessian_.row(0).noalias() = Eigen::Vector4d(
281 angular_hessian_.row(1).noalias() = Eigen::Vector4d(
284 angular_hessian_.row(2).noalias() =
285 Eigen::Vector4d((cx * cy *
cz), (-cx * cy *
sz), (cx *
sy), 0.0f);
286 angular_hessian_.row(3).noalias() =
287 Eigen::Vector4d((
sx * cy *
cz), (-
sx * cy *
sz), (
sx *
sy), 0.0f);
289 angular_hessian_.row(4).noalias() = Eigen::Vector4d(
291 angular_hessian_.row(5).noalias() = Eigen::Vector4d(
294 angular_hessian_.row(6).noalias() =
295 Eigen::Vector4d((-cy *
cz), (cy *
sz), (
sy), 0.0f);
296 angular_hessian_.row(7).noalias() =
297 Eigen::Vector4d((-
sx *
sy *
cz), (
sx *
sy *
sz), (
sx * cy), 0.0f);
298 angular_hessian_.row(8).noalias() =
299 Eigen::Vector4d((cx *
sy *
cz), (-cx *
sy *
sz), (-cx * cy), 0.0f);
301 angular_hessian_.row(9).noalias() =
302 Eigen::Vector4d((
sy *
sz), (
sy *
cz), 0, 0.0f);
303 angular_hessian_.row(10).noalias() =
304 Eigen::Vector4d((-
sx * cy *
sz), (-
sx * cy *
cz), 0, 0.0f);
305 angular_hessian_.row(11).noalias() =
306 Eigen::Vector4d((cx * cy *
sz), (cx * cy *
cz), 0, 0.0f);
308 angular_hessian_.row(12).noalias() =
309 Eigen::Vector4d((-cy *
cz), (cy *
sz), 0, 0.0f);
310 angular_hessian_.row(13).noalias() = Eigen::Vector4d(
312 angular_hessian_.row(14).noalias() = Eigen::Vector4d(
317template <
typename Po
intSource,
typename Po
intTarget>
326 angular_jacobian_ * Eigen::Vector4d(x[0], x[1], x[2], 0.0);
338 angular_hessian_ * Eigen::Vector4d(x[0], x[1], x[2], 0.0);
351 point_hessian_.block<3, 1>(9, 3) = a;
352 point_hessian_.block<3, 1>(12, 3) = b;
353 point_hessian_.block<3, 1>(15, 3) =
c;
354 point_hessian_.block<3, 1>(9, 4) = b;
355 point_hessian_.block<3, 1>(12, 4) = d;
356 point_hessian_.block<3, 1>(15, 4) =
e;
357 point_hessian_.block<3, 1>(9, 5) =
c;
358 point_hessian_.block<3, 1>(12, 5) =
e;
359 point_hessian_.block<3, 1>(15, 5) = f;
363template <
typename Po
intSource,
typename Po
intTarget>
367 Eigen::Matrix<double, 6, 6>& hessian,
368 const Eigen::Vector3d&
x_trans,
369 const Eigen::Matrix3d&
c_inv,
388 for (
int i = 0; i < 6; i++) {
397 for (Eigen::Index j = 0; j < hessian.cols(); j++) {
402 x_trans.dot(
c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
411template <
typename Po
intSource,
typename Po
intTarget>
421 for (std::size_t idx = 0; idx < input_->size(); idx++) {
428 std::vector<float> distances;
433 const auto&
x_pt = (*input_)[idx];
437 const Eigen::Vector3d
x_trans =
441 const Eigen::Matrix3d
c_inv =
cell->getInverseCov();
445 computePointDerivatives(x);
453template <
typename Po
intSource,
typename Po
intTarget>
456 Eigen::Matrix<double, 6, 6>& hessian,
457 const Eigen::Vector3d&
x_trans,
458 const Eigen::Matrix3d&
c_inv)
const
472 for (
int i = 0; i < 6; i++) {
477 for (Eigen::Index j = 0; j < hessian.cols(); j++) {
482 x_trans.dot(
c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
488template <
typename Po
intSource,
typename Po
intTarget>
533template <
typename Po
intSource,
typename Po
intTarget>
563 else if (std::fabs(
g_t) <= std::fabs(
g_l)) {
571 case EndpointsCondition::Case1: {
575 const double w = std::sqrt(z * z -
g_t *
g_l);
590 case EndpointsCondition::Case2: {
594 const double w = std::sqrt(z * z -
g_t *
g_l);
608 case EndpointsCondition::Case3: {
612 const double w = std::sqrt(z * z -
g_t *
g_l);
635 case EndpointsCondition::Case4: {
639 const double w = std::sqrt(z * z -
g_t *
g_u);
646template <
typename Po
intSource,
typename Po
intTarget>
649 const Eigen::Matrix<double, 6, 1>& x,
650 Eigen::Matrix<double, 6, 1>&
step_dir,
656 Eigen::Matrix<double, 6, 6>& hessian,
660 const double phi_0 = -score;
680 const double mu = 1.e-4;
682 const double nu = 0.9;
706 convertTransform(
x_t, final_transformation_);
718 double phi_t = -score;
747 convertTransform(
x_t, final_transformation_);
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &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.