Point Cloud Library (PCL)  1.11.1-dev
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/boost.h>
45 
46 #include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver, EigenSolver
47 
48 #include <cmath>
49 #include <memory>
50 
51 namespace pcl {
52 
53 namespace ndt2d {
54 /** \brief Class to store vector value and first and second derivatives
55  * (grad vector and hessian matrix), so they can be returned easily from
56  * functions
57  */
58 template <unsigned N = 3, typename T = double>
61 
62  Eigen::Matrix<T, N, N> hessian;
63  Eigen::Matrix<T, N, 1> grad;
64  T value;
65 
67  Zero()
68  {
70  r.hessian = Eigen::Matrix<T, N, N>::Zero();
71  r.grad = Eigen::Matrix<T, N, 1>::Zero();
72  r.value = 0;
73  return r;
74  }
75 
78  {
79  hessian += r.hessian;
80  grad += r.grad;
81  value += r.value;
82  return *this;
83  }
84 };
85 
86 /** \brief A normal distribution estimation class.
87  *
88  * First the indices of of the points from a point cloud that should be
89  * modelled by the distribution are added with addIdx (...).
90  *
91  * Then estimateParams (...) uses the stored point indices to estimate the
92  * parameters of a normal distribution, and discards the stored indices.
93  *
94  * Finally the distriubution, and its derivatives, may be evaluated at any
95  * point using test (...).
96  */
97 template <typename PointT>
98 class NormalDist {
100 
101 public:
102  NormalDist() : min_n_(3), n_(0) {}
103 
104  /** \brief Store a point index to use later for estimating distribution parameters.
105  * \param[in] i Point index to store
106  */
107  void
108  addIdx(std::size_t i)
109  {
110  pt_indices_.push_back(i);
111  }
112 
113  /** \brief Estimate the normal distribution parameters given the point indices
114  * provided. Memory of point indices is cleared. \param[in] cloud Point cloud
115  * corresponding to indices passed to addIdx. \param[in] min_covar_eigvalue_mult Set
116  * the smallest eigenvalue to this times the largest.
117  */
118  void
119  estimateParams(const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001)
120  {
121  Eigen::Vector2d sx = Eigen::Vector2d::Zero();
122  Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero();
123 
124  for (auto i = pt_indices_.cbegin(); i != pt_indices_.cend(); i++) {
125  Eigen::Vector2d p(cloud[*i].x, cloud[*i].y);
126  sx += p;
127  sxx += p * p.transpose();
128  }
129 
130  n_ = pt_indices_.size();
131 
132  if (n_ >= min_n_) {
133  mean_ = sx / static_cast<double>(n_);
134  // Using maximum likelihood estimation as in the original paper
135  Eigen::Matrix2d covar =
136  (sxx - 2 * (sx * mean_.transpose())) / static_cast<double>(n_) +
137  mean_ * mean_.transpose();
138 
139  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver(covar);
140  if (solver.eigenvalues()[0] < min_covar_eigvalue_mult * solver.eigenvalues()[1]) {
141  PCL_DEBUG("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting "
142  "eigenvalue %f\n",
143  solver.eigenvalues()[0]);
144  Eigen::Matrix2d l = solver.eigenvalues().asDiagonal();
145  Eigen::Matrix2d q = solver.eigenvectors();
146  // set minimum smallest eigenvalue:
147  l(0, 0) = l(1, 1) * min_covar_eigvalue_mult;
148  covar = q * l * q.transpose();
149  }
150  covar_inv_ = covar.inverse();
151  }
152 
153  pt_indices_.clear();
154  }
155 
156  /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of
157  * the point p given this distribution. \param[in] transformed_pt Location to
158  * evaluate at. \param[in] cos_theta sin(theta) of the current rotation angle
159  * of rigid transformation: to avoid repeated evaluation \param[in] sin_theta
160  * cos(theta) of the current rotation angle of rigid transformation: to avoid repeated
161  * evaluation estimateParams must have been called after at least three points were
162  * provided, or this will return zero.
163  *
164  */
166  test(const PointT& transformed_pt,
167  const double& cos_theta,
168  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 << 1, 0, -(x * sin_theta + y * cos_theta), 0, 1,
184  x * cos_theta - y * sin_theta;
185 
186  for (std::size_t i = 0; i < 3; i++)
187  r.grad[i] = double(qt_cvi * jacobian.col(i)) * exp_qt_cvi_q;
188 
189  // second derivative only for i == j == 2:
190  const Eigen::Vector2d d2q_didj(y * sin_theta - x * cos_theta,
191  -(x * sin_theta + y * cos_theta));
192 
193  for (std::size_t i = 0; i < 3; i++)
194  for (std::size_t j = 0; j < 3; j++)
195  r.hessian(i, j) =
196  -exp_qt_cvi_q *
197  (double(-qt_cvi * jacobian.col(i)) * double(-qt_cvi * jacobian.col(j)) +
198  (-qt_cvi * ((i == 2 && j == 2) ? d2q_didj : Eigen::Vector2d::Zero())) +
199  (-jacobian.col(j).transpose() * covar_inv_ * jacobian.col(i)));
200 
201  return r;
202  }
203 
204 protected:
205  const std::size_t min_n_;
206 
207  std::size_t n_;
208  std::vector<std::size_t> pt_indices_;
209  Eigen::Vector2d mean_;
210  Eigen::Matrix2d covar_inv_;
211 };
212 
213 /** \brief Build a set of normal distributions modelling a 2D point cloud,
214  * and provide the value and derivatives of the model at any point via the
215  * test (...) function.
216  */
217 template <typename PointT>
218 class NDTSingleGrid : public boost::noncopyable {
220  using PointCloudConstPtr = typename PointCloud::ConstPtr;
222 
223 public:
224  NDTSingleGrid(PointCloudConstPtr cloud,
225  const Eigen::Vector2f& about,
226  const Eigen::Vector2f& extent,
227  const Eigen::Vector2f& step)
228  : min_(about - extent)
229  , max_(min_ + 2 * extent)
230  , step_(step)
231  , cells_((max_[0] - min_[0]) / step_[0], (max_[1] - min_[1]) / step_[1])
233  {
234  // sort through all points, assigning them to distributions:
235  std::size_t used_points = 0;
236  for (std::size_t i = 0; i < cloud->size(); i++)
237  if (NormalDist* n = normalDistForPoint(cloud->at(i))) {
238  n->addIdx(i);
239  used_points++;
240  }
241 
242  PCL_DEBUG("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n",
243  cells_[0],
244  cells_[1],
245  used_points,
246  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
256  * the point p given this distribution. \param[in] transformed_pt Location to
257  * evaluate at. \param[in] cos_theta sin(theta) of the current rotation angle
258  * of rigid transformation: to avoid repeated evaluation \param[in] sin_theta
259  * cos(theta) of the current rotation angle of rigid transformation: to avoid repeated
260  * evaluation
261  */
263  test(const PointT& transformed_pt,
264  const double& cos_theta,
265  const double& sin_theta) const
266  {
267  const NormalDist* n = normalDistForPoint(transformed_pt);
268  // index is in grid, return score from the normal distribution from
269  // the correct part of the grid:
270  if (n)
271  return n->test(transformed_pt, cos_theta, sin_theta);
273  }
274 
275 protected:
276  /** \brief Return the normal distribution covering the location of point p
277  * \param[in] p a point
278  */
279  NormalDist*
280  normalDistForPoint(PointT const& p) const
281  {
282  // this would be neater in 3d...
283  Eigen::Vector2f idxf;
284  for (std::size_t i = 0; i < 2; i++)
285  idxf[i] = (p.getVector3fMap()[i] - min_[i]) / step_[i];
286  Eigen::Vector2i idxi = idxf.cast<int>();
287  for (std::size_t i = 0; i < 2; i++)
288  if (idxi[i] >= cells_[i] || idxi[i] < 0)
289  return nullptr;
290  // const cast to avoid duplicating this function in const and
291  // non-const variants...
292  return const_cast<NormalDist*>(&normal_distributions_.coeffRef(idxi[0], idxi[1]));
293  }
294 
295  Eigen::Vector2f min_;
296  Eigen::Vector2f max_;
297  Eigen::Vector2f step_;
298  Eigen::Vector2i cells_;
299 
300  Eigen::Matrix<NormalDist, Eigen::Dynamic, Eigen::Dynamic> normal_distributions_;
301 };
302 
303 /** \brief Build a Normal Distributions Transform of a 2D point cloud. This
304  * consists of the sum of four overlapping models of the original points
305  * with normal distributions.
306  * The value and derivatives of the model at any point can be evaluated
307  * with the test (...) function.
308  */
309 template <typename PointT>
310 class NDT2D : public boost::noncopyable {
312  using PointCloudConstPtr = typename PointCloud::ConstPtr;
314 
315 public:
316  /** \brief
317  * \param[in] cloud the input point cloud
318  * \param[in] about Centre of the grid for normal distributions model
319  * \param[in] extent Extent of grid for normal distributions model
320  * \param[in] step Size of region that each normal distribution will model
321  */
322  NDT2D(PointCloudConstPtr cloud,
323  const Eigen::Vector2f& about,
324  const Eigen::Vector2f& extent,
325  const Eigen::Vector2f& step)
326  {
327  Eigen::Vector2f dx(step[0] / 2, 0);
328  Eigen::Vector2f dy(0, step[1] / 2);
329  single_grids_[0].reset(new SingleGrid(cloud, about, extent, step));
330  single_grids_[1].reset(new SingleGrid(cloud, about + dx, extent, step));
331  single_grids_[2].reset(new SingleGrid(cloud, about + dy, extent, step));
332  single_grids_[3].reset(new SingleGrid(cloud, about + dx + dy, extent, step));
333  }
334 
335  /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of
336  * the point p given this distribution. \param[in] transformed_pt Location to
337  * evaluate at. \param[in] cos_theta sin(theta) of the current rotation angle
338  * of rigid transformation: to avoid repeated evaluation \param[in] sin_theta
339  * cos(theta) of the current rotation angle of rigid transformation: to avoid repeated
340  * evaluation
341  */
343  test(const PointT& transformed_pt,
344  const double& cos_theta,
345  const double& sin_theta) const
346  {
348  for (const auto& single_grid : single_grids_)
349  r += single_grid->test(transformed_pt, cos_theta, sin_theta);
350  return r;
351  }
352 
353 protected:
354  std::shared_ptr<SingleGrid> single_grids_[4];
355 };
356 
357 } // namespace ndt2d
358 } // namespace pcl
359 
360 namespace Eigen {
361 
362 /* This NumTraits specialisation is necessary because NormalDist is used as
363  * the element type of an Eigen Matrix.
364  */
365 template <typename PointT>
366 struct NumTraits<pcl::ndt2d::NormalDist<PointT>> {
367  using Real = double;
368  using Literal = double;
369  static Real
371  {
372  return 1.0;
373  }
374  enum {
375  IsComplex = 0,
376  IsInteger = 0,
377  IsSigned = 0,
378  RequireInitialization = 1,
379  ReadCost = 1,
380  AddCost = 1,
381  MulCost = 1
382  };
383 };
384 
385 } // namespace Eigen
386 
387 namespace pcl {
388 
389 template <typename PointSource, typename PointTarget>
390 void
392  PointCloudSource& output, const Eigen::Matrix4f& guess)
393 {
394  PointCloudSource intm_cloud = output;
395 
396  nr_iterations_ = 0;
397  converged_ = false;
398 
399  if (guess != Eigen::Matrix4f::Identity()) {
400  transformation_ = guess;
401  transformPointCloud(output, intm_cloud, transformation_);
402  }
403 
404  // build Normal Distribution Transform of target cloud:
405  ndt2d::NDT2D<PointTarget> target_ndt(target_, grid_centre_, grid_extent_, grid_step_);
406 
407  // can't seem to use .block<> () member function on transformation_
408  // directly... gcc bug?
409  Eigen::Matrix4f& transformation = transformation_;
410 
411  // work with x translation, y translation and z rotation: extending to 3D
412  // would be some tricky maths, but not impossible.
413  const Eigen::Matrix3f initial_rot(transformation.block<3, 3>(0, 0));
414  const Eigen::Vector3f rot_x(initial_rot * Eigen::Vector3f::UnitX());
415  const double z_rotation = std::atan2(rot_x[1], rot_x[0]);
416 
417  Eigen::Vector3d xytheta_transformation(
418  transformation(0, 3), transformation(1, 3), z_rotation);
419 
420  while (!converged_) {
421  const double cos_theta = std::cos(xytheta_transformation[2]);
422  const double sin_theta = std::sin(xytheta_transformation[2]);
423  previous_transformation_ = transformation;
424 
427  for (std::size_t i = 0; i < intm_cloud.size(); i++)
428  score += target_ndt.test(intm_cloud[i], cos_theta, sin_theta);
429 
430  PCL_DEBUG("[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score "
431  "%f (x=%f,y=%f,r=%f)\n",
432  float(score.value),
433  xytheta_transformation[0],
434  xytheta_transformation[1],
435  xytheta_transformation[2]);
436 
437  if (score.value != 0) {
438  // test for positive definiteness, and adjust to ensure it if necessary:
439  Eigen::EigenSolver<Eigen::Matrix3d> solver;
440  solver.compute(score.hessian, false);
441  double min_eigenvalue = 0;
442  for (int i = 0; i < 3; i++)
443  if (solver.eigenvalues()[i].real() < min_eigenvalue)
444  min_eigenvalue = solver.eigenvalues()[i].real();
445 
446  // ensure "safe" positive definiteness: this is a detail missing
447  // from the original paper
448  if (min_eigenvalue < 0) {
449  double lambda = 1.1 * min_eigenvalue - 1;
450  score.hessian += Eigen::Vector3d(-lambda, -lambda, -lambda).asDiagonal();
451  solver.compute(score.hessian, false);
452  PCL_DEBUG("[pcl::NormalDistributionsTransform2D::computeTransformation] adjust "
453  "hessian: %f: new eigenvalues:%f %f %f\n",
454  float(lambda),
455  solver.eigenvalues()[0].real(),
456  solver.eigenvalues()[1].real(),
457  solver.eigenvalues()[2].real());
458  }
459  assert(solver.eigenvalues()[0].real() >= 0 &&
460  solver.eigenvalues()[1].real() >= 0 &&
461  solver.eigenvalues()[2].real() >= 0);
462 
463  Eigen::Vector3d delta_transformation(-score.hessian.inverse() * score.grad);
464  Eigen::Vector3d new_transformation =
465  xytheta_transformation + newton_lambda_.cwiseProduct(delta_transformation);
466 
467  xytheta_transformation = new_transformation;
468 
469  // update transformation matrix from x, y, theta:
470  transformation.block<3, 3>(0, 0).matrix() = Eigen::Matrix3f(Eigen::AngleAxisf(
471  static_cast<float>(xytheta_transformation[2]), Eigen::Vector3f::UnitZ()));
472  transformation.block<3, 1>(0, 3).matrix() =
473  Eigen::Vector3f(static_cast<float>(xytheta_transformation[0]),
474  static_cast<float>(xytheta_transformation[1]),
475  0.0f);
476 
477  // std::cout << "new transformation:\n" << transformation << std::endl;
478  }
479  else {
480  PCL_ERROR("[pcl::NormalDistributionsTransform2D::computeTransformation] no "
481  "overlap: try increasing the size or reducing the step of the grid\n");
482  break;
483  }
484 
485  transformPointCloud(output, intm_cloud, transformation);
486 
487  nr_iterations_++;
488 
489  if (update_visualizer_)
490  update_visualizer_(output, *indices_, *target_, *indices_);
491 
492  // std::cout << "eps=" << std::abs ((transformation - previous_transformation_).sum
493  // ()) << std::endl;
494 
495  Eigen::Matrix4f transformation_delta =
496  transformation.inverse() * previous_transformation_;
497  double cos_angle =
498  0.5 * (transformation_delta.coeff(0, 0) + transformation_delta.coeff(1, 1) +
499  transformation_delta.coeff(2, 2) - 1);
500  double translation_sqr =
501  transformation_delta.coeff(0, 3) * transformation_delta.coeff(0, 3) +
502  transformation_delta.coeff(1, 3) * transformation_delta.coeff(1, 3) +
503  transformation_delta.coeff(2, 3) * transformation_delta.coeff(2, 3);
504 
505  if (nr_iterations_ >= max_iterations_ ||
506  ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
507  (transformation_rotation_epsilon_ > 0 &&
508  cos_angle >= transformation_rotation_epsilon_)) ||
509  ((transformation_epsilon_ <= 0) &&
510  (transformation_rotation_epsilon_ > 0 &&
511  cos_angle >= transformation_rotation_epsilon_)) ||
512  ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
513  (transformation_rotation_epsilon_ <= 0))) {
514  converged_ = true;
515  }
516  }
517  final_transformation_ = transformation;
518  output = intm_cloud;
519 }
520 
521 } // namespace pcl
522 
523 #endif // PCL_NDT_2D_IMPL_H_
pcl::ndt2d::ValueAndDerivatives
Class to store vector value and first and second derivatives (grad vector and hessian matrix),...
Definition: ndt_2d.hpp:59
pcl::ndt2d::ValueAndDerivatives::Zero
static ValueAndDerivatives< N, T > Zero()
Definition: ndt_2d.hpp:67
pcl
Definition: convolution.h:46
pcl::ndt2d::ValueAndDerivatives::value
T value
Definition: ndt_2d.hpp:64
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:108
Eigen
Definition: bfgs.h:10
pcl::ndt2d::NormalDist::covar_inv_
Eigen::Matrix2d covar_inv_
Definition: ndt_2d.hpp:210
pcl::ndt2d::NormalDist
A normal distribution estimation class.
Definition: ndt_2d.hpp:98
pcl::ndt2d::ValueAndDerivatives::ValueAndDerivatives
ValueAndDerivatives()
Definition: ndt_2d.hpp:60
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:343
pcl::ndt2d::NDTSingleGrid::normalDistForPoint
NormalDist * normalDistForPoint(PointT const &p) const
Return the normal distribution covering the location of point p.
Definition: ndt_2d.hpp:280
pcl::ndt2d::NDTSingleGrid::normal_distributions_
Eigen::Matrix< NormalDist, Eigen::Dynamic, Eigen::Dynamic > normal_distributions_
Definition: ndt_2d.hpp:300
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
Eigen::NumTraits< pcl::ndt2d::NormalDist< PointT > >::dummy_precision
static Real dummy_precision()
Definition: ndt_2d.hpp:370
pcl::NormalDistributionsTransform2D::computeTransformation
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
Definition: ndt_2d.hpp:391
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::ndt2d::NDT2D::NDT2D
NDT2D(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Definition: ndt_2d.hpp:322
pcl::transformPointCloud
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.
Definition: transforms.hpp:221
pcl::ndt2d::NormalDist::n_
std::size_t n_
Definition: ndt_2d.hpp:207
pcl::ndt2d::NDTSingleGrid::cells_
Eigen::Vector2i cells_
Definition: ndt_2d.hpp:298
pcl::ndt2d::NDTSingleGrid::min_
Eigen::Vector2f min_
Definition: ndt_2d.hpp:295
pcl::ndt2d::ValueAndDerivatives::operator+=
ValueAndDerivatives< N, T > & operator+=(ValueAndDerivatives< N, T > const &r)
Definition: ndt_2d.hpp:77
Eigen::NumTraits< pcl::ndt2d::NormalDist< PointT > >::Real
double Real
Definition: ndt_2d.hpp:367
pcl::ndt2d::NDTSingleGrid
Build a set of normal distributions modelling a 2D point cloud, and provide the value and derivatives...
Definition: ndt_2d.hpp:218
pcl::ndt2d::NDT2D::single_grids_
std::shared_ptr< SingleGrid > single_grids_[4]
Definition: ndt_2d.hpp:354
pcl::ndt2d::NDTSingleGrid::step_
Eigen::Vector2f step_
Definition: ndt_2d.hpp:297
pcl::ndt2d::NormalDist::min_n_
const std::size_t min_n_
Definition: ndt_2d.hpp:205
Eigen::NumTraits< pcl::ndt2d::NormalDist< PointT > >::Literal
double Literal
Definition: ndt_2d.hpp:368
pcl::ndt2d::NDTSingleGrid::NDTSingleGrid
NDTSingleGrid(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Definition: ndt_2d.hpp:224
pcl::ndt2d::ValueAndDerivatives::grad
Eigen::Matrix< T, N, 1 > grad
Definition: ndt_2d.hpp:63
pcl::ndt2d::NDTSingleGrid::max_
Eigen::Vector2f max_
Definition: ndt_2d.hpp:296
pcl::ndt2d::NormalDist::mean_
Eigen::Vector2d mean_
Definition: ndt_2d.hpp:209
pcl::ndt2d::NormalDist::pt_indices_
std::vector< std::size_t > pt_indices_
Definition: ndt_2d.hpp:208
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:408
pcl::ndt2d::NormalDist::NormalDist
NormalDist()
Definition: ndt_2d.hpp:102
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:119
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:166
pcl::ndt2d::ValueAndDerivatives::hessian
Eigen::Matrix< T, N, N > hessian
Definition: ndt_2d.hpp:62
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:263
pcl::ndt2d::NDT2D
Build a Normal Distributions Transform of a 2D point cloud.
Definition: ndt_2d.hpp:310