Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
transformation_estimation_lm.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
41#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
42
43#include <pcl/registration/warp_point_rigid_6d.h>
44
45#include <unsupported/Eigen/NonLinearOptimization>
46
47//////////////////////////////////////////////////////////////////////////////////////////////
48template <typename PointSource, typename PointTarget, typename MatScalar>
51: tmp_src_()
52, tmp_tgt_()
53, tmp_idx_src_()
54, tmp_idx_tgt_()
55, warp_point_(new WarpPointRigid6D<PointSource, PointTarget, MatScalar>){};
56
57//////////////////////////////////////////////////////////////////////////////////////////////
58template <typename PointSource, typename PointTarget, typename MatScalar>
59void
62 const pcl::PointCloud<PointTarget>& cloud_tgt,
63 Matrix4& transformation_matrix) const
64{
65
66 // <cloud_src,cloud_src> is the source dataset
67 if (cloud_src.size() != cloud_tgt.size()) {
68 PCL_ERROR("[pcl::registration::TransformationEstimationLM::"
69 "estimateRigidTransformation] ");
70 PCL_ERROR("Number or points in source (%zu) differs than target (%zu)!\n",
71 static_cast<std::size_t>(cloud_src.size()),
72 static_cast<std::size_t>(cloud_tgt.size()));
73 return;
74 }
75 if (cloud_src.size() < 4) // need at least 4 samples
76 {
77 PCL_ERROR("[pcl::registration::TransformationEstimationLM::"
78 "estimateRigidTransformation] ");
79 PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
80 "%zu points!\n",
81 static_cast<std::size_t>(cloud_src.size()));
82 return;
83 }
84
85 int n_unknowns = warp_point_->getDimension();
86 VectorX x(n_unknowns);
87 x.setZero();
88
89 // Set temporary pointers
90 tmp_src_ = &cloud_src;
91 tmp_tgt_ = &cloud_tgt;
92
93 OptimizationFunctor functor(static_cast<int>(cloud_src.size()), this);
94 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
95 // Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm
96 // (num_diff);
97 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm(
98 num_diff);
99 int info = lm.minimize(x);
100
101 // Compute the norm of the residuals
102 PCL_DEBUG(
103 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
104 PCL_DEBUG("LM solver finished with exit code %i, having a residual norm of %g. \n",
105 info,
106 lm.fvec.norm());
107 PCL_DEBUG("Final solution: [%f", x[0]);
108 for (int i = 1; i < n_unknowns; ++i)
109 PCL_DEBUG(" %f", x[i]);
110 PCL_DEBUG("]\n");
111
112 // Return the correct transformation
113 warp_point_->setParam(x);
114 transformation_matrix = warp_point_->getTransform();
115
116 tmp_src_ = nullptr;
117 tmp_tgt_ = nullptr;
118}
119
120//////////////////////////////////////////////////////////////////////////////////////////////
121template <typename PointSource, typename PointTarget, typename MatScalar>
122void
125 const pcl::Indices& indices_src,
126 const pcl::PointCloud<PointTarget>& cloud_tgt,
127 Matrix4& transformation_matrix) const
129 if (indices_src.size() != cloud_tgt.size()) {
130 PCL_ERROR(
131 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
132 "Number or points in source (%zu) differs than target (%zu)!\n",
133 indices_src.size(),
134 static_cast<std::size_t>(cloud_tgt.size()));
135 return;
136 }
137
138 // <cloud_src,cloud_src> is the source dataset
139 transformation_matrix.setIdentity();
140
141 const auto nr_correspondences = cloud_tgt.size();
142 pcl::Indices indices_tgt;
143 indices_tgt.resize(nr_correspondences);
144 for (std::size_t i = 0; i < nr_correspondences; ++i)
145 indices_tgt[i] = i;
146
147 estimateRigidTransformation(
148 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
149}
150
151//////////////////////////////////////////////////////////////////////////////////////////////
152template <typename PointSource, typename PointTarget, typename MatScalar>
153inline void
156 const pcl::Indices& indices_src,
157 const pcl::PointCloud<PointTarget>& cloud_tgt,
158 const pcl::Indices& indices_tgt,
159 Matrix4& transformation_matrix) const
160{
161 if (indices_src.size() != indices_tgt.size()) {
162 PCL_ERROR(
163 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
164 "Number or points in source (%lu) differs than target (%lu)!\n",
165 indices_src.size(),
166 indices_tgt.size());
167 return;
168 }
169
170 if (indices_src.size() < 4) // need at least 4 samples
171 {
172 PCL_ERROR("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
173 PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
174 "%lu points!",
175 indices_src.size());
176 return;
177 }
178
179 int n_unknowns = warp_point_->getDimension(); // get dimension of unknown space
180 VectorX x(n_unknowns);
181 x.setConstant(n_unknowns, 0);
182
183 // Set temporary pointers
184 tmp_src_ = &cloud_src;
185 tmp_tgt_ = &cloud_tgt;
186 tmp_idx_src_ = &indices_src;
187 tmp_idx_tgt_ = &indices_tgt;
188
189 OptimizationFunctorWithIndices functor(static_cast<int>(indices_src.size()), this);
190 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff(functor);
191 // Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm
192 // (num_diff);
193 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>,
194 MatScalar>
195 lm(num_diff);
196 int info = lm.minimize(x);
197
198 // Compute the norm of the residuals
199 PCL_DEBUG(
200 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM "
201 "solver finished with exit code %i, having a residual norm of %g. \n",
202 info,
203 lm.fvec.norm());
204 PCL_DEBUG("Final solution: [%f", x[0]);
205 for (int i = 1; i < n_unknowns; ++i)
206 PCL_DEBUG(" %f", x[i]);
207 PCL_DEBUG("]\n");
208
209 // Return the correct transformation
210 warp_point_->setParam(x);
211 transformation_matrix = warp_point_->getTransform();
212
213 tmp_src_ = nullptr;
214 tmp_tgt_ = nullptr;
215 tmp_idx_src_ = tmp_idx_tgt_ = nullptr;
216}
217
218//////////////////////////////////////////////////////////////////////////////////////////////
219template <typename PointSource, typename PointTarget, typename MatScalar>
220inline void
223 const pcl::PointCloud<PointTarget>& cloud_tgt,
224 const pcl::Correspondences& correspondences,
225 Matrix4& transformation_matrix) const
226{
227 const auto nr_correspondences = correspondences.size();
228 pcl::Indices indices_src(nr_correspondences);
229 pcl::Indices indices_tgt(nr_correspondences);
230 for (std::size_t i = 0; i < nr_correspondences; ++i) {
231 indices_src[i] = correspondences[i].index_query;
232 indices_tgt[i] = correspondences[i].index_match;
233 }
234
235 estimateRigidTransformation(
236 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
237}
238
239//////////////////////////////////////////////////////////////////////////////////////////////
240template <typename PointSource, typename PointTarget, typename MatScalar>
241int
244{
245 const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
246 const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
247
248 // Initialize the warp function with the given parameters
249 estimator_->warp_point_->setParam(x);
250
251 // Transform each source point and compute its distance to the corresponding target
252 // point
253 for (int i = 0; i < values(); ++i) {
254 const PointSource& p_src = src_points[i];
255 const PointTarget& p_tgt = tgt_points[i];
256
257 // Transform the source point based on the current warp parameters
258 Vector4 p_src_warped;
259 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
260
261 // Estimate the distance (cost function)
262 fvec[i] = estimator_->computeDistance(p_src_warped, p_tgt);
263 }
264 return (0);
265}
266
267//////////////////////////////////////////////////////////////////////////////////////////////
268template <typename PointSource, typename PointTarget, typename MatScalar>
269int
272{
273 const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
274 const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
275 const pcl::Indices& src_indices = *estimator_->tmp_idx_src_;
276 const pcl::Indices& tgt_indices = *estimator_->tmp_idx_tgt_;
277
278 // Initialize the warp function with the given parameters
279 estimator_->warp_point_->setParam(x);
280
281 // Transform each source point and compute its distance to the corresponding target
282 // point
283 for (int i = 0; i < values(); ++i) {
284 const PointSource& p_src = src_points[src_indices[i]];
285 const PointTarget& p_tgt = tgt_points[tgt_indices[i]];
286
287 // Transform the source point based on the current warp parameters
288 Vector4 p_src_warped;
289 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
290
291 // Estimate the distance (cost function)
292 fvec[i] = estimator_->computeDistance(p_src_warped, p_tgt);
293 }
294 return (0);
296
297//#define PCL_INSTANTIATE_TransformationEstimationLM(T,U) template class PCL_EXPORTS
298// pcl::registration::TransformationEstimationLM<T,U>;
299
300#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ */
PointCloud represents the base class in PCL for storing collections of 3D points.
std::size_t size() const
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const override
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
int operator()(const VectorX &x, VectorX &fvec) const
Fill fvec from x.