Point Cloud Library (PCL)  1.12.1-dev
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/registration/distances.h>
42 #include <pcl/registration/transformation_estimation_point_to_plane.h>
43 #include <pcl/registration/warp_point_rigid.h>
44 #include <pcl/memory.h>
45 #include <pcl/pcl_macros.h>
46 
47 namespace pcl {
48 namespace registration {
49 /** @b TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt
50  * optimization to find the transformation that minimizes the point-to-plane distance
51  * between the given correspondences. In addition to the
52  * TransformationEstimationPointToPlane class, this version takes per-correspondence
53  * weights and optimizes accordingly.
54  *
55  * \author Alexandru-Eugen Ichim
56  * \ingroup registration
57  */
58 template <typename PointSource, typename PointTarget, typename MatScalar = float>
60 : public TransformationEstimationPointToPlane<PointSource, PointTarget, MatScalar> {
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,
72  PointTarget,
73  MatScalar>>;
74  using ConstPtr =
75  shared_ptr<const TransformationEstimationPointToPlaneWeighted<PointSource,
76  PointTarget,
77  MatScalar>>;
78 
79  using VectorX = Eigen::Matrix<MatScalar, Eigen::Dynamic, 1>;
80  using Vector4 = Eigen::Matrix<MatScalar, 4, 1>;
81  using Matrix4 =
83 
84  /** \brief Constructor. */
86 
87  /** \brief Copy constructor.
88  * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into
89  * this
90  */
93  : tmp_src_(src.tmp_src_)
94  , tmp_tgt_(src.tmp_tgt_)
100 
101  /** \brief Copy operator.
102  * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into
103  * this
104  */
107  {
108  tmp_src_ = src.tmp_src_;
109  tmp_tgt_ = src.tmp_tgt_;
112  warp_point_ = src.warp_point_;
115  return (*this);
116  }
117 
118  /** \brief Destructor. */
120 
121  /** \brief Estimate a rigid rotation transformation between a source and a target
122  * point cloud using LM. \param[in] cloud_src the source point cloud dataset
123  * \param[in] cloud_tgt the target point cloud dataset
124  * \param[out] transformation_matrix the resultant transformation matrix
125  * \note Uses the weights given by setWeights.
126  */
127  inline void
129  const pcl::PointCloud<PointTarget>& cloud_tgt,
130  Matrix4& transformation_matrix) const;
131 
132  /** \brief Estimate a rigid rotation transformation between a source and a target
133  * point cloud using LM. \param[in] cloud_src the source point cloud dataset
134  * \param[in] indices_src the vector of indices describing the points of interest in
135  * \a cloud_src
136  * \param[in] cloud_tgt the target point cloud dataset
137  * \param[out] transformation_matrix the resultant transformation matrix
138  * \note Uses the weights given by setWeights.
139  */
140  inline void
142  const pcl::Indices& indices_src,
143  const pcl::PointCloud<PointTarget>& cloud_tgt,
144  Matrix4& transformation_matrix) const;
145 
146  /** \brief Estimate a rigid rotation transformation between a source and a target
147  * point cloud using LM. \param[in] cloud_src the source point cloud dataset
148  * \param[in] indices_src the vector of indices describing the points of interest in
149  * \a cloud_src
150  * \param[in] cloud_tgt the target point cloud dataset
151  * \param[in] indices_tgt the vector of indices describing the correspondences of the
152  * interest points from \a indices_src
153  * \param[out] transformation_matrix the resultant transformation matrix
154  * \note Uses the weights given by setWeights.
155  */
156  void
158  const pcl::Indices& indices_src,
159  const pcl::PointCloud<PointTarget>& cloud_tgt,
160  const pcl::Indices& indices_tgt,
161  Matrix4& transformation_matrix) const;
162 
163  /** \brief Estimate a rigid rotation transformation between a source and a target
164  * point cloud using LM. \param[in] cloud_src the source point cloud dataset
165  * \param[in] cloud_tgt the target point cloud dataset
166  * \param[in] correspondences the vector of correspondences between source and target
167  * point cloud \param[out] transformation_matrix the resultant transformation matrix
168  * \note Uses the weights given by setWeights.
169  */
170  void
172  const pcl::PointCloud<PointTarget>& cloud_tgt,
173  const pcl::Correspondences& correspondences,
174  Matrix4& transformation_matrix) const;
175 
176  inline void
177  setWeights(const std::vector<double>& weights)
178  {
179  correspondence_weights_ = weights;
180  }
181 
182  /// use the weights given in the pcl::CorrespondencesPtr for one of the
183  /// estimateTransformation (...) methods
184  inline void
185  setUseCorrespondenceWeights(bool use_correspondence_weights)
186  {
187  use_correspondence_weights_ = use_correspondence_weights;
188  }
189 
190  /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
191  * \param[in] warp_fcn a shared pointer to an object that warps points
192  */
193  void
196  {
197  warp_point_ = warp_fcn;
198  }
199 
200 protected:
202  mutable std::vector<double> correspondence_weights_;
203 
204  /** \brief Temporary pointer to the source dataset. */
205  mutable const PointCloudSource* tmp_src_;
206 
207  /** \brief Temporary pointer to the target dataset. */
208  mutable const PointCloudTarget* tmp_tgt_;
209 
210  /** \brief Temporary pointer to the source dataset indices. */
211  mutable const pcl::Indices* tmp_idx_src_;
212 
213  /** \brief Temporary pointer to the target dataset indices. */
214  mutable const pcl::Indices* tmp_idx_tgt_;
215 
216  /** \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,
222  * Eigen::VectorXd& fvec) or operator() (const Eigen::VectorXf& x, Eigen::VectorXf&
223  * fvec) depending on the chosen _Scalar
224  */
225  template <typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
226  struct Functor {
227  using Scalar = _Scalar;
229  using InputType = Eigen::Matrix<_Scalar, InputsAtCompileTime, 1>;
230  using ValueType = Eigen::Matrix<_Scalar, ValuesAtCompileTime, 1>;
231  using JacobianType =
232  Eigen::Matrix<_Scalar, ValuesAtCompileTime, InputsAtCompileTime>;
233 
234  /** \brief Empty Constructor. */
236 
237  /** \brief Constructor
238  * \param[in] m_data_points number of data points to evaluate.
239  */
240  Functor(int m_data_points) : m_data_points_(m_data_points) {}
241 
242  /** \brief Destructor. */
243  virtual ~Functor() = default;
244 
245  /** \brief Get the number of values. */
246  int
247  values() const
248  {
249  return (m_data_points_);
250  }
251 
252  protected:
254  };
255 
256  struct OptimizationFunctor : public Functor<MatScalar> {
258 
259  /** Functor constructor
260  * \param[in] m_data_points the number of data points to evaluate
261  * \param[in,out] estimator pointer to the estimator object
262  */
263  OptimizationFunctor(int m_data_points,
265  : Functor<MatScalar>(m_data_points), estimator_(estimator)
266  {}
267 
268  /** Copy constructor
269  * \param[in] src the optimization functor to copy into this
270  */
272  : Functor<MatScalar>(src.m_data_points_), estimator_()
273  {
274  *this = src;
275  }
276 
277  /** Copy operator
278  * \param[in] src the optimization functor to copy into this
279  */
280  inline OptimizationFunctor&
282  {
284  estimator_ = src.estimator_;
285  return (*this);
286  }
287 
288  /** \brief Destructor. */
289  virtual ~OptimizationFunctor() = default;
290 
291  /** Fill fvec from x. For the current state vector x fill the f values
292  * \param[in] x state vector
293  * \param[out] fvec f values vector
294  */
295  int
296  operator()(const VectorX& x, VectorX& fvec) const;
297 
299  PointTarget,
300  MatScalar>* estimator_;
301  };
302 
303  struct OptimizationFunctorWithIndices : public Functor<MatScalar> {
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  */
311  int m_data_points,
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. */
337  virtual ~OptimizationFunctorWithIndices() = default;
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  PointTarget,
348  MatScalar>* estimator_;
349  };
350 
351 public:
353 };
354 } // namespace registration
355 } // namespace pcl
356 
357 #include <pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp>
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:414
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
TransformationEstimationPointToPlaneWeighted & operator=(const TransformationEstimationPointToPlaneWeighted &src)
Copy operator.
const pcl::Indices * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
const pcl::Indices * tmp_idx_src_
Temporary pointer to the source dataset indices.
shared_ptr< const TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > > ConstPtr
virtual ~TransformationEstimationPointToPlaneWeighted()=default
Destructor.
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Defines all the PCL and non-PCL macros used.
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:13
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:14
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.