Point Cloud Library (PCL)  1.9.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 #ifndef PCL_NDT_2D_IMPL_H_
41 #define PCL_NDT_2D_IMPL_H_
42 
43 #include <pcl/registration/eigen.h>
44 #include <pcl/registration/boost.h>
45 
46 #include <cmath>
47 #include <memory>
48 
49 namespace pcl
50 {
51  namespace ndt2d
52  {
53  /** \brief Class to store vector value and first and second derivatives
54  * (grad vector and hessian matrix), so they can be returned easily from
55  * functions
56  */
57  template<unsigned N=3, typename T=double>
59  {
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
99  {
101 
102  public:
104  : min_n_ (3), n_ (0)
105  {
106  }
107 
108  /** \brief Store a point index to use later for estimating distribution parameters.
109  * \param[in] i Point index to store
110  */
111  void
112  addIdx (std::size_t i)
113  {
114  pt_indices_.push_back (i);
115  }
116 
117  /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared.
118  * \param[in] cloud Point cloud corresponding to indices passed to addIdx.
119  * \param[in] min_covar_eigvalue_mult Set the smallest eigenvalue to this times the largest.
120  */
121  void
122  estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001)
123  {
124  Eigen::Vector2d sx = Eigen::Vector2d::Zero ();
125  Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();
126 
127  for (auto i = pt_indices_.cbegin (); i != pt_indices_.cend (); i++)
128  {
129  Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);
130  sx += p;
131  sxx += p * p.transpose ();
132  }
133 
134  n_ = pt_indices_.size ();
135 
136  if (n_ >= min_n_)
137  {
138  mean_ = sx / static_cast<double> (n_);
139  // Using maximum likelihood estimation as in the original paper
140  Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast<double> (n_) + mean_ * mean_.transpose ();
141 
142  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver (covar);
143  if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1])
144  {
145  PCL_DEBUG ("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]);
146  Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();
147  Eigen::Matrix2d q = solver.eigenvectors ();
148  // set minimum smallest eigenvalue:
149  l (0,0) = l (1,1) * min_covar_eigvalue_mult;
150  covar = q * l * q.transpose ();
151  }
152  covar_inv_ = covar.inverse ();
153  }
154 
155  pt_indices_.clear ();
156  }
157 
158  /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
159  * \param[in] transformed_pt Location to evaluate at.
160  * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
161  * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
162  * estimateParams must have been called after at least three points were provided, or this will return zero.
163  *
164  */
166  test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
167  {
168  if (n_ < min_n_)
170 
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;
179 
180  Eigen::Matrix<double, 2, 3> jacobian;
181  jacobian <<
182  1, 0, -(x * sin_theta + y*cos_theta),
183  0, 1, x * cos_theta - y*sin_theta;
184 
185  for (std::size_t i = 0; i < 3; i++)
186  r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q;
187 
188  // second derivative only for i == j == 2:
189  const Eigen::Vector2d d2q_didj (
190  y * sin_theta - x*cos_theta,
191  -(x * sin_theta + y*cos_theta)
192  );
193 
194  for (std::size_t i = 0; i < 3; i++)
195  for (std::size_t j = 0; j < 3; j++)
196  r.hessian (i,j) = -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 
202  return r;
203  }
204 
205  protected:
206  const std::size_t min_n_;
207 
208  std::size_t n_;
209  std::vector<std::size_t> pt_indices_;
210  Eigen::Vector2d mean_;
211  Eigen::Matrix2d covar_inv_;
212  };
213 
214  /** \brief Build a set of normal distributions modelling a 2D point cloud,
215  * and provide the value and derivatives of the model at any point via the
216  * test (...) function.
217  */
218  template <typename PointT>
219  class NDTSingleGrid: public boost::noncopyable
220  {
222  using PointCloudConstPtr = typename PointCloud::ConstPtr;
224 
225  public:
226  NDTSingleGrid (PointCloudConstPtr cloud,
227  const Eigen::Vector2f& about,
228  const Eigen::Vector2f& extent,
229  const Eigen::Vector2f& step)
230  : min_ (about - extent), max_ (min_ + 2*extent), step_ (step),
231  cells_ ((max_[0]-min_[0]) / step_[0],
232  (max_[1]-min_[1]) / step_[1]),
233  normal_distributions_ (cells_[0], cells_[1])
234  {
235  // sort through all points, assigning them to distributions:
236  NormalDist* n;
237  std::size_t used_points = 0;
238  for (std::size_t i = 0; i < cloud->size (); i++)
239  if ((n = normalDistForPoint (cloud->at (i))))
240  {
241  n->addIdx (i);
242  used_points++;
243  }
244 
245  PCL_DEBUG ("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n", cells_[0], cells_[1], used_points, cloud->size ());
246 
247  // then bake the distributions such that they approximate the
248  // points (and throw away memory of the points)
249  for (int x = 0; x < cells_[0]; x++)
250  for (int y = 0; y < cells_[1]; y++)
251  normal_distributions_.coeffRef (x,y).estimateParams (*cloud);
252  }
253 
254  /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
255  * \param[in] transformed_pt Location to evaluate at.
256  * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
257  * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
258  */
260  test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
261  {
262  const NormalDist* n = normalDistForPoint (transformed_pt);
263  // index is in grid, return score from the normal distribution from
264  // the correct part of the grid:
265  if (n)
266  return n->test (transformed_pt, cos_theta, sin_theta);
268  }
269 
270  protected:
271  /** \brief Return the normal distribution covering the location of point p
272  * \param[in] p a point
273  */
274  NormalDist*
275  normalDistForPoint (PointT const& p) const
276  {
277  // this would be neater in 3d...
278  Eigen::Vector2f idxf;
279  for (std::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 (std::size_t i = 0; i < 2; i++)
283  if (idxi[i] >= cells_[i] || idxi[i] < 0)
284  return nullptr;
285  // const cast to avoid duplicating this function in const and
286  // non-const variants...
287  return const_cast<NormalDist*> (&normal_distributions_.coeffRef (idxi[0], idxi[1]));
288  }
289 
290  Eigen::Vector2f min_;
291  Eigen::Vector2f max_;
292  Eigen::Vector2f step_;
293  Eigen::Vector2i cells_;
294 
295  Eigen::Matrix<NormalDist, Eigen::Dynamic, Eigen::Dynamic> normal_distributions_;
296  };
297 
298  /** \brief Build a Normal Distributions Transform of a 2D point cloud. This
299  * consists of the sum of four overlapping models of the original points
300  * with normal distributions.
301  * The value and derivatives of the model at any point can be evaluated
302  * with the test (...) function.
303  */
304  template <typename PointT>
305  class NDT2D: public boost::noncopyable
306  {
308  using PointCloudConstPtr = typename PointCloud::ConstPtr;
310 
311  public:
312  /** \brief
313  * \param[in] cloud the input point cloud
314  * \param[in] about Centre of the grid for normal distributions model
315  * \param[in] extent Extent of grid for normal distributions model
316  * \param[in] step Size of region that each normal distribution will model
317  */
318  NDT2D (PointCloudConstPtr cloud,
319  const Eigen::Vector2f& about,
320  const Eigen::Vector2f& extent,
321  const Eigen::Vector2f& step)
322  {
323  Eigen::Vector2f dx (step[0]/2, 0);
324  Eigen::Vector2f dy (0, step[1]/2);
325  single_grids_[0].reset(new SingleGrid (cloud, about, extent, step));
326  single_grids_[1].reset(new SingleGrid (cloud, about +dx, extent, step));
327  single_grids_[2].reset(new SingleGrid (cloud, about +dy, extent, step));
328  single_grids_[3].reset(new SingleGrid (cloud, about +dx+dy, extent, step));
329  }
330 
331  /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
332  * \param[in] transformed_pt Location to evaluate at.
333  * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
334  * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation
335  */
337  test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
338  {
340  for (const auto &single_grid : single_grids_)
341  r += single_grid->test (transformed_pt, cos_theta, sin_theta);
342  return r;
343  }
344 
345  protected:
346  std::shared_ptr<SingleGrid> single_grids_[4];
347  };
348 
349  } // namespace ndt2d
350 } // namespace pcl
351 
352 
353 namespace Eigen
354 {
355  /* This NumTraits specialisation is necessary because NormalDist is used as
356  * the element type of an Eigen Matrix.
357  */
358  template<typename PointT> struct NumTraits<pcl::ndt2d::NormalDist<PointT> >
359  {
360  using Real = double;
361  using Literal = double;
362  static Real dummy_precision () { return 1.0; }
363  enum {
364  IsComplex = 0,
365  IsInteger = 0,
366  IsSigned = 0,
367  RequireInitialization = 1,
368  ReadCost = 1,
369  AddCost = 1,
370  MulCost = 1
371  };
372  };
373 }
374 
375 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
376 template <typename PointSource, typename PointTarget> void
377 pcl::NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
378 {
379  PointCloudSource intm_cloud = output;
380 
381  nr_iterations_ = 0;
382  converged_ = false;
383 
384  if (guess != Eigen::Matrix4f::Identity ())
385  {
386  transformation_ = guess;
387  transformPointCloud (output, intm_cloud, transformation_);
388  }
389 
390  // build Normal Distribution Transform of target cloud:
391  ndt2d::NDT2D<PointTarget> target_ndt (target_, grid_centre_, grid_extent_, grid_step_);
392 
393  // can't seem to use .block<> () member function on transformation_
394  // directly... gcc bug?
395  Eigen::Matrix4f& transformation = transformation_;
396 
397 
398  // work with x translation, y translation and z rotation: extending to 3D
399  // would be some tricky maths, but not impossible.
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]);
403 
404  Eigen::Vector3d xytheta_transformation (
405  transformation (0,3),
406  transformation (1,3),
407  z_rotation
408  );
409 
410  while (!converged_)
411  {
412  const double cos_theta = std::cos (xytheta_transformation[2]);
413  const double sin_theta = std::sin (xytheta_transformation[2]);
414  previous_transformation_ = transformation;
415 
417  for (std::size_t i = 0; i < intm_cloud.size (); i++)
418  score += target_ndt.test (intm_cloud[i], cos_theta, sin_theta);
419 
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]
422  );
423 
424  if (score.value != 0)
425  {
426  // test for positive definiteness, and adjust to ensure it if necessary:
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 ();
433 
434  // ensure "safe" positive definiteness: this is a detail missing
435  // from the original paper
436  if (min_eigenvalue < 0)
437  {
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",
442  float (lambda),
443  solver.eigenvalues ()[0].real (),
444  solver.eigenvalues ()[1].real (),
445  solver.eigenvalues ()[2].real ()
446  );
447  }
448  assert (solver.eigenvalues ()[0].real () >= 0 &&
449  solver.eigenvalues ()[1].real () >= 0 &&
450  solver.eigenvalues ()[2].real () >= 0);
451 
452  Eigen::Vector3d delta_transformation (-score.hessian.inverse () * score.grad);
453  Eigen::Vector3d new_transformation = xytheta_transformation + newton_lambda_.cwiseProduct (delta_transformation);
454 
455  xytheta_transformation = new_transformation;
456 
457  // update transformation matrix from x, y, theta:
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);
460 
461  //std::cout << "new transformation:\n" << transformation << std::endl;
462  }
463  else
464  {
465  PCL_ERROR ("[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n");
466  break;
467  }
468 
469  transformPointCloud (output, intm_cloud, transformation);
470 
471  nr_iterations_++;
472 
473  if (update_visualizer_)
474  update_visualizer_ (output, *indices_, *target_, *indices_);
475 
476  //std::cout << "eps=" << std::abs ((transformation - previous_transformation_).sum ()) << std::endl;
477 
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);
483 
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)))
488  {
489  converged_ = true;
490  }
491  }
492  final_transformation_ = transformation;
493  output = intm_cloud;
494 }
495 
496 #endif // PCL_NDT_2D_IMPL_H_
497 
Eigen::Vector2d mean_
Definition: ndt_2d.hpp:210
Eigen::Vector2f step_
Definition: ndt_2d.hpp:292
NDT2D(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Definition: ndt_2d.hpp:318
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the &#39;score&#39; (denormalised likelihood) and derivatives of score of the point p given this distr...
Definition: ndt_2d.hpp:337
ValueAndDerivatives< N, T > & operator+=(ValueAndDerivatives< N, T > const &r)
Definition: ndt_2d.hpp:77
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:122
Eigen::Matrix< T, N, 1 > grad
Definition: ndt_2d.hpp:63
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the &#39;score&#39; (denormalised likelihood) and derivatives of score of the point p given this distr...
Definition: ndt_2d.hpp:260
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
Eigen::Vector2f max_
Definition: ndt_2d.hpp:291
Definition: bfgs.h:9
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
Definition: ndt_2d.hpp:377
std::vector< std::size_t > pt_indices_
Definition: ndt_2d.hpp:209
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the &#39;score&#39; (denormalised likelihood) and derivatives of score of the point p given this distr...
Definition: ndt_2d.hpp:166
A normal distribution estimation class.
Definition: ndt_2d.hpp:98
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.
Definition: transforms.hpp:215
Build a Normal Distributions Transform of a 2D point cloud.
Definition: ndt_2d.hpp:305
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Eigen::Matrix< NormalDist, Eigen::Dynamic, Eigen::Dynamic > normal_distributions_
Definition: ndt_2d.hpp:295
NormalDist * normalDistForPoint(PointT const &p) const
Return the normal distribution covering the location of point p.
Definition: ndt_2d.hpp:275
static ValueAndDerivatives< N, T > Zero()
Definition: ndt_2d.hpp:67
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:412
void addIdx(std::size_t i)
Store a point index to use later for estimating distribution parameters.
Definition: ndt_2d.hpp:112
Eigen::Matrix2d covar_inv_
Definition: ndt_2d.hpp:211
Eigen::Vector2i cells_
Definition: ndt_2d.hpp:293
Eigen::Vector2f min_
Definition: ndt_2d.hpp:290
Class to store vector value and first and second derivatives (grad vector and hessian matrix)...
Definition: ndt_2d.hpp:58
const std::size_t min_n_
Definition: ndt_2d.hpp:206
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)
Definition: ndt_2d.hpp:226
Eigen::Matrix< T, N, N > hessian
Definition: ndt_2d.hpp:62
Build a set of normal distributions modelling a 2D point cloud, and provide the value and derivatives...
Definition: ndt_2d.hpp:219