Point Cloud Library (PCL) 1.12.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, tmp_idx_src_()
57, tmp_idx_tgt_()
58, warp_point_(new WarpPointRigid6D<PointSource, PointTarget, MatScalar>)
59, use_correspondence_weights_(true){};
60
61//////////////////////////////////////////////////////////////////////////////////////////////
62template <typename PointSource, typename PointTarget, typename MatScalar>
63void
69{
70
71 // <cloud_src,cloud_src> is the source dataset
72 if (cloud_src.size() != cloud_tgt.size()) {
73 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
74 "estimateRigidTransformation] ");
75 PCL_ERROR("Number or points in source (%zu) differs than target (%zu)!\n",
76 static_cast<std::size_t>(cloud_src.size()),
77 static_cast<std::size_t>(cloud_tgt.size()));
78 return;
79 }
80 if (cloud_src.size() < 4) // need at least 4 samples
81 {
82 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
83 "estimateRigidTransformation] ");
84 PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
85 "%zu points!\n",
86 static_cast<std::size_t>(cloud_src.size()));
87 return;
88 }
89
90 if (correspondence_weights_.size() != cloud_src.size()) {
91 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
92 "estimateRigidTransformation] ");
93 PCL_ERROR("Number of weights (%zu) differs than number of points (%zu)!\n",
94 correspondence_weights_.size(),
95 static_cast<std::size_t>(cloud_src.size()));
96 return;
97 }
98
99 int n_unknowns = warp_point_->getDimension();
100 VectorX x(n_unknowns);
101 x.setZero();
102
103 // Set temporary pointers
104 tmp_src_ = &cloud_src;
105 tmp_tgt_ = &cloud_tgt;
106
107 OptimizationFunctor functor(static_cast<int>(cloud_src.size()), this);
108 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
109 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm(
110 num_diff);
111 int info = lm.minimize(x);
112
113 // Compute the norm of the residuals
114 PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
115 "estimateRigidTransformation]");
116 PCL_DEBUG("LM solver finished with exit code %i, having a residual norm of %g. \n",
117 info,
118 lm.fvec.norm());
119 PCL_DEBUG("Final solution: [%f", x[0]);
120 for (int i = 1; i < n_unknowns; ++i)
121 PCL_DEBUG(" %f", x[i]);
122 PCL_DEBUG("]\n");
123
124 // Return the correct transformation
125 warp_point_->setParam(x);
126 transformation_matrix = warp_point_->getTransform();
128 tmp_src_ = NULL;
129 tmp_tgt_ = NULL;
130}
131
132//////////////////////////////////////////////////////////////////////////////////////////////
133template <typename PointSource, typename PointTarget, typename MatScalar>
134void
141{
142 if (indices_src.size() != cloud_tgt.size()) {
143 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
144 "estimateRigidTransformation] Number or points in source (%zu) differs "
145 "than target (%zu)!\n",
147 static_cast<std::size_t>(cloud_tgt.size()));
148 return;
149 }
150
151 if (correspondence_weights_.size() != indices_src.size()) {
152 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
153 "estimateRigidTransformation] ");
154 PCL_ERROR("Number of weights (%zu) differs than number of points (%zu)!\n",
155 correspondence_weights_.size(),
157 return;
158 }
159
160 // <cloud_src,cloud_src> is the source dataset
161 transformation_matrix.setIdentity();
162
163 const auto nr_correspondences = cloud_tgt.size();
166 for (std::size_t i = 0; i < nr_correspondences; ++i)
167 indices_tgt[i] = i;
168
169 estimateRigidTransformation(
171}
172
173//////////////////////////////////////////////////////////////////////////////////////////////
174template <typename PointSource, typename PointTarget, typename MatScalar>
175inline void
183{
184 if (indices_src.size() != indices_tgt.size()) {
185 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
186 "estimateRigidTransformation] Number or points in source (%lu) differs "
187 "than target (%lu)!\n",
189 indices_tgt.size());
190 return;
191 }
192
193 if (indices_src.size() < 4) // need at least 4 samples
194 {
195 PCL_ERROR("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
196 PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
197 "%lu points!\n",
198 indices_src.size());
199 return;
200 }
201
202 if (correspondence_weights_.size() != indices_src.size()) {
203 PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
204 "estimateRigidTransformation] ");
205 PCL_ERROR("Number of weights (%lu) differs than number of points (%lu)!\n",
206 correspondence_weights_.size(),
207 indices_src.size());
208 return;
209 }
210
211 int n_unknowns = warp_point_->getDimension(); // get dimension of unknown space
213 x.setConstant(n_unknowns, 0);
214
215 // Set temporary pointers
216 tmp_src_ = &cloud_src;
217 tmp_tgt_ = &cloud_tgt;
218 tmp_idx_src_ = &indices_src;
219 tmp_idx_tgt_ = &indices_tgt;
220
221 OptimizationFunctorWithIndices functor(static_cast<int>(indices_src.size()), this);
222 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff(functor);
223 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>,
224 MatScalar>
225 lm(num_diff);
226 int info = lm.minimize(x);
227
228 // Compute the norm of the residuals
229 PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
230 "estimateRigidTransformation] LM solver finished with exit code %i, having "
231 "a residual norm of %g. \n",
232 info,
233 lm.fvec.norm());
234 PCL_DEBUG("Final solution: [%f", x[0]);
235 for (int i = 1; i < n_unknowns; ++i)
236 PCL_DEBUG(" %f", x[i]);
237 PCL_DEBUG("]\n");
238
239 // Return the correct transformation
240 warp_point_->setParam(x);
241 transformation_matrix = warp_point_->getTransform();
242
243 tmp_src_ = NULL;
244 tmp_tgt_ = NULL;
245 tmp_idx_src_ = tmp_idx_tgt_ = NULL;
246}
247
248//////////////////////////////////////////////////////////////////////////////////////////////
249template <typename PointSource, typename PointTarget, typename MatScalar>
250inline void
255 const pcl::Correspondences& correspondences,
257{
258 const int nr_correspondences = static_cast<int>(correspondences.size());
261 for (int i = 0; i < nr_correspondences; ++i) {
262 indices_src[i] = correspondences[i].index_query;
263 indices_tgt[i] = correspondences[i].index_match;
264 }
265
266 if (use_correspondence_weights_) {
267 correspondence_weights_.resize(nr_correspondences);
268 for (std::size_t i = 0; i < nr_correspondences; ++i)
269 correspondence_weights_[i] = correspondences[i].weight;
270 }
271
272 estimateRigidTransformation(
274}
275
276//////////////////////////////////////////////////////////////////////////////////////////////
277template <typename PointSource, typename PointTarget, typename MatScalar>
278int
280 PointSource,
281 PointTarget,
282 MatScalar>::OptimizationFunctor::operator()(const VectorX& x, VectorX& fvec) const
283{
284 const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
285 const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
286
287 // Initialize the warp function with the given parameters
288 estimator_->warp_point_->setParam(x);
289
290 // Transform each source point and compute its distance to the corresponding target
291 // point
292 for (int i = 0; i < values(); ++i) {
293 const PointSource& p_src = src_points[i];
294 const PointTarget& p_tgt = tgt_points[i];
296 // Transform the source point based on the current warp parameters
298 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
299
300 // Estimate the distance (cost function)
301 fvec[i] = estimator_->correspondence_weights_[i] *
302 estimator_->computeDistance(p_src_warped, p_tgt);
303 }
304 return (0);
305}
306
307//////////////////////////////////////////////////////////////////////////////////////////////
308template <typename PointSource, typename PointTarget, typename MatScalar>
309int
311 PointSource,
312 PointTarget,
313 MatScalar>::OptimizationFunctorWithIndices::operator()(const VectorX& x,
314 VectorX& fvec) const
315{
316 const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
317 const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
318 const pcl::Indices& src_indices = *estimator_->tmp_idx_src_;
319 const pcl::Indices& tgt_indices = *estimator_->tmp_idx_tgt_;
320
321 // Initialize the warp function with the given parameters
322 estimator_->warp_point_->setParam(x);
323
324 // Transform each source point and compute its distance to the corresponding target
325 // point
326 for (int i = 0; i < values(); ++i) {
327 const PointSource& p_src = src_points[src_indices[i]];
328 const PointTarget& p_tgt = tgt_points[tgt_indices[i]];
329
330 // Transform the source point based on the current warp parameters
332 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
333
334 // Estimate the distance (cost function)
335 fvec[i] = estimator_->correspondence_weights_[i] *
336 estimator_->computeDistance(p_src_warped, p_tgt);
337 }
338 return (0);
339}
340
341#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ */
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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