Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
transformation_estimation_point_to_plane_weighted.hpp
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#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_
40#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_
41
42#include <pcl/registration/distances.h>
43#include <pcl/registration/warp_point_rigid.h>
44#include <pcl/registration/warp_point_rigid_6d.h>
45
46#include <unsupported/Eigen/NonLinearOptimization>
47
48//////////////////////////////////////////////////////////////////////////////////////////////
49template <typename PointSource, typename PointTarget, typename MatScalar>
51 PointSource,
52 PointTarget,
53 MatScalar>::TransformationEstimationPointToPlaneWeighted()
54: tmp_src_()
55, tmp_tgt_()
56, warp_point_(new WarpPointRigid6D<PointSource, PointTarget, MatScalar>)
57{}
58
59//////////////////////////////////////////////////////////////////////////////////////////////
60template <typename PointSource, typename PointTarget, typename MatScalar>
61void
65 const pcl::PointCloud<PointTarget>& cloud_tgt,
66 Matrix4& transformation_matrix) const
67{
68
69 // <cloud_src,cloud_src> is the source dataset
70 if (cloud_src.size() != cloud_tgt.size()) {
71 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
72 "estimateRigidTransformation] ");
73 PCL_ERROR("Number or points in source (%zu) differs than target (%zu)!\n",
74 static_cast<std::size_t>(cloud_src.size()),
75 static_cast<std::size_t>(cloud_tgt.size()));
76 return;
77 }
78 if (cloud_src.size() < 4) // need at least 4 samples
79 {
80 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
81 "estimateRigidTransformation] ");
82 PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
83 "%zu points!\n",
84 static_cast<std::size_t>(cloud_src.size()));
85 return;
86 }
87
88 if (correspondence_weights_.size() != cloud_src.size()) {
89 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
90 "estimateRigidTransformation] ");
91 PCL_ERROR("Number of weights (%zu) differs than number of points (%zu)!\n",
92 correspondence_weights_.size(),
93 static_cast<std::size_t>(cloud_src.size()));
94 return;
95 }
96
97 int n_unknowns = warp_point_->getDimension();
98 VectorX x(n_unknowns);
99 x.setZero();
100
101 // Set temporary pointers
102 tmp_src_ = &cloud_src;
103 tmp_tgt_ = &cloud_tgt;
104
105 OptimizationFunctor functor(static_cast<int>(cloud_src.size()), this);
106 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
107 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm(
108 num_diff);
109 int info = lm.minimize(x);
110
111 // Compute the norm of the residuals
112 PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
113 "estimateRigidTransformation]");
114 PCL_DEBUG("LM solver finished with exit code %i, having a residual norm of %g. \n",
115 info,
116 lm.fvec.norm());
117 PCL_DEBUG("Final solution: [%f", x[0]);
118 for (int i = 1; i < n_unknowns; ++i)
119 PCL_DEBUG(" %f", x[i]);
120 PCL_DEBUG("]\n");
121
122 // Return the correct transformation
123 warp_point_->setParam(x);
124 transformation_matrix = warp_point_->getTransform();
125
126 tmp_src_ = NULL;
127 tmp_tgt_ = NULL;
129
130//////////////////////////////////////////////////////////////////////////////////////////////
131template <typename PointSource, typename PointTarget, typename MatScalar>
132void
136 const pcl::Indices& indices_src,
137 const pcl::PointCloud<PointTarget>& cloud_tgt,
138 Matrix4& transformation_matrix) const
139{
140 if (indices_src.size() != cloud_tgt.size()) {
141 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
142 "estimateRigidTransformation] Number or points in source (%zu) differs "
143 "than target (%zu)!\n",
144 indices_src.size(),
145 static_cast<std::size_t>(cloud_tgt.size()));
146 return;
147 }
148
149 if (correspondence_weights_.size() != indices_src.size()) {
150 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
151 "estimateRigidTransformation] ");
152 PCL_ERROR("Number of weights (%zu) differs than number of points (%zu)!\n",
153 correspondence_weights_.size(),
154 indices_src.size());
155 return;
156 }
158 // <cloud_src,cloud_src> is the source dataset
159 transformation_matrix.setIdentity();
160
161 const auto nr_correspondences = cloud_tgt.size();
162 pcl::Indices indices_tgt;
163 indices_tgt.resize(nr_correspondences);
164 for (std::size_t i = 0; i < nr_correspondences; ++i)
165 indices_tgt[i] = i;
166
167 estimateRigidTransformation(
168 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
169}
170
171//////////////////////////////////////////////////////////////////////////////////////////////
172template <typename PointSource, typename PointTarget, typename MatScalar>
173inline void
177 const pcl::Indices& indices_src,
178 const pcl::PointCloud<PointTarget>& cloud_tgt,
179 const pcl::Indices& indices_tgt,
180 Matrix4& transformation_matrix) const
181{
182 if (indices_src.size() != indices_tgt.size()) {
183 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
184 "estimateRigidTransformation] Number or points in source (%lu) differs "
185 "than target (%lu)!\n",
186 indices_src.size(),
187 indices_tgt.size());
188 return;
189 }
190
191 if (indices_src.size() < 4) // need at least 4 samples
192 {
193 PCL_ERROR("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
194 PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
195 "%lu points!\n",
196 indices_src.size());
197 return;
198 }
199
200 if (correspondence_weights_.size() != indices_src.size()) {
201 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
202 "estimateRigidTransformation] ");
203 PCL_ERROR("Number of weights (%lu) differs than number of points (%lu)!\n",
204 correspondence_weights_.size(),
205 indices_src.size());
206 return;
207 }
208
209 int n_unknowns = warp_point_->getDimension(); // get dimension of unknown space
210 VectorX x(n_unknowns);
211 x.setConstant(n_unknowns, 0);
212
213 // Set temporary pointers
214 tmp_src_ = &cloud_src;
215 tmp_tgt_ = &cloud_tgt;
216 tmp_idx_src_ = &indices_src;
217 tmp_idx_tgt_ = &indices_tgt;
218
219 OptimizationFunctorWithIndices functor(static_cast<int>(indices_src.size()), this);
220 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff(functor);
221 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>,
222 MatScalar>
223 lm(num_diff);
224 int info = lm.minimize(x);
225
226 // Compute the norm of the residuals
227 PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
228 "estimateRigidTransformation] LM solver finished with exit code %i, having "
229 "a residual norm of %g. \n",
230 info,
231 lm.fvec.norm());
232 PCL_DEBUG("Final solution: [%f", x[0]);
233 for (int i = 1; i < n_unknowns; ++i)
234 PCL_DEBUG(" %f", x[i]);
235 PCL_DEBUG("]\n");
236
237 // Return the correct transformation
238 warp_point_->setParam(x);
239 transformation_matrix = warp_point_->getTransform();
240
241 tmp_src_ = NULL;
242 tmp_tgt_ = NULL;
243 tmp_idx_src_ = tmp_idx_tgt_ = nullptr;
244}
245
246//////////////////////////////////////////////////////////////////////////////////////////////
247template <typename PointSource, typename PointTarget, typename MatScalar>
248inline void
252 const pcl::PointCloud<PointTarget>& cloud_tgt,
253 const pcl::Correspondences& correspondences,
254 Matrix4& transformation_matrix) const
255{
256 const int nr_correspondences = static_cast<int>(correspondences.size());
257 pcl::Indices indices_src(nr_correspondences);
258 pcl::Indices indices_tgt(nr_correspondences);
259 for (int i = 0; i < nr_correspondences; ++i) {
260 indices_src[i] = correspondences[i].index_query;
261 indices_tgt[i] = correspondences[i].index_match;
262 }
263
264 if (use_correspondence_weights_) {
265 correspondence_weights_.resize(nr_correspondences);
266 for (std::size_t i = 0; i < nr_correspondences; ++i)
267 correspondence_weights_[i] = correspondences[i].weight;
268 }
269
270 estimateRigidTransformation(
271 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
272}
273
274//////////////////////////////////////////////////////////////////////////////////////////////
275template <typename PointSource, typename PointTarget, typename MatScalar>
276int
278 PointSource,
279 PointTarget,
280 MatScalar>::OptimizationFunctor::operator()(const VectorX& x, VectorX& fvec) const
281{
282 const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
283 const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
284
285 // Initialize the warp function with the given parameters
286 estimator_->warp_point_->setParam(x);
287
288 // Transform each source point and compute its distance to the corresponding target
289 // point
290 for (int i = 0; i < values(); ++i) {
291 const PointSource& p_src = src_points[i];
292 const PointTarget& p_tgt = tgt_points[i];
293
294 // Transform the source point based on the current warp parameters
295 Vector4 p_src_warped;
296 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
297
298 // Estimate the distance (cost function)
299 fvec[i] = estimator_->correspondence_weights_[i] *
300 estimator_->computeDistance(p_src_warped, p_tgt);
301 }
302 return (0);
303}
304
305//////////////////////////////////////////////////////////////////////////////////////////////
306template <typename PointSource, typename PointTarget, typename MatScalar>
307int
309 PointSource,
310 PointTarget,
311 MatScalar>::OptimizationFunctorWithIndices::operator()(const VectorX& x,
312 VectorX& fvec) const
313{
314 const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
315 const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
316 const pcl::Indices& src_indices = *estimator_->tmp_idx_src_;
317 const pcl::Indices& tgt_indices = *estimator_->tmp_idx_tgt_;
318
319 // Initialize the warp function with the given parameters
320 estimator_->warp_point_->setParam(x);
321
322 // Transform each source point and compute its distance to the corresponding target
323 // point
324 for (int i = 0; i < values(); ++i) {
325 const PointSource& p_src = src_points[src_indices[i]];
326 const PointTarget& p_tgt = tgt_points[tgt_indices[i]];
327
328 // Transform the source point based on the current warp parameters
329 Vector4 p_src_warped;
330 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
331
332 // Estimate the distance (cost function)
333 fvec[i] = estimator_->correspondence_weights_[i] *
334 estimator_->computeDistance(p_src_warped, p_tgt);
335 }
336 return (0);
337}
338
339#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ */
PointCloud represents the base class in PCL for storing collections of 3D points.
std::size_t size() const
TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt optimization to find the transf...
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
WarpPointRigid3D enables 6D (3D rotation + 3D translation) transformations for points.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133