Point Cloud Library (PCL) 1.13.0
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
47namespace pcl {
48namespace 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 */
58template <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
70public:
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_;
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
200protected:
202 mutable std::vector<double> correspondence_weights_;
203
204 /** \brief Temporary pointer to the source dataset. */
206
207 /** \brief Temporary pointer to the target dataset. */
209
210 /** \brief Temporary pointer to the source dataset indices. */
212
213 /** \brief Temporary pointer to the target dataset indices. */
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>;
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> {
257 using Functor<MatScalar>::values;
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 {
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> {
304 using Functor<MatScalar>::values;
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 {
333 return (*this);
334 }
335
336 /** \brief Destructor. */
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
351public:
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.
pcl::registration::WarpPointRigid< PointSource, PointTarget, MatScalar >::Ptr warp_point_
The parameterized function used to warp the source to the target.
shared_ptr< TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > > Ptr
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_
const TransformationEstimationPointToPlaneWeighted< PointSource, PointTarget, MatScalar > * estimator_
OptimizationFunctorWithIndices & operator=(const OptimizationFunctorWithIndices &src)
Copy operator.
OptimizationFunctorWithIndices(int m_data_points, const TransformationEstimationPointToPlaneWeighted *estimator)
Functor constructor.