Point Cloud Library (PCL)  1.7.1
ndt.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-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_REGISTRATION_NDT_H_
42 #define PCL_REGISTRATION_NDT_H_
43 
44 #include <pcl/registration/registration.h>
45 #include <pcl/filters/voxel_grid_covariance.h>
46 
47 #include <unsupported/Eigen/NonLinearOptimization>
48 
49 namespace pcl
50 {
51  /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
52  * \note For more information please see
53  * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
54  * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
55  * PhD thesis, Orebro University. Orebro Studies in Technology 36.</b>,
56  * <b>More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease
57  * In ACM Transactions on Mathematical Software.</b> and
58  * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100
59  * \note Math refactored by Todor Stoyanov.
60  * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
61  */
62  template<typename PointSource, typename PointTarget>
63  class NormalDistributionsTransform : public Registration<PointSource, PointTarget>
64  {
65  protected:
66 
68  typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
69  typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
70 
72  typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
73  typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
74 
77 
78  /** \brief Typename of searchable voxel grid containing mean and covariance. */
80  /** \brief Typename of pointer to searchable voxel grid. */
82  /** \brief Typename of const pointer to searchable voxel grid. */
84  /** \brief Typename of const pointer to searchable voxel grid leaf. */
86 
87 
88  public:
89 
90  typedef boost::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> > Ptr;
91  typedef boost::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> > ConstPtr;
92 
93 
94  /** \brief Constructor.
95  * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0
96  */
98 
99  /** \brief Empty destructor */
101 
102  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to).
103  * \param[in] cloud the input point cloud target
104  */
105  inline void
107  {
109  init ();
110  }
111 
112  /** \brief Set/change the voxel grid resolution.
113  * \param[in] resolution side length of voxels
114  */
115  inline void
116  setResolution (float resolution)
117  {
118  // Prevents unnessary voxel initiations
119  if (resolution_ != resolution)
120  {
121  resolution_ = resolution;
122  if (input_)
123  init ();
124  }
125  }
126 
127  /** \brief Get voxel grid resolution.
128  * \return side length of voxels
129  */
130  inline float
131  getResolution () const
132  {
133  return (resolution_);
134  }
135 
136  /** \brief Get the newton line search maximum step length.
137  * \return maximum step length
138  */
139  inline double
140  getStepSize () const
141  {
142  return (step_size_);
143  }
144 
145  /** \brief Set/change the newton line search maximum step length.
146  * \param[in] step_size maximum step length
147  */
148  inline void
149  setStepSize (double step_size)
150  {
151  step_size_ = step_size;
152  }
153 
154  /** \brief Get the point cloud outlier ratio.
155  * \return outlier ratio
156  */
157  inline double
158  getOulierRatio () const
159  {
160  return (outlier_ratio_);
161  }
162 
163  /** \brief Set/change the point cloud outlier ratio.
164  * \param[in] outlier_ratio outlier ratio
165  */
166  inline void
167  setOulierRatio (double outlier_ratio)
168  {
169  outlier_ratio_ = outlier_ratio;
170  }
171 
172  /** \brief Get the registration alignment probability.
173  * \return transformation probability
174  */
175  inline double
177  {
178  return (trans_probability_);
179  }
180 
181  /** \brief Get the number of iterations required to calculate alignment.
182  * \return final number of iterations
183  */
184  inline int
186  {
187  return (nr_iterations_);
188  }
189 
190  /** \brief Convert 6 element transformation vector to affine transformation.
191  * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
192  * \param[out] trans affine transform corresponding to given transfomation vector
193  */
194  static void
195  convertTransform (const Eigen::Matrix<double, 6, 1> &x, Eigen::Affine3f &trans)
196  {
197  trans = Eigen::Translation<float, 3>(float (x (0)), float (x (1)), float (x (2))) *
198  Eigen::AngleAxis<float>(float (x (3)), Eigen::Vector3f::UnitX ()) *
199  Eigen::AngleAxis<float>(float (x (4)), Eigen::Vector3f::UnitY ()) *
200  Eigen::AngleAxis<float>(float (x (5)), Eigen::Vector3f::UnitZ ());
201  }
202 
203  /** \brief Convert 6 element transformation vector to transformation matrix.
204  * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
205  * \param[out] trans 4x4 transformation matrix corresponding to given transfomation vector
206  */
207  static void
208  convertTransform (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix4f &trans)
209  {
210  Eigen::Affine3f _affine;
211  convertTransform (x, _affine);
212  trans = _affine.matrix ();
213  }
214 
215  protected:
216 
231 
233 
234  /** \brief Estimate the transformation and returns the transformed source (input) as output.
235  * \param[out] output the resultant input transfomed point cloud dataset
236  */
237  virtual void
239  {
240  computeTransformation (output, Eigen::Matrix4f::Identity ());
241  }
242 
243  /** \brief Estimate the transformation and returns the transformed source (input) as output.
244  * \param[out] output the resultant input transfomed point cloud dataset
245  * \param[in] guess the initial gross estimation of the transformation
246  */
247  virtual void
248  computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess);
249 
250  /** \brief Initiate covariance voxel structure. */
251  void inline
252  init ()
253  {
256  // Initiate voxel structure.
257  target_cells_.filter (true);
258  }
259 
260  /** \brief Compute derivatives of probability function w.r.t. the transformation vector.
261  * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
262  * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
263  * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
264  * \param[in] trans_cloud transformed point cloud
265  * \param[in] p the current transform vector
266  * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
267  */
268  double
269  computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
270  Eigen::Matrix<double, 6, 6> &hessian,
271  PointCloudSource &trans_cloud,
272  Eigen::Matrix<double, 6, 1> &p,
273  bool compute_hessian = true);
274 
275  /** \brief Compute individual point contirbutions to derivatives of probability function w.r.t. the transformation vector.
276  * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
277  * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
278  * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
279  * \param[in] x_trans transformed point minus mean of occupied covariance voxel
280  * \param[in] c_inv covariance of occupied covariance voxel
281  * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
282  */
283  double
284  updateDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
285  Eigen::Matrix<double, 6, 6> &hessian,
286  Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv,
287  bool compute_hessian = true);
288 
289  /** \brief Precompute anglular components of derivatives.
290  * \note Equation 6.19 and 6.21 [Magnusson 2009].
291  * \param[in] p the current transform vector
292  * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
293  */
294  void
295  computeAngleDerivatives (Eigen::Matrix<double, 6, 1> &p, bool compute_hessian = true);
296 
297  /** \brief Compute point derivatives.
298  * \note Equation 6.18-21 [Magnusson 2009].
299  * \param[in] x point from the input cloud
300  * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
301  */
302  void
303  computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian = true);
304 
305  /** \brief Compute hessian of probability function w.r.t. the transformation vector.
306  * \note Equation 6.13 [Magnusson 2009].
307  * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
308  * \param[in] trans_cloud transformed point cloud
309  * \param[in] p the current transform vector
310  */
311  void
312  computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
313  PointCloudSource &trans_cloud,
314  Eigen::Matrix<double, 6, 1> &p);
315 
316  /** \brief Compute individual point contirbutions to hessian of probability function w.r.t. the transformation vector.
317  * \note Equation 6.13 [Magnusson 2009].
318  * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
319  * \param[in] x_trans transformed point minus mean of occupied covariance voxel
320  * \param[in] c_inv covariance of occupied covariance voxel
321  */
322  void
323  updateHessian (Eigen::Matrix<double, 6, 6> &hessian,
324  Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv);
325 
326  /** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method.
327  * \note Search Algorithm [More, Thuente 1994]
328  * \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
329  * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009]
330  * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
331  * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994)
332  * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994)
333  * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009]
334  * \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009]
335  * \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009]
336  * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009]
337  * \return final step length
338  */
339  double
340  computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x,
341  Eigen::Matrix<double, 6, 1> &step_dir,
342  double step_init,
343  double step_max, double step_min,
344  double &score,
345  Eigen::Matrix<double, 6, 1> &score_gradient,
346  Eigen::Matrix<double, 6, 6> &hessian,
347  PointCloudSource &trans_cloud);
348 
349  /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994)
350  * \note Updating Algorithm until some value satifies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
351  * and Modified Updating Algorithm from then on [More, Thuente 1994].
352  * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
353  * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm
354  * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm
355  * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
356  * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm
357  * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified Update Algorithm
358  * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
359  * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm
360  * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm
361  * \return if interval converges
362  */
363  bool
364  updateIntervalMT (double &a_l, double &f_l, double &g_l,
365  double &a_u, double &f_u, double &g_u,
366  double a_t, double f_t, double g_t);
367 
368  /** \brief Select new trial value for More-Thuente method.
369  * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$
370  * until some value satifies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
371  * then \f$ \phi(\alpha_k) \f$ is used from then on.
372  * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).<\b>
373  * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
374  * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994)
375  * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994)
376  * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
377  * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994)
378  * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994)
379  * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
380  * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994)
381  * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994)
382  * \return new trial value
383  */
384  double
385  trialValueSelectionMT (double a_l, double f_l, double g_l,
386  double a_u, double f_u, double g_u,
387  double a_t, double f_t, double g_t);
388 
389  /** \brief Auxilary function used to determin endpoints of More-Thuente interval.
390  * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
391  * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
392  * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994)
393  * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994)
394  * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
395  * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
396  * \return sufficent decrease value
397  */
398  inline double
399  auxilaryFunction_PsiMT (double a, double f_a, double f_0, double g_0, double mu = 1.e-4)
400  {
401  return (f_a - f_0 - mu * g_0 * a);
402  }
403 
404  /** \brief Auxilary function derivative used to determin endpoints of More-Thuente interval.
405  * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994)
406  * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994)
407  * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
408  * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
409  * \return sufficent decrease derivative
410  */
411  inline double
412  auxilaryFunction_dPsiMT (double g_a, double g_0, double mu = 1.e-4)
413  {
414  return (g_a - mu * g_0);
415  }
416 
417  /** \brief The voxel grid generated from target cloud containing point means and covariances. */
419 
420  //double fitness_epsilon_;
421 
422  /** \brief The side length of voxels. */
423  float resolution_;
424 
425  /** \brief The maximum step length. */
426  double step_size_;
427 
428  /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */
430 
431  /** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */
433 
434  /** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */
436 
437  /** \brief Precomputed Angular Gradient
438  *
439  * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009].
440  */
442 
443  /** \brief Precomputed Angular Hessian
444  *
445  * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009].
446  */
447  Eigen::Vector3d h_ang_a2_, h_ang_a3_,
453 
454  /** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */
455  Eigen::Matrix<double, 3, 6> point_gradient_;
456 
457  /** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */
458  Eigen::Matrix<double, 18, 6> point_hessian_;
459 
460  public:
461  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
462 
463  };
464 
465 }
466 
467 #include <pcl/registration/impl/ndt.hpp>
468 
469 #endif // PCL_REGISTRATION_NDT_H_