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