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  }
116 
117  /** \brief Destructor. */
119 
120  /** \brief Estimate a rigid rotation transformation between a source and a target
121  * point cloud using LM. \param[in] cloud_src the source point cloud dataset
122  * \param[in] cloud_tgt the target point cloud dataset
123  * \param[out] transformation_matrix the resultant transformation matrix
124  * \note Uses the weights given by setWeights.
125  */
126  inline void
128  const pcl::PointCloud<PointTarget>& cloud_tgt,
129  Matrix4& transformation_matrix) const;
130 
131  /** \brief Estimate a rigid rotation transformation between a source and a target
132  * point cloud using LM. \param[in] cloud_src the source point cloud dataset
133  * \param[in] indices_src the vector of indices describing the points of interest in
134  * \a cloud_src
135  * \param[in] cloud_tgt the target point cloud dataset
136  * \param[out] transformation_matrix the resultant transformation matrix
137  * \note Uses the weights given by setWeights.
138  */
139  inline void
141  const pcl::Indices& indices_src,
142  const pcl::PointCloud<PointTarget>& cloud_tgt,
143  Matrix4& transformation_matrix) const;
144 
145  /** \brief Estimate a rigid rotation transformation between a source and a target
146  * point cloud using LM. \param[in] cloud_src the source point cloud dataset
147  * \param[in] indices_src the vector of indices describing the points of interest in
148  * \a cloud_src
149  * \param[in] cloud_tgt the target point cloud dataset
150  * \param[in] indices_tgt the vector of indices describing the correspondences of the
151  * interest points from \a indices_src
152  * \param[out] transformation_matrix the resultant transformation matrix
153  * \note Uses the weights given by setWeights.
154  */
155  void
157  const pcl::Indices& indices_src,
158  const pcl::PointCloud<PointTarget>& cloud_tgt,
159  const pcl::Indices& indices_tgt,
160  Matrix4& transformation_matrix) const;
161 
162  /** \brief Estimate a rigid rotation transformation between a source and a target
163  * point cloud using LM. \param[in] cloud_src the source point cloud dataset
164  * \param[in] cloud_tgt the target point cloud dataset
165  * \param[in] correspondences the vector of correspondences between source and target
166  * point cloud \param[out] transformation_matrix the resultant transformation matrix
167  * \note Uses the weights given by setWeights.
168  */
169  void
171  const pcl::PointCloud<PointTarget>& cloud_tgt,
172  const pcl::Correspondences& correspondences,
173  Matrix4& transformation_matrix) const;
174 
175  inline void
176  setWeights(const std::vector<double>& weights)
177  {
178  correspondence_weights_ = weights;
179  }
180 
181  /// use the weights given in the pcl::CorrespondencesPtr for one of the
182  /// estimateTransformation (...) methods
183  inline void
184  setUseCorrespondenceWeights(bool use_correspondence_weights)
185  {
186  use_correspondence_weights_ = use_correspondence_weights;
187  }
188 
189  /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
190  * \param[in] warp_fcn a shared pointer to an object that warps points
191  */
192  void
195  {
196  warp_point_ = warp_fcn;
197  }
198 
199 protected:
201  mutable std::vector<double> correspondence_weights_;
202 
203  /** \brief Temporary pointer to the source dataset. */
204  mutable const PointCloudSource* tmp_src_;
205 
206  /** \brief Temporary pointer to the target dataset. */
207  mutable const PointCloudTarget* tmp_tgt_;
208 
209  /** \brief Temporary pointer to the source dataset indices. */
210  mutable const pcl::Indices* tmp_idx_src_;
211 
212  /** \brief Temporary pointer to the target dataset indices. */
213  mutable const pcl::Indices* tmp_idx_tgt_;
214 
215  /** \brief The parameterized function used to warp the source to the target. */
218 
219  /** Base functor all the models that need non linear optimization must
220  * define their own one and implement operator() (const Eigen::VectorXd& x,
221  * Eigen::VectorXd& fvec) or operator() (const Eigen::VectorXf& x, Eigen::VectorXf&
222  * fvec) depending on the chosen _Scalar
223  */
224  template <typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
225  struct Functor {
226  using Scalar = _Scalar;
228  using InputType = Eigen::Matrix<_Scalar, InputsAtCompileTime, 1>;
229  using ValueType = Eigen::Matrix<_Scalar, ValuesAtCompileTime, 1>;
230  using JacobianType =
231  Eigen::Matrix<_Scalar, ValuesAtCompileTime, InputsAtCompileTime>;
232 
233  /** \brief Empty Constructor. */
235 
236  /** \brief Constructor
237  * \param[in] m_data_points number of data points to evaluate.
238  */
239  Functor(int m_data_points) : m_data_points_(m_data_points) {}
240 
241  /** \brief Destructor. */
242  virtual ~Functor() = default;
243 
244  /** \brief Get the number of values. */
245  int
246  values() const
247  {
248  return (m_data_points_);
249  }
250 
251  protected:
253  };
254 
255  struct OptimizationFunctor : public Functor<MatScalar> {
257 
258  /** Functor constructor
259  * \param[in] m_data_points the number of data points to evaluate
260  * \param[in,out] estimator pointer to the estimator object
261  */
262  OptimizationFunctor(int m_data_points,
264  : Functor<MatScalar>(m_data_points), estimator_(estimator)
265  {}
266 
267  /** Copy constructor
268  * \param[in] src the optimization functor to copy into this
269  */
271  : Functor<MatScalar>(src.m_data_points_), estimator_()
272  {
273  *this = src;
274  }
275 
276  /** Copy operator
277  * \param[in] src the optimization functor to copy into this
278  */
279  inline OptimizationFunctor&
281  {
283  estimator_ = src.estimator_;
284  return (*this);
285  }
286 
287  /** \brief Destructor. */
288  virtual ~OptimizationFunctor() = default;
289 
290  /** Fill fvec from x. For the current state vector x fill the f values
291  * \param[in] x state vector
292  * \param[out] fvec f values vector
293  */
294  int
295  operator()(const VectorX& x, VectorX& fvec) const;
296 
298  PointTarget,
299  MatScalar>* estimator_;
300  };
301 
302  struct OptimizationFunctorWithIndices : public Functor<MatScalar> {
304 
305  /** Functor constructor
306  * \param[in] m_data_points the number of data points to evaluate
307  * \param[in,out] estimator pointer to the estimator object
308  */
310  int m_data_points,
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. */
336  virtual ~OptimizationFunctorWithIndices() = default;
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  PointTarget,
347  MatScalar>* estimator_;
348  };
349 
350 public:
352 };
353 } // namespace registration
354 } // namespace pcl
355 
356 #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.