Point Cloud Library (PCL)  1.9.1
transformation_estimation_point_to_plane_weighted.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  * Copyright (c) Alexandru-Eugen Ichim
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  */
38 
39 
40 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_
41 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_
42 
43 #include <pcl/registration/transformation_estimation_point_to_plane.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 TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt optimization to find the transformation
52  * that minimizes the point-to-plane distance between the given correspondences. In addition to the
53  * TransformationEstimationPointToPlane class, this version takes per-correspondence weights and optimizes accordingly.
54  *
55  * \author Alexandru-Eugen Ichim
56  * \ingroup registration
57  */
58  template <typename PointSource, typename PointTarget, typename MatScalar = float>
59  class TransformationEstimationPointToPlaneWeighted : public TransformationEstimationPointToPlane<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<TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> > Ptr;
72  typedef boost::shared_ptr<const TransformationEstimationPointToPlaneWeighted<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 TransformationEstimationPointToPlaneWeighted object to copy into this
83  */
85  tmp_src_ (src.tmp_src_),
86  tmp_tgt_ (src.tmp_tgt_),
92  {};
93 
94  /** \brief Copy operator.
95  * \param[in] src the TransformationEstimationPointToPlaneWeighted 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_;
107  }
108 
109  /** \brief Destructor. */
111 
112  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
113  * \param[in] cloud_src the source point cloud dataset
114  * \param[in] cloud_tgt the target point cloud dataset
115  * \param[out] transformation_matrix the resultant transformation matrix
116  * \note Uses the weights given by setWeights.
117  */
118  inline void
120  const pcl::PointCloud<PointSource> &cloud_src,
121  const pcl::PointCloud<PointTarget> &cloud_tgt,
122  Matrix4 &transformation_matrix) const;
123 
124  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
125  * \param[in] cloud_src the source point cloud dataset
126  * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
127  * \param[in] cloud_tgt the target point cloud dataset
128  * \param[out] transformation_matrix the resultant transformation matrix
129  * \note Uses the weights given by setWeights.
130  */
131  inline void
133  const pcl::PointCloud<PointSource> &cloud_src,
134  const std::vector<int> &indices_src,
135  const pcl::PointCloud<PointTarget> &cloud_tgt,
136  Matrix4 &transformation_matrix) const;
137 
138  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
139  * \param[in] cloud_src the source point cloud dataset
140  * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
141  * \param[in] cloud_tgt the target point cloud dataset
142  * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from
143  * \a indices_src
144  * \param[out] transformation_matrix the resultant transformation matrix
145  * \note Uses the weights given by setWeights.
146  */
147  void
149  const pcl::PointCloud<PointSource> &cloud_src,
150  const std::vector<int> &indices_src,
151  const pcl::PointCloud<PointTarget> &cloud_tgt,
152  const std::vector<int> &indices_tgt,
153  Matrix4 &transformation_matrix) const;
154 
155  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
156  * \param[in] cloud_src the source point cloud dataset
157  * \param[in] cloud_tgt the target point cloud dataset
158  * \param[in] correspondences the vector of correspondences between source and target point cloud
159  * \param[out] transformation_matrix the resultant transformation matrix
160  * \note Uses the weights given by setWeights.
161  */
162  void
164  const pcl::PointCloud<PointSource> &cloud_src,
165  const pcl::PointCloud<PointTarget> &cloud_tgt,
166  const pcl::Correspondences &correspondences,
167  Matrix4 &transformation_matrix) const;
168 
169 
170  inline void
171  setWeights (const std::vector<double> &weights)
172  { correspondence_weights_ = weights; }
173 
174  /// use the weights given in the pcl::CorrespondencesPtr for one of the estimateTransformation (...) methods
175  inline void
176  setUseCorrespondenceWeights (bool use_correspondence_weights)
177  { use_correspondence_weights_ = use_correspondence_weights; }
178 
179  /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
180  * \param[in] warp_fcn a shared pointer to an object that warps points
181  */
182  void
184  { warp_point_ = warp_fcn; }
185 
186  protected:
188  mutable std::vector<double> correspondence_weights_;
189 
190  /** \brief Temporary pointer to the source dataset. */
191  mutable const PointCloudSource *tmp_src_;
192 
193  /** \brief Temporary pointer to the target dataset. */
194  mutable const PointCloudTarget *tmp_tgt_;
195 
196  /** \brief Temporary pointer to the source dataset indices. */
197  mutable const std::vector<int> *tmp_idx_src_;
198 
199  /** \brief Temporary pointer to the target dataset indices. */
200  mutable const std::vector<int> *tmp_idx_tgt_;
201 
202  /** \brief The parameterized function used to warp the source to the target. */
203  boost::shared_ptr<pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar> > warp_point_;
204 
205  /** Base functor all the models that need non linear optimization must
206  * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
207  * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar
208  */
209  template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
210  struct Functor
211  {
212  typedef _Scalar Scalar;
213  enum
214  {
217  };
218  typedef Eigen::Matrix<_Scalar,InputsAtCompileTime,1> InputType;
219  typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,1> ValueType;
220  typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
221 
222  /** \brief Empty Constructor. */
224 
225  /** \brief Constructor
226  * \param[in] m_data_points number of data points to evaluate.
227  */
228  Functor (int m_data_points) : m_data_points_ (m_data_points) {}
229 
230  /** \brief Destructor. */
231  virtual ~Functor () {}
232 
233  /** \brief Get the number of values. */
234  int
235  values () const { return (m_data_points_); }
236 
237  protected:
239  };
240 
241  struct OptimizationFunctor : public Functor<MatScalar>
242  {
244 
245  /** Functor constructor
246  * \param[in] m_data_points the number of data points to evaluate
247  * \param[in,out] estimator pointer to the estimator object
248  */
249  OptimizationFunctor (int m_data_points,
251  : Functor<MatScalar> (m_data_points), estimator_ (estimator)
252  {}
253 
254  /** Copy constructor
255  * \param[in] src the optimization functor to copy into this
256  */
258  Functor<MatScalar> (src.m_data_points_), estimator_ ()
259  {
260  *this = src;
261  }
262 
263  /** Copy operator
264  * \param[in] src the optimization functor to copy into this
265  */
266  inline OptimizationFunctor&
268  {
270  estimator_ = src.estimator_;
271  return (*this);
272  }
273 
274  /** \brief Destructor. */
275  virtual ~OptimizationFunctor () {}
276 
277  /** Fill fvec from x. For the current state vector x fill the f values
278  * \param[in] x state vector
279  * \param[out] fvec f values vector
280  */
281  int
282  operator () (const VectorX &x, VectorX &fvec) const;
283 
285  };
286 
287  struct OptimizationFunctorWithIndices : public Functor<MatScalar>
288  {
290 
291  /** Functor constructor
292  * \param[in] m_data_points the number of data points to evaluate
293  * \param[in,out] estimator pointer to the estimator object
294  */
295  OptimizationFunctorWithIndices (int m_data_points,
297  : Functor<MatScalar> (m_data_points), estimator_ (estimator)
298  {}
299 
300  /** Copy constructor
301  * \param[in] src the optimization functor to copy into this
302  */
304  : Functor<MatScalar> (src.m_data_points_), estimator_ ()
305  {
306  *this = src;
307  }
308 
309  /** Copy operator
310  * \param[in] src the optimization functor to copy into this
311  */
314  {
316  estimator_ = src.estimator_;
317  return (*this);
318  }
319 
320  /** \brief Destructor. */
322 
323  /** Fill fvec from x. For the current state vector x fill the f values
324  * \param[in] x state vector
325  * \param[out] fvec f values vector
326  */
327  int
328  operator () (const VectorX &x, VectorX &fvec) const;
329 
331  };
332  public:
333  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
334  };
335  }
336 }
337 
338 #include <pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp>
339 
340 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_ */
341 
boost::shared_ptr< const TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > > ConstPtr
boost::shared_ptr< pcl::registration::WarpPointRigid< PointSource, PointTarget, MatScalar > > warp_point_
The parameterized function used to warp the source to the target.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
void setUseCorrespondenceWeights(bool use_correspondence_weights)
use the weights given in the pcl::CorrespondencesPtr for one of the estimateTransformation (....
boost::shared_ptr< TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > > Ptr
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
TransformationEstimation< PointSource, PointTarget, MatScalar >::Matrix4 Matrix4
boost::shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:428
void setWarpFunction(const boost::shared_ptr< WarpPointRigid< PointSource, PointTarget, MatScalar > > &warp_fcn)
Set the function we use to warp points.
OptimizationFunctorWithIndices & operator=(const OptimizationFunctorWithIndices &src)
Copy operator.
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const
Estimate a rigid rotation transformation between a source and a target point cloud using LM.
Base functor all the models that need non linear optimization must define their own one and implement...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt optimization to find the transf...
boost::shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:429
OptimizationFunctor(int m_data_points, const TransformationEstimationPointToPlaneWeighted *estimator)
Functor constructor.
Eigen::Matrix< MatScalar, Eigen::Dynamic, 1 > VectorX
TransformationEstimationPointToPlaneWeighted & operator=(const TransformationEstimationPointToPlaneWeighted &src)
Copy operator.
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
TransformationEstimationPointToPlaneWeighted(const TransformationEstimationPointToPlaneWeighted &src)
Copy constructor.
TransformationEstimationPointToPlane uses Levenberg Marquardt optimization to find the transformation...
OptimizationFunctorWithIndices(int m_data_points, const TransformationEstimationPointToPlaneWeighted *estimator)
Functor constructor.
TransformationEstimation represents the base class for methods for transformation estimation based on...
const TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > * estimator_
const TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > * estimator_
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
Definition: PointIndices.h:23