41 #ifndef PCL_NDT_2D_IMPL_H_
42 #define PCL_NDT_2D_IMPL_H_
44 #include <Eigen/Eigenvalues>
56 template <
unsigned N = 3,
typename T =
double>
61 Eigen::Matrix<T, N, 1>
grad;
68 r.
hessian = Eigen::Matrix<T, N, N>::Zero();
69 r.
grad = Eigen::Matrix<T, N, 1>::Zero();
95 template <
typename Po
intT>
119 Eigen::Vector2d sx = Eigen::Vector2d::Zero();
120 Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero();
123 Eigen::Vector2d p(cloud[*i].x, cloud[*i].y);
125 sxx += p * p.transpose();
131 mean_ = sx /
static_cast<double>(
n_);
133 Eigen::Matrix2d covar =
134 (sxx - 2 * (sx *
mean_.transpose())) /
static_cast<double>(
n_) +
137 Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver(covar);
138 if (solver.eigenvalues()[0] < min_covar_eigvalue_mult * solver.eigenvalues()[1]) {
139 PCL_DEBUG(
"[pcl::NormalDist::estimateParams] NDT normal fit: adjusting "
141 solver.eigenvalues()[0]);
142 Eigen::Matrix2d l = solver.eigenvalues().asDiagonal();
143 Eigen::Matrix2d q = solver.eigenvectors();
145 l(0, 0) = l(1, 1) * min_covar_eigvalue_mult;
146 covar = q * l * q.transpose();
165 const double& cos_theta,
166 const double& sin_theta)
const
172 const double x = transformed_pt.x;
173 const double y = transformed_pt.y;
174 const Eigen::Vector2d p_xy(transformed_pt.x, transformed_pt.y);
175 const Eigen::Vector2d q = p_xy -
mean_;
176 const Eigen::RowVector2d qt_cvi(q.transpose() *
covar_inv_);
177 const double exp_qt_cvi_q = std::exp(-0.5 *
double(qt_cvi * q));
178 r.
value = -exp_qt_cvi_q;
180 Eigen::Matrix<double, 2, 3> jacobian;
181 jacobian << 1, 0, -(x * sin_theta + y * cos_theta), 0, 1,
182 x * cos_theta - y * sin_theta;
184 for (std::size_t i = 0; i < 3; i++)
185 r.
grad[i] =
double(qt_cvi * jacobian.col(i)) * exp_qt_cvi_q;
188 const Eigen::Vector2d d2q_didj(y * sin_theta - x * cos_theta,
189 -(x * sin_theta + y * cos_theta));
191 for (std::size_t i = 0; i < 3; i++)
192 for (std::size_t j = 0; j < 3; j++)
195 (double(-qt_cvi * jacobian.col(i)) * double(-qt_cvi * jacobian.col(j)) +
196 (-qt_cvi * ((i == 2 && j == 2) ? d2q_didj : Eigen::Vector2d::Zero())) +
197 (-jacobian.col(j).transpose() *
covar_inv_ * jacobian.col(i)));
215 template <
typename Po
intT>
223 const Eigen::Vector2f& about,
224 const Eigen::Vector2f& extent,
225 const Eigen::Vector2f& step)
226 :
min_(about - extent)
233 std::size_t used_points = 0;
234 for (std::size_t i = 0; i < cloud->size(); i++)
240 PCL_DEBUG(
"[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n",
248 for (
int x = 0; x <
cells_[0]; x++)
249 for (
int y = 0; y <
cells_[1]; y++)
262 const double& cos_theta,
263 const double& sin_theta)
const
269 return n->
test(transformed_pt, cos_theta, sin_theta);
281 Eigen::Vector2f idxf;
282 for (std::size_t i = 0; i < 2; i++)
283 idxf[i] = (p.getVector3fMap()[i] -
min_[i]) /
step_[i];
284 Eigen::Vector2i idxi = idxf.cast<
int>();
285 for (std::size_t i = 0; i < 2; i++)
286 if (idxi[i] >=
cells_[i] || idxi[i] < 0)
307 template <
typename Po
intT>
308 class NDT2D :
public boost::noncopyable {
321 const Eigen::Vector2f& about,
322 const Eigen::Vector2f& extent,
323 const Eigen::Vector2f& step)
325 Eigen::Vector2f dx(step[0] / 2, 0);
326 Eigen::Vector2f dy(0, step[1] / 2);
342 const double& cos_theta,
343 const double& sin_theta)
const
347 r += single_grid->test(transformed_pt, cos_theta, sin_theta);
363 template <
typename Po
intT>
364 struct NumTraits<
pcl::ndt2d::NormalDist<PointT>> {
376 RequireInitialization = 1,
387 template <
typename Po
intSource,
typename Po
intTarget>
390 PointCloudSource& output,
const Eigen::Matrix4f& guess)
392 PointCloudSource intm_cloud = output;
397 if (guess != Eigen::Matrix4f::Identity()) {
398 transformation_ = guess;
407 Eigen::Matrix4f& transformation = transformation_;
411 const Eigen::Matrix3f initial_rot(transformation.block<3, 3>(0, 0));
412 const Eigen::Vector3f rot_x(initial_rot * Eigen::Vector3f::UnitX());
413 const double z_rotation = std::atan2(rot_x[1], rot_x[0]);
415 Eigen::Vector3d xytheta_transformation(
416 transformation(0, 3), transformation(1, 3), z_rotation);
418 while (!converged_) {
419 const double cos_theta = std::cos(xytheta_transformation[2]);
420 const double sin_theta = std::sin(xytheta_transformation[2]);
421 previous_transformation_ = transformation;
425 for (std::size_t i = 0; i < intm_cloud.size(); i++)
426 score += target_ndt.
test(intm_cloud[i], cos_theta, sin_theta);
428 PCL_DEBUG(
"[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score "
429 "%f (x=%f,y=%f,r=%f)\n",
431 xytheta_transformation[0],
432 xytheta_transformation[1],
433 xytheta_transformation[2]);
435 if (score.
value != 0) {
437 Eigen::EigenSolver<Eigen::Matrix3d> solver;
438 solver.compute(score.
hessian,
false);
439 double min_eigenvalue = 0;
440 for (
int i = 0; i < 3; i++)
441 if (solver.eigenvalues()[i].real() < min_eigenvalue)
442 min_eigenvalue = solver.eigenvalues()[i].real();
446 if (min_eigenvalue < 0) {
447 double lambda = 1.1 * min_eigenvalue - 1;
448 score.
hessian += Eigen::Vector3d(-lambda, -lambda, -lambda).asDiagonal();
449 solver.compute(score.
hessian,
false);
450 PCL_DEBUG(
"[pcl::NormalDistributionsTransform2D::computeTransformation] adjust "
451 "hessian: %f: new eigenvalues:%f %f %f\n",
453 solver.eigenvalues()[0].real(),
454 solver.eigenvalues()[1].real(),
455 solver.eigenvalues()[2].real());
457 assert(solver.eigenvalues()[0].real() >= 0 &&
458 solver.eigenvalues()[1].real() >= 0 &&
459 solver.eigenvalues()[2].real() >= 0);
461 Eigen::Vector3d delta_transformation(-score.
hessian.inverse() * score.
grad);
462 Eigen::Vector3d new_transformation =
463 xytheta_transformation + newton_lambda_.cwiseProduct(delta_transformation);
465 xytheta_transformation = new_transformation;
468 transformation.block<3, 3>(0, 0).matrix() = Eigen::Matrix3f(Eigen::AngleAxisf(
469 static_cast<float>(xytheta_transformation[2]), Eigen::Vector3f::UnitZ()));
470 transformation.block<3, 1>(0, 3).matrix() =
471 Eigen::Vector3f(
static_cast<float>(xytheta_transformation[0]),
472 static_cast<float>(xytheta_transformation[1]),
478 PCL_ERROR(
"[pcl::NormalDistributionsTransform2D::computeTransformation] no "
479 "overlap: try increasing the size or reducing the step of the grid\n");
487 if (update_visualizer_)
488 update_visualizer_(output, *indices_, *target_, *indices_);
493 Eigen::Matrix4f transformation_delta =
494 transformation.inverse() * previous_transformation_;
496 0.5 * (transformation_delta.coeff(0, 0) + transformation_delta.coeff(1, 1) +
497 transformation_delta.coeff(2, 2) - 1);
498 double translation_sqr =
499 transformation_delta.coeff(0, 3) * transformation_delta.coeff(0, 3) +
500 transformation_delta.coeff(1, 3) * transformation_delta.coeff(1, 3) +
501 transformation_delta.coeff(2, 3) * transformation_delta.coeff(2, 3);
503 if (nr_iterations_ >= max_iterations_ ||
504 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
505 (transformation_rotation_epsilon_ > 0 &&
506 cos_angle >= transformation_rotation_epsilon_)) ||
507 ((transformation_epsilon_ <= 0) &&
508 (transformation_rotation_epsilon_ > 0 &&
509 cos_angle >= transformation_rotation_epsilon_)) ||
510 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
511 (transformation_rotation_epsilon_ <= 0))) {
515 final_transformation_ = transformation;
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< const PointCloud< PointT > > ConstPtr
Build a Normal Distributions Transform of a 2D point cloud.
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...
std::shared_ptr< SingleGrid > single_grids_[4]
NDT2D(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Build a set of normal distributions modelling a 2D point cloud, and provide the value and derivatives...
NormalDist * normalDistForPoint(PointT const &p) const
Return the normal distribution covering the location of point p.
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...
NDTSingleGrid(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Eigen::Matrix< NormalDist, Eigen::Dynamic, Eigen::Dynamic > normal_distributions_
A normal distribution estimation class.
std::vector< std::size_t > pt_indices_
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...
void addIdx(std::size_t i)
Store a point index to use later for estimating distribution parameters.
void estimateParams(const PointCloud &cloud, double min_covar_eigvalue_mult=0.001)
Estimate the normal distribution parameters given the point indices provided.
Eigen::Matrix2d covar_inv_
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.
static Real dummy_precision()
A point structure representing Euclidean xyz coordinates, and the RGB color.
Class to store vector value and first and second derivatives (grad vector and hessian matrix),...
Eigen::Matrix< T, N, N > hessian
static ValueAndDerivatives< N, T > Zero()
ValueAndDerivatives< N, T > & operator+=(ValueAndDerivatives< N, T > const &r)
Eigen::Matrix< T, N, 1 > grad