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