Point Cloud Library (PCL)  1.9.1-dev
transformation_estimation_lm.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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 #pragma once
42 
43 #include <pcl/pcl_macros.h>
44 #include <pcl/registration/transformation_estimation.h>
45 #include <pcl/registration/warp_point_rigid.h>
46 #include <pcl/registration/distances.h>
47 
48 namespace pcl
49 {
50  namespace registration
51  {
52  /** @b TransformationEstimationLM implements Levenberg Marquardt-based
53  * estimation of the transformation aligning the given correspondences.
54  *
55  * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
56  * \author Radu B. Rusu
57  * \ingroup registration
58  */
59  template <typename PointSource, typename PointTarget, typename MatScalar = float>
60  class TransformationEstimationLM : public TransformationEstimation<PointSource, PointTarget, MatScalar>
61  {
63  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
64  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
65 
67 
68  using PointIndicesPtr = PointIndices::Ptr;
69  using PointIndicesConstPtr = PointIndices::ConstPtr;
70 
71  public:
72  using Ptr = boost::shared_ptr<TransformationEstimationLM<PointSource, PointTarget, MatScalar> >;
73  using ConstPtr = boost::shared_ptr<const TransformationEstimationLM<PointSource, PointTarget, MatScalar> >;
74 
75  using VectorX = Eigen::Matrix<MatScalar, Eigen::Dynamic, 1>;
76  using Vector4 = Eigen::Matrix<MatScalar, 4, 1>;
78 
79  /** \brief Constructor. */
81 
82  /** \brief Copy constructor.
83  * \param[in] src the TransformationEstimationLM object to copy into this
84  */
86  tmp_src_ (src.tmp_src_),
87  tmp_tgt_ (src.tmp_tgt_),
91  {};
92 
93  /** \brief Copy operator.
94  * \param[in] src the TransformationEstimationLM object to copy into this
95  */
98  {
99  tmp_src_ = src.tmp_src_;
100  tmp_tgt_ = src.tmp_tgt_;
102  tmp_idx_tgt_ = src.tmp_idx_tgt_;
103  warp_point_ = src.warp_point_;
104  }
105 
106  /** \brief Destructor. */
108 
109  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
110  * \param[in] cloud_src the source point cloud dataset
111  * \param[in] cloud_tgt the target point cloud dataset
112  * \param[out] transformation_matrix the resultant transformation matrix
113  */
114  inline void
116  const pcl::PointCloud<PointSource> &cloud_src,
117  const pcl::PointCloud<PointTarget> &cloud_tgt,
118  Matrix4 &transformation_matrix) const override;
119 
120  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
121  * \param[in] cloud_src the source point cloud dataset
122  * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
123  * \param[in] cloud_tgt the target point cloud dataset
124  * \param[out] transformation_matrix the resultant transformation matrix
125  */
126  inline void
128  const pcl::PointCloud<PointSource> &cloud_src,
129  const std::vector<int> &indices_src,
130  const pcl::PointCloud<PointTarget> &cloud_tgt,
131  Matrix4 &transformation_matrix) const override;
132 
133  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
134  * \param[in] cloud_src the source point cloud dataset
135  * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
136  * \param[in] cloud_tgt the target point cloud dataset
137  * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from
138  * \a indices_src
139  * \param[out] transformation_matrix the resultant transformation matrix
140  */
141  inline void
143  const pcl::PointCloud<PointSource> &cloud_src,
144  const std::vector<int> &indices_src,
145  const pcl::PointCloud<PointTarget> &cloud_tgt,
146  const std::vector<int> &indices_tgt,
147  Matrix4 &transformation_matrix) const override;
148 
149  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
150  * \param[in] cloud_src the source point cloud dataset
151  * \param[in] cloud_tgt the target point cloud dataset
152  * \param[in] correspondences the vector of correspondences between source and target point cloud
153  * \param[out] transformation_matrix the resultant transformation matrix
154  */
155  inline void
157  const pcl::PointCloud<PointSource> &cloud_src,
158  const pcl::PointCloud<PointTarget> &cloud_tgt,
159  const pcl::Correspondences &correspondences,
160  Matrix4 &transformation_matrix) const override;
161 
162  /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
163  * \param[in] warp_fcn a shared pointer to an object that warps points
164  */
165  void
167  {
168  warp_point_ = warp_fcn;
169  }
170 
171  protected:
172  /** \brief Compute the distance between a source point and its corresponding target point
173  * \param[in] p_src The source point
174  * \param[in] p_tgt The target point
175  * \return The distance between \a p_src and \a p_tgt
176  *
177  * \note Older versions of PCL used this method internally for calculating the
178  * optimization gradient. Since PCL 1.7, a switch has been made to the
179  * computeDistance method using Vector4 types instead. This method is only
180  * kept for API compatibility reasons.
181  */
182  virtual MatScalar
183  computeDistance (const PointSource &p_src, const PointTarget &p_tgt) const
184  {
185  Vector4 s (p_src.x, p_src.y, p_src.z, 0);
186  Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
187  return ((s - t).norm ());
188  }
189 
190  /** \brief Compute the distance between a source point and its corresponding target point
191  * \param[in] p_src The source point
192  * \param[in] p_tgt The target point
193  * \return The distance between \a p_src and \a p_tgt
194  *
195  * \note A different distance function can be defined by creating a subclass of
196  * TransformationEstimationLM and overriding this method.
197  * (See \a TransformationEstimationPointToPlane)
198  */
199  virtual MatScalar
200  computeDistance (const Vector4 &p_src, const PointTarget &p_tgt) const
201  {
202  Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
203  return ((p_src - t).norm ());
204  }
205 
206  /** \brief Temporary pointer to the source dataset. */
207  mutable const PointCloudSource *tmp_src_;
208 
209  /** \brief Temporary pointer to the target dataset. */
210  mutable const PointCloudTarget *tmp_tgt_;
211 
212  /** \brief Temporary pointer to the source dataset indices. */
213  mutable const std::vector<int> *tmp_idx_src_;
214 
215  /** \brief Temporary pointer to the target dataset indices. */
216  mutable const std::vector<int> *tmp_idx_tgt_;
217 
218  /** \brief The parameterized function used to warp the source to the target. */
220 
221  /** Base functor all the models that need non linear optimization must
222  * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
223  * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar
224  */
225  template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
226  struct Functor
227  {
228  using Scalar = _Scalar;
229  enum
230  {
233  };
234  using InputType = Eigen::Matrix<_Scalar,InputsAtCompileTime,1>;
235  using ValueType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,1>;
236  using JacobianType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
237 
238  /** \brief Empty Constructor. */
240 
241  /** \brief Constructor
242  * \param[in] m_data_points number of data points to evaluate.
243  */
244  Functor (int m_data_points) : m_data_points_ (m_data_points) {}
245 
246  /** \brief Destructor. */
247  virtual ~Functor () {}
248 
249  /** \brief Get the number of values. */
250  int
251  values () const { return (m_data_points_); }
252 
253  protected:
255  };
256 
257  struct OptimizationFunctor : public Functor<MatScalar>
258  {
260 
261  /** Functor constructor
262  * \param[in] m_data_points the number of data points to evaluate
263  * \param[in,out] estimator pointer to the estimator object
264  */
265  OptimizationFunctor (int m_data_points,
266  const TransformationEstimationLM *estimator)
267  : Functor<MatScalar> (m_data_points), estimator_ (estimator)
268  {}
269 
270  /** Copy constructor
271  * \param[in] src the optimization functor to copy into this
272  */
274  Functor<MatScalar> (src.m_data_points_), estimator_ ()
275  {
276  *this = src;
277  }
278 
279  /** Copy operator
280  * \param[in] src the optimization functor to copy into this
281  */
282  inline OptimizationFunctor&
284  {
286  estimator_ = src.estimator_;
287  return (*this);
288  }
289 
290  /** \brief Destructor. */
292 
293  /** Fill fvec from x. For the current state vector x fill the f values
294  * \param[in] x state vector
295  * \param[out] fvec f values vector
296  */
297  int
298  operator () (const VectorX &x, VectorX &fvec) const;
299 
301  };
302 
303  struct OptimizationFunctorWithIndices : public Functor<MatScalar>
304  {
306 
307  /** Functor constructor
308  * \param[in] m_data_points the number of data points to evaluate
309  * \param[in,out] estimator pointer to the estimator object
310  */
311  OptimizationFunctorWithIndices (int m_data_points,
312  const TransformationEstimationLM *estimator)
313  : Functor<MatScalar> (m_data_points), estimator_ (estimator)
314  {}
315 
316  /** Copy constructor
317  * \param[in] src the optimization functor to copy into this
318  */
320  : Functor<MatScalar> (src.m_data_points_), estimator_ ()
321  {
322  *this = src;
323  }
324 
325  /** Copy operator
326  * \param[in] src the optimization functor to copy into this
327  */
330  {
332  estimator_ = src.estimator_;
333  return (*this);
334  }
335 
336  /** \brief Destructor. */
338 
339  /** Fill fvec from x. For the current state vector x fill the f values
340  * \param[in] x state vector
341  * \param[out] fvec f values vector
342  */
343  int
344  operator () (const VectorX &x, VectorX &fvec) const;
345 
347  };
348  public:
350  };
351  }
352 }
353 
354 #include <pcl/registration/impl/transformation_estimation_lm.hpp>
Eigen::Matrix< MatScalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
TransformationEstimationLM & operator=(const TransformationEstimationLM &src)
Copy operator.
const TransformationEstimationLM< PointSource, PointTarget, MatScalar > * estimator_
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::shared_ptr< TransformationEstimation< PointSource, PointTarget, Scalar > > Ptr
OptimizationFunctorWithIndices(const OptimizationFunctorWithIndices &src)
Copy constructor.
const TransformationEstimationLM< PointSource, PointTarget, MatScalar > * estimator_
virtual MatScalar computeDistance(const Vector4 &p_src, const PointTarget &p_tgt) const
Compute the distance between a source point and its corresponding target point.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:359
virtual MatScalar computeDistance(const PointSource &p_src, const PointTarget &p_tgt) const
Compute the distance between a source point and its corresponding target point.
boost::shared_ptr< const TransformationEstimation< PointSource, PointTarget, Scalar > > ConstPtr
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const override
Estimate a rigid rotation transformation between a source and a target point cloud using LM...
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
Base functor all the models that need non linear optimization must define their own one and implement...
pcl::registration::WarpPointRigid< PointSource, PointTarget, MatScalar >::Ptr warp_point_
The parameterized function used to warp the source to the target.
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
void setWarpFunction(const typename WarpPointRigid< PointSource, PointTarget, MatScalar >::Ptr &warp_fcn)
Set the function we use to warp points.
boost::shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:411
boost::shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:23
boost::shared_ptr< WarpPointRigid< PointSourceT, PointTargetT, Scalar > > Ptr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
TransformationEstimationLM(const TransformationEstimationLM &src)
Copy constructor.
OptimizationFunctorWithIndices(int m_data_points, const TransformationEstimationLM *estimator)
Functor constructor.
TransformationEstimationLM implements Levenberg Marquardt-based estimation of the transformation alig...
boost::shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:412
OptimizationFunctor(int m_data_points, const TransformationEstimationLM *estimator)
Functor constructor.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
TransformationEstimation represents the base class for methods for transformation estimation based on...
OptimizationFunctor(const OptimizationFunctor &src)
Copy constructor.
Defines all the PCL and non-PCL macros used.
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.