Point Cloud Library (PCL) 1.12.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 }
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
199protected:
201 mutable std::vector<double> correspondence_weights_;
202
203 /** \brief Temporary pointer to the source dataset. */
205
206 /** \brief Temporary pointer to the target dataset. */
208
209 /** \brief Temporary pointer to the source dataset indices. */
211
212 /** \brief Temporary pointer to the target dataset indices. */
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>;
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() {}
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 {
284 return (*this);
285 }
286
287 /** \brief Destructor. */
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 {
332 return (*this);
333 }
334
335 /** \brief Destructor. */
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
350public:
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
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.