40 #ifndef PCL_NDT_2D_IMPL_H_ 41 #define PCL_NDT_2D_IMPL_H_ 44 #include <pcl/registration/eigen.h> 45 #include <pcl/registration/boost.h> 55 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>
122 Eigen::Vector2d sx = Eigen::Vector2d::Zero ();
123 Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();
125 std::vector<size_t>::const_iterator i;
128 Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);
130 sxx += p * p.transpose ();
137 mean_ = sx / static_cast<double> (
n_);
139 Eigen::Matrix2d covar = (sxx - 2 * (sx *
mean_.transpose ())) / static_cast<double> (
n_) +
mean_ *
mean_.transpose ();
141 Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver (covar);
142 if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1])
144 PCL_DEBUG (
"[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]);
145 Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();
146 Eigen::Matrix2d q = solver.eigenvectors ();
148 l (0,0) = l (1,1) * min_covar_eigvalue_mult;
149 covar = q * l * q.transpose ();
165 test (
const PointT& transformed_pt,
const double& cos_theta,
const double& sin_theta)
const 171 const double x = transformed_pt.x;
172 const double y = transformed_pt.y;
173 const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y);
174 const Eigen::Vector2d q = p_xy -
mean_;
175 const Eigen::RowVector2d qt_cvi (q.transpose () *
covar_inv_);
176 const double exp_qt_cvi_q = std::exp (-0.5 *
double (qt_cvi * q));
177 r.
value = -exp_qt_cvi_q;
179 Eigen::Matrix<double, 2, 3> jacobian;
181 1, 0, -(x * sin_theta + y*cos_theta),
182 0, 1, x * cos_theta - y*sin_theta;
184 for (
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 (
189 y * sin_theta - x*cos_theta,
190 -(x * sin_theta + y*cos_theta)
193 for (
size_t i = 0; i < 3; i++)
194 for (
size_t j = 0; j < 3; j++)
195 r.
hessian (i,j) = -exp_qt_cvi_q * (
196 double (-qt_cvi*jacobian.col (i)) *
double (-qt_cvi*jacobian.col (j)) +
197 (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) +
198 (-jacobian.col (j).transpose () *
covar_inv_ * jacobian.col (i))
217 template <
typename Po
intT>
226 const Eigen::Vector2f& about,
227 const Eigen::Vector2f& extent,
228 const Eigen::Vector2f& step)
236 size_t used_points = 0;
237 for (
size_t i = 0; i < cloud->size (); i++)
244 PCL_DEBUG (
"[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n",
cells_[0],
cells_[1], used_points, cloud->size ());
248 for (
int x = 0; x <
cells_[0]; x++)
249 for (
int y = 0; y <
cells_[1]; y++)
259 test (
const PointT& transformed_pt,
const double& cos_theta,
const double& sin_theta)
const 265 return n->
test (transformed_pt, cos_theta, sin_theta);
278 Eigen::Vector2f idxf;
279 for (
size_t i = 0; i < 2; i++)
280 idxf[i] = (p.getVector3fMap ()[i] -
min_[i]) /
step_[i];
281 Eigen::Vector2i idxi = idxf.cast<
int> ();
282 for (
size_t i = 0; i < 2; i++)
283 if (idxi[i] >=
cells_[i] || idxi[i] < 0)
304 template <
typename Po
intT>
305 class NDT2D:
public boost::noncopyable
319 const Eigen::Vector2f& about,
320 const Eigen::Vector2f& extent,
321 const Eigen::Vector2f& step)
323 Eigen::Vector2f dx (step[0]/2, 0);
324 Eigen::Vector2f dy (0, step[1]/2);
325 single_grids_[0] = boost::make_shared<SingleGrid> (cloud, about, extent, step);
326 single_grids_[1] = boost::make_shared<SingleGrid> (cloud, about +dx, extent, step);
327 single_grids_[2] = boost::make_shared<SingleGrid> (cloud, about +dy, extent, step);
328 single_grids_[3] = boost::make_shared<SingleGrid> (cloud, about +dx+dy, extent, step);
337 test (
const PointT& transformed_pt,
const double& cos_theta,
const double& sin_theta)
const 340 for (
size_t i = 0; i < 4; i++)
358 template<
typename Po
intT>
struct NumTraits<
pcl::ndt2d::NormalDist<PointT> >
367 RequireInitialization = 1,
376 template <
typename Po
intSource,
typename Po
intTarget>
void 379 PointCloudSource intm_cloud = output;
384 if (guess != Eigen::Matrix4f::Identity ())
386 transformation_ = guess;
395 Eigen::Matrix4f& transformation = transformation_;
400 const Eigen::Matrix3f initial_rot (transformation.block<3,3> (0,0));
401 const Eigen::Vector3f rot_x (initial_rot*Eigen::Vector3f::UnitX ());
402 const double z_rotation = std::atan2 (rot_x[1], rot_x[0]);
404 Eigen::Vector3d xytheta_transformation (
405 transformation (0,3),
406 transformation (1,3),
412 const double cos_theta = std::cos (xytheta_transformation[2]);
413 const double sin_theta = std::sin (xytheta_transformation[2]);
414 previous_transformation_ = transformation;
417 for (
size_t i = 0; i < intm_cloud.size (); i++)
418 score += target_ndt.
test (intm_cloud[i], cos_theta, sin_theta);
420 PCL_DEBUG (
"[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score %f (x=%f,y=%f,r=%f)\n",
421 float (score.
value), xytheta_transformation[0], xytheta_transformation[1], xytheta_transformation[2]
424 if (score.
value != 0)
427 Eigen::EigenSolver<Eigen::Matrix3d> solver;
428 solver.compute (score.
hessian,
false);
429 double min_eigenvalue = 0;
430 for (
int i = 0; i <3; i++)
431 if (solver.eigenvalues ()[i].real () < min_eigenvalue)
432 min_eigenvalue = solver.eigenvalues ()[i].real ();
436 if (min_eigenvalue < 0)
438 double lambda = 1.1 * min_eigenvalue - 1;
439 score.
hessian += Eigen::Vector3d (-lambda, -lambda, -lambda).asDiagonal ();
440 solver.compute (score.
hessian,
false);
441 PCL_DEBUG (
"[pcl::NormalDistributionsTransform2D::computeTransformation] adjust hessian: %f: new eigenvalues:%f %f %f\n",
443 solver.eigenvalues ()[0].real (),
444 solver.eigenvalues ()[1].real (),
445 solver.eigenvalues ()[2].real ()
448 assert (solver.eigenvalues ()[0].real () >= 0 &&
449 solver.eigenvalues ()[1].real () >= 0 &&
450 solver.eigenvalues ()[2].real () >= 0);
452 Eigen::Vector3d delta_transformation (-score.
hessian.inverse () * score.
grad);
453 Eigen::Vector3d new_transformation = xytheta_transformation + newton_lambda_.cwiseProduct (delta_transformation);
455 xytheta_transformation = new_transformation;
458 transformation.block<3,3> (0,0).matrix () = Eigen::Matrix3f (Eigen::AngleAxisf (static_cast<float> (xytheta_transformation[2]), Eigen::Vector3f::UnitZ ()));
459 transformation.block<3,1> (0,3).matrix () = Eigen::Vector3f (static_cast<float> (xytheta_transformation[0]), static_cast<float> (xytheta_transformation[1]), 0.0f);
465 PCL_ERROR (
"[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n");
473 if (update_visualizer_ != 0)
474 update_visualizer_ (output, *indices_, *target_, *indices_);
478 Eigen::Matrix4f transformation_delta = transformation.inverse() * previous_transformation_;
479 double cos_angle = 0.5 * (transformation_delta.coeff (0, 0) + transformation_delta.coeff (1, 1) + transformation_delta.coeff (2, 2) - 1);
480 double translation_sqr = transformation_delta.coeff (0, 3) * transformation_delta.coeff (0, 3) +
481 transformation_delta.coeff (1, 3) * transformation_delta.coeff (1, 3) +
482 transformation_delta.coeff (2, 3) * transformation_delta.coeff (2, 3);
484 if (nr_iterations_ >= max_iterations_ ||
485 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
486 ((transformation_epsilon_ <= 0) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
487 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0)))
492 final_transformation_ = transformation;
496 #endif // PCL_NDT_2D_IMPL_H_
NDT2D(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
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...
ValueAndDerivatives< N, T > & operator+=(ValueAndDerivatives< N, T > const &r)
void estimateParams(const PointCloud &cloud, double min_covar_eigvalue_mult=0.001)
Estimate the normal distribution parameters given the point indices provided.
Eigen::Matrix< T, N, 1 > grad
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...
static Real dummy_precision()
This file defines compatibility wrappers for low level I/O functions.
std::vector< size_t > pt_indices_
void addIdx(size_t i)
Store a point index to use later for estimating distribution parameters.
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...
A normal distribution estimation class.
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.
Build a Normal Distributions Transform of a 2D point cloud.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
Eigen::Matrix< NormalDist, Eigen::Dynamic, Eigen::Dynamic > normal_distributions_
NormalDist * normalDistForPoint(PointT const &p) const
Return the normal distribution covering the location of point p.
static ValueAndDerivatives< N, T > Zero()
boost::shared_ptr< SingleGrid > single_grids_[4]
Eigen::Matrix2d covar_inv_
Class to store vector value and first and second derivatives (grad vector and hessian matrix),...
A point structure representing Euclidean xyz coordinates, and the RGB color.
NDTSingleGrid(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Eigen::Matrix< T, N, N > hessian
Build a set of normal distributions modelling a 2D point cloud, and provide the value and derivatives...