Point Cloud Library (PCL)  1.11.0
ndt_2d.hpp
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40 
41 #ifndef PCL_NDT_2D_IMPL_H_
42 #define PCL_NDT_2D_IMPL_H_
43 
44 #include <pcl/registration/eigen.h>
45 #include <pcl/registration/boost.h>
46 
47 #include <cmath>
48 #include <memory>
49 
50 namespace pcl
51 {
52 
53 namespace ndt2d
54 {
55 /** \brief Class to store vector value and first and second derivatives
56  * (grad vector and hessian matrix), so they can be returned easily from
57  * functions
58  */
59 template<unsigned N=3, typename T=double>
61 {
63 
64  Eigen::Matrix<T, N, N> hessian;
65  Eigen::Matrix<T, N, 1> grad;
66  T value;
67 
69  Zero ()
70  {
72  r.hessian = Eigen::Matrix<T, N, N>::Zero ();
73  r.grad = Eigen::Matrix<T, N, 1>::Zero ();
74  r.value = 0;
75  return r;
76  }
77 
80  {
81  hessian += r.hessian;
82  grad += r.grad;
83  value += r.value;
84  return *this;
85  }
86 };
87 
88 /** \brief A normal distribution estimation class.
89  *
90  * First the indices of of the points from a point cloud that should be
91  * modelled by the distribution are added with addIdx (...).
92  *
93  * Then estimateParams (...) uses the stored point indices to estimate the
94  * parameters of a normal distribution, and discards the stored indices.
95  *
96  * Finally the distriubution, and its derivatives, may be evaluated at any
97  * point using test (...).
98  */
99 template <typename PointT>
101 {
103 
104 public:
106  : min_n_ (3), n_ (0)
107  {
108  }
109 
110  /** \brief Store a point index to use later for estimating distribution parameters.
111  * \param[in] i Point index to store
112  */
113  void
114  addIdx (std::size_t i)
115  {
116  pt_indices_.push_back (i);
117  }
118 
119  /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared.
120  * \param[in] cloud Point cloud corresponding to indices passed to addIdx.
121  * \param[in] min_covar_eigvalue_mult Set the smallest eigenvalue to this times the largest.
122  */
123  void
124  estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001)
125  {
126  Eigen::Vector2d sx = Eigen::Vector2d::Zero ();
127  Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();
128 
129  for (auto i = pt_indices_.cbegin (); i != pt_indices_.cend (); i++)
130  {
131  Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);
132  sx += p;
133  sxx += p * p.transpose ();
134  }
135 
136  n_ = pt_indices_.size ();
137 
138  if (n_ >= min_n_)
139  {
140  mean_ = sx / static_cast<double> (n_);
141  // Using maximum likelihood estimation as in the original paper
142  Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast<double> (n_) + mean_ * mean_.transpose ();
143 
144  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver (covar);
145  if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1])
146  {
147  PCL_DEBUG ("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]);
148  Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();
149  Eigen::Matrix2d q = solver.eigenvectors ();
150  // set minimum smallest eigenvalue:
151  l (0,0) = l (1,1) * min_covar_eigvalue_mult;
152  covar = q * l * q.transpose ();
153  }
154  covar_inv_ = covar.inverse ();
155  }
156 
157  pt_indices_.clear ();
158  }
159 
160  /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
161  * \param[in] transformed_pt Location to evaluate at.
162  * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
163  * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
164  * estimateParams must have been called after at least three points were provided, or this will return zero.
165  *
166  */
168  test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
169  {
170  if (n_ < min_n_)
172 
174  const double x = transformed_pt.x;
175  const double y = transformed_pt.y;
176  const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y);
177  const Eigen::Vector2d q = p_xy - mean_;
178  const Eigen::RowVector2d qt_cvi (q.transpose () * covar_inv_);
179  const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q));
180  r.value = -exp_qt_cvi_q;
181 
182  Eigen::Matrix<double, 2, 3> jacobian;
183  jacobian <<
184  1, 0, -(x * sin_theta + y*cos_theta),
185  0, 1, x * cos_theta - y*sin_theta;
186 
187  for (std::size_t i = 0; i < 3; i++)
188  r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q;
189 
190  // second derivative only for i == j == 2:
191  const Eigen::Vector2d d2q_didj (
192  y * sin_theta - x*cos_theta,
193  -(x * sin_theta + y*cos_theta)
194  );
195 
196  for (std::size_t i = 0; i < 3; i++)
197  for (std::size_t j = 0; j < 3; j++)
198  r.hessian (i,j) = -exp_qt_cvi_q * (
199  double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) +
200  (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) +
201  (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i))
202  );
203 
204  return r;
205  }
206 
207 protected:
208  const std::size_t min_n_;
209 
210  std::size_t n_;
211  std::vector<std::size_t> pt_indices_;
212  Eigen::Vector2d mean_;
213  Eigen::Matrix2d covar_inv_;
214 };
215 
216 /** \brief Build a set of normal distributions modelling a 2D point cloud,
217  * and provide the value and derivatives of the model at any point via the
218  * test (...) function.
219  */
220 template <typename PointT>
221 class NDTSingleGrid: public boost::noncopyable
222 {
224  using PointCloudConstPtr = typename PointCloud::ConstPtr;
226 
227 public:
228  NDTSingleGrid (PointCloudConstPtr cloud,
229  const Eigen::Vector2f& about,
230  const Eigen::Vector2f& extent,
231  const Eigen::Vector2f& step)
232  : min_ (about - extent), max_ (min_ + 2*extent), step_ (step),
233  cells_ ((max_[0]-min_[0]) / step_[0],
234  (max_[1]-min_[1]) / step_[1]),
236  {
237  // sort through all points, assigning them to distributions:
238  std::size_t used_points = 0;
239  for (std::size_t i = 0; i < cloud->size (); i++)
240  if (NormalDist* n = normalDistForPoint (cloud->at (i)))
241  {
242  n->addIdx (i);
243  used_points++;
244  }
245 
246  PCL_DEBUG ("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n", cells_[0], cells_[1], used_points, cloud->size ());
247 
248  // then bake the distributions such that they approximate the
249  // points (and throw away memory of the points)
250  for (int x = 0; x < cells_[0]; x++)
251  for (int y = 0; y < cells_[1]; y++)
252  normal_distributions_.coeffRef (x,y).estimateParams (*cloud);
253  }
254 
255  /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
256  * \param[in] transformed_pt Location to evaluate at.
257  * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
258  * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
259  */
261  test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
262  {
263  const NormalDist* n = normalDistForPoint (transformed_pt);
264  // index is in grid, return score from the normal distribution from
265  // the correct part of the grid:
266  if (n)
267  return n->test (transformed_pt, cos_theta, sin_theta);
269  }
270 
271 protected:
272  /** \brief Return the normal distribution covering the location of point p
273  * \param[in] p a point
274  */
275  NormalDist*
276  normalDistForPoint (PointT const& p) const
277  {
278  // this would be neater in 3d...
279  Eigen::Vector2f idxf;
280  for (std::size_t i = 0; i < 2; i++)
281  idxf[i] = (p.getVector3fMap ()[i] - min_[i]) / step_[i];
282  Eigen::Vector2i idxi = idxf.cast<int> ();
283  for (std::size_t i = 0; i < 2; i++)
284  if (idxi[i] >= cells_[i] || idxi[i] < 0)
285  return nullptr;
286  // const cast to avoid duplicating this function in const and
287  // non-const variants...
288  return const_cast<NormalDist*> (&normal_distributions_.coeffRef (idxi[0], idxi[1]));
289  }
290 
291  Eigen::Vector2f min_;
292  Eigen::Vector2f max_;
293  Eigen::Vector2f step_;
294  Eigen::Vector2i cells_;
295 
296  Eigen::Matrix<NormalDist, Eigen::Dynamic, Eigen::Dynamic> normal_distributions_;
297 };
298 
299 /** \brief Build a Normal Distributions Transform of a 2D point cloud. This
300  * consists of the sum of four overlapping models of the original points
301  * with normal distributions.
302  * The value and derivatives of the model at any point can be evaluated
303  * with the test (...) function.
304  */
305 template <typename PointT>
306 class NDT2D: public boost::noncopyable
307 {
309  using PointCloudConstPtr = typename PointCloud::ConstPtr;
311 
312 public:
313  /** \brief
314  * \param[in] cloud the input point cloud
315  * \param[in] about Centre of the grid for normal distributions model
316  * \param[in] extent Extent of grid for normal distributions model
317  * \param[in] step Size of region that each normal distribution will model
318  */
319  NDT2D (PointCloudConstPtr cloud,
320  const Eigen::Vector2f& about,
321  const Eigen::Vector2f& extent,
322  const Eigen::Vector2f& step)
323  {
324  Eigen::Vector2f dx (step[0]/2, 0);
325  Eigen::Vector2f dy (0, step[1]/2);
326  single_grids_[0].reset(new SingleGrid (cloud, about, extent, step));
327  single_grids_[1].reset(new SingleGrid (cloud, about +dx, extent, step));
328  single_grids_[2].reset(new SingleGrid (cloud, about +dy, extent, step));
329  single_grids_[3].reset(new SingleGrid (cloud, about +dx+dy, extent, step));
330  }
331 
332  /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
333  * \param[in] transformed_pt Location to evaluate at.
334  * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
335  * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
336  */
338  test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
339  {
341  for (const auto &single_grid : single_grids_)
342  r += single_grid->test (transformed_pt, cos_theta, sin_theta);
343  return r;
344  }
345 
346 protected:
347  std::shared_ptr<SingleGrid> single_grids_[4];
348 };
349 
350 } // namespace ndt2d
351 } // namespace pcl
352 
353 
354 namespace Eigen
355 {
356 
357 /* This NumTraits specialisation is necessary because NormalDist is used as
358 * the element type of an Eigen Matrix.
359 */
360 template<typename PointT>
361 struct NumTraits<pcl::ndt2d::NormalDist<PointT> >
362 {
363  using Real = double;
364  using Literal = double;
365  static Real dummy_precision () { return 1.0; }
366  enum
367  {
368  IsComplex = 0,
369  IsInteger = 0,
370  IsSigned = 0,
371  RequireInitialization = 1,
372  ReadCost = 1,
373  AddCost = 1,
374  MulCost = 1
375  };
376 };
377 
378 } // namespace Eigen
379 
380 
381 namespace pcl
382 {
383 
384 template <typename PointSource, typename PointTarget> void
385 NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
386 {
387  PointCloudSource intm_cloud = output;
388 
389  nr_iterations_ = 0;
390  converged_ = false;
391 
392  if (guess != Eigen::Matrix4f::Identity ())
393  {
394  transformation_ = guess;
395  transformPointCloud (output, intm_cloud, transformation_);
396  }
397 
398  // build Normal Distribution Transform of target cloud:
399  ndt2d::NDT2D<PointTarget> target_ndt (target_, grid_centre_, grid_extent_, grid_step_);
400 
401  // can't seem to use .block<> () member function on transformation_
402  // directly... gcc bug?
403  Eigen::Matrix4f& transformation = transformation_;
404 
405 
406  // work with x translation, y translation and z rotation: extending to 3D
407  // would be some tricky maths, but not impossible.
408  const Eigen::Matrix3f initial_rot (transformation.block<3,3> (0,0));
409  const Eigen::Vector3f rot_x (initial_rot*Eigen::Vector3f::UnitX ());
410  const double z_rotation = std::atan2 (rot_x[1], rot_x[0]);
411 
412  Eigen::Vector3d xytheta_transformation (
413  transformation (0,3),
414  transformation (1,3),
415  z_rotation
416  );
417 
418  while (!converged_)
419  {
420  const double cos_theta = std::cos (xytheta_transformation[2]);
421  const double sin_theta = std::sin (xytheta_transformation[2]);
422  previous_transformation_ = transformation;
423 
425  for (std::size_t i = 0; i < intm_cloud.size (); i++)
426  score += target_ndt.test (intm_cloud[i], cos_theta, sin_theta);
427 
428  PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score %f (x=%f,y=%f,r=%f)\n",
429  float (score.value), xytheta_transformation[0], xytheta_transformation[1], xytheta_transformation[2]
430  );
431 
432  if (score.value != 0)
433  {
434  // test for positive definiteness, and adjust to ensure it if necessary:
435  Eigen::EigenSolver<Eigen::Matrix3d> solver;
436  solver.compute (score.hessian, false);
437  double min_eigenvalue = 0;
438  for (int i = 0; i <3; i++)
439  if (solver.eigenvalues ()[i].real () < min_eigenvalue)
440  min_eigenvalue = solver.eigenvalues ()[i].real ();
441 
442  // ensure "safe" positive definiteness: this is a detail missing
443  // from the original paper
444  if (min_eigenvalue < 0)
445  {
446  double lambda = 1.1 * min_eigenvalue - 1;
447  score.hessian += Eigen::Vector3d (-lambda, -lambda, -lambda).asDiagonal ();
448  solver.compute (score.hessian, false);
449  PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] adjust hessian: %f: new eigenvalues:%f %f %f\n",
450  float (lambda),
451  solver.eigenvalues ()[0].real (),
452  solver.eigenvalues ()[1].real (),
453  solver.eigenvalues ()[2].real ()
454  );
455  }
456  assert (solver.eigenvalues ()[0].real () >= 0 &&
457  solver.eigenvalues ()[1].real () >= 0 &&
458  solver.eigenvalues ()[2].real () >= 0);
459 
460  Eigen::Vector3d delta_transformation (-score.hessian.inverse () * score.grad);
461  Eigen::Vector3d new_transformation = xytheta_transformation + newton_lambda_.cwiseProduct (delta_transformation);
462 
463  xytheta_transformation = new_transformation;
464 
465  // update transformation matrix from x, y, theta:
466  transformation.block<3,3> (0,0).matrix () = Eigen::Matrix3f (Eigen::AngleAxisf (static_cast<float> (xytheta_transformation[2]), Eigen::Vector3f::UnitZ ()));
467  transformation.block<3,1> (0,3).matrix () = Eigen::Vector3f (static_cast<float> (xytheta_transformation[0]), static_cast<float> (xytheta_transformation[1]), 0.0f);
468 
469  //std::cout << "new transformation:\n" << transformation << std::endl;
470  }
471  else
472  {
473  PCL_ERROR ("[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n");
474  break;
475  }
476 
477  transformPointCloud (output, intm_cloud, transformation);
478 
479  nr_iterations_++;
480 
481  if (update_visualizer_)
482  update_visualizer_ (output, *indices_, *target_, *indices_);
483 
484  //std::cout << "eps=" << std::abs ((transformation - previous_transformation_).sum ()) << std::endl;
485 
486  Eigen::Matrix4f transformation_delta = transformation.inverse() * previous_transformation_;
487  double cos_angle = 0.5 * (transformation_delta.coeff (0, 0) + transformation_delta.coeff (1, 1) + transformation_delta.coeff (2, 2) - 1);
488  double translation_sqr = transformation_delta.coeff (0, 3) * transformation_delta.coeff (0, 3) +
489  transformation_delta.coeff (1, 3) * transformation_delta.coeff (1, 3) +
490  transformation_delta.coeff (2, 3) * transformation_delta.coeff (2, 3);
491 
492  if (nr_iterations_ >= max_iterations_ ||
493  ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
494  ((transformation_epsilon_ <= 0) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
495  ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0)))
496  {
497  converged_ = true;
498  }
499  }
500  final_transformation_ = transformation;
501  output = intm_cloud;
502 }
503 
504 } // namespace pcl
505 
506 #endif // PCL_NDT_2D_IMPL_H_
507 
pcl::ndt2d::ValueAndDerivatives
Class to store vector value and first and second derivatives (grad vector and hessian matrix),...
Definition: ndt_2d.hpp:61
pcl
Definition: convolution.h:46
pcl::ndt2d::ValueAndDerivatives::value
T value
Definition: ndt_2d.hpp:66
pcl::ndt2d::NormalDist::addIdx
void addIdx(std::size_t i)
Store a point index to use later for estimating distribution parameters.
Definition: ndt_2d.hpp:114
Eigen
Definition: bfgs.h:10
pcl::ndt2d::NormalDist::covar_inv_
Eigen::Matrix2d covar_inv_
Definition: ndt_2d.hpp:213
pcl::ndt2d::NormalDist
A normal distribution estimation class.
Definition: ndt_2d.hpp:101
pcl::ndt2d::NormalDist::test
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...
Definition: ndt_2d.hpp:168
pcl::ndt2d::ValueAndDerivatives::ValueAndDerivatives
ValueAndDerivatives()
Definition: ndt_2d.hpp:62
pcl::ndt2d::ValueAndDerivatives::operator+=
ValueAndDerivatives< N, T > & operator+=(ValueAndDerivatives< N, T > const &r)
Definition: ndt_2d.hpp:79
pcl::ndt2d::NDTSingleGrid::normalDistForPoint
NormalDist * normalDistForPoint(PointT const &p) const
Return the normal distribution covering the location of point p.
Definition: ndt_2d.hpp:276
pcl::ndt2d::NDTSingleGrid::normal_distributions_
Eigen::Matrix< NormalDist, Eigen::Dynamic, Eigen::Dynamic > normal_distributions_
Definition: ndt_2d.hpp:296
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
Eigen::NumTraits< pcl::ndt2d::NormalDist< PointT > >::dummy_precision
static Real dummy_precision()
Definition: ndt_2d.hpp:365
pcl::NormalDistributionsTransform2D::computeTransformation
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
Definition: ndt_2d.hpp:385
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:629
pcl::ndt2d::NDT2D::NDT2D
NDT2D(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Definition: ndt_2d.hpp:319
pcl::ndt2d::NormalDist::n_
std::size_t n_
Definition: ndt_2d.hpp:210
pcl::ndt2d::NDTSingleGrid::cells_
Eigen::Vector2i cells_
Definition: ndt_2d.hpp:294
pcl::ndt2d::NDTSingleGrid::min_
Eigen::Vector2f min_
Definition: ndt_2d.hpp:291
pcl::transformPointCloud
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.
Definition: transforms.hpp:221
Eigen::NumTraits< pcl::ndt2d::NormalDist< PointT > >::Real
double Real
Definition: ndt_2d.hpp:363
pcl::ndt2d::NDTSingleGrid
Build a set of normal distributions modelling a 2D point cloud, and provide the value and derivatives...
Definition: ndt_2d.hpp:222
pcl::ndt2d::NDT2D::single_grids_
std::shared_ptr< SingleGrid > single_grids_[4]
Definition: ndt_2d.hpp:347
pcl::ndt2d::NDTSingleGrid::step_
Eigen::Vector2f step_
Definition: ndt_2d.hpp:293
pcl::ndt2d::NormalDist::min_n_
const std::size_t min_n_
Definition: ndt_2d.hpp:208
Eigen::NumTraits< pcl::ndt2d::NormalDist< PointT > >::Literal
double Literal
Definition: ndt_2d.hpp:364
pcl::ndt2d::NDTSingleGrid::NDTSingleGrid
NDTSingleGrid(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Definition: ndt_2d.hpp:228
pcl::ndt2d::NDTSingleGrid::test
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...
Definition: ndt_2d.hpp:261
pcl::ndt2d::ValueAndDerivatives::grad
Eigen::Matrix< T, N, 1 > grad
Definition: ndt_2d.hpp:65
pcl::ndt2d::NDTSingleGrid::max_
Eigen::Vector2f max_
Definition: ndt_2d.hpp:292
pcl::ndt2d::NormalDist::mean_
Eigen::Vector2d mean_
Definition: ndt_2d.hpp:212
pcl::ndt2d::NormalDist::pt_indices_
std::vector< std::size_t > pt_indices_
Definition: ndt_2d.hpp:211
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::ndt2d::NDT2D::test
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...
Definition: ndt_2d.hpp:338
pcl::ndt2d::NormalDist::NormalDist
NormalDist()
Definition: ndt_2d.hpp:105
pcl::ndt2d::NormalDist::estimateParams
void estimateParams(const PointCloud &cloud, double min_covar_eigvalue_mult=0.001)
Estimate the normal distribution parameters given the point indices provided.
Definition: ndt_2d.hpp:124
pcl::ndt2d::ValueAndDerivatives::hessian
Eigen::Matrix< T, N, N > hessian
Definition: ndt_2d.hpp:64
pcl::ndt2d::ValueAndDerivatives::Zero
static ValueAndDerivatives< N, T > Zero()
Definition: ndt_2d.hpp:69
pcl::ndt2d::NDT2D
Build a Normal Distributions Transform of a 2D point cloud.
Definition: ndt_2d.hpp:307