Point Cloud Library (PCL)  1.11.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 #pragma once
40 
41 #include <pcl/memory.h>
42 #include <pcl/pcl_macros.h>
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  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
63  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
64 
66 
67  using PointIndicesPtr = PointIndices::Ptr;
68  using PointIndicesConstPtr = PointIndices::ConstPtr;
69 
70  public:
71  using Ptr = shared_ptr<TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> >;
72  using ConstPtr = shared_ptr<const TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> >;
73 
74  using VectorX = Eigen::Matrix<MatScalar, Eigen::Dynamic, 1>;
75  using Vector4 = Eigen::Matrix<MatScalar, 4, 1>;
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. */
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  using Scalar = _Scalar;
213  enum
214  {
217  };
218  using InputType = Eigen::Matrix<_Scalar,InputsAtCompileTime,1>;
219  using ValueType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,1>;
220  using JacobianType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
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:
334  };
335  }
336 }
337 
338 #include <pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp>
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:429
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:430
TransformationEstimation represents the base class for methods for transformation estimation based on...
TransformationEstimationPointToPlane uses Levenberg Marquardt optimization to find the transformation...
TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt optimization to find the transf...
void setWarpFunction(const typename WarpPointRigid< PointSource, PointTarget, MatScalar >::Ptr &warp_fcn)
Set the function we use to warp points.
TransformationEstimationPointToPlaneWeighted(const TransformationEstimationPointToPlaneWeighted &src)
Copy constructor.
void setUseCorrespondenceWeights(bool use_correspondence_weights)
use the weights given in the pcl::CorrespondencesPtr for one of the estimateTransformation (....
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.
typename TransformationEstimation< PointSource, PointTarget, MatScalar >::Matrix4 Matrix4
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
TransformationEstimationPointToPlaneWeighted & operator=(const TransformationEstimationPointToPlaneWeighted &src)
Copy operator.
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
shared_ptr< const TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > > ConstPtr
shared_ptr< TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > > Ptr
pcl::registration::WarpPointRigid< PointSource, PointTarget, MatScalar >::Ptr warp_point_
The parameterized function used to warp the source to the target.
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
Defines functions, macros and traits for allocating and using memory.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Defines all the PCL and non-PCL macros used.
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:15
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:16
Base functor all the models that need non linear optimization must define their own one and implement...
OptimizationFunctor(int m_data_points, const TransformationEstimationPointToPlaneWeighted *estimator)
Functor constructor.
const TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > * estimator_
OptimizationFunctorWithIndices & operator=(const OptimizationFunctorWithIndices &src)
Copy operator.
const TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > * estimator_
OptimizationFunctorWithIndices(int m_data_points, const TransformationEstimationPointToPlaneWeighted *estimator)
Functor constructor.