Point Cloud Library (PCL) 1.15.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, warp_point_(new WarpPointRigid6D<PointSource, PointTarget, MatScalar>){};
54
55//////////////////////////////////////////////////////////////////////////////////////////////
56template <typename PointSource, typename PointTarget, typename MatScalar>
57void
60 const pcl::PointCloud<PointTarget>& cloud_tgt,
61 Matrix4& transformation_matrix) const
62{
63
64 // <cloud_src,cloud_src> is the source dataset
65 if (cloud_src.size() != cloud_tgt.size()) {
66 PCL_ERROR("[pcl::registration::TransformationEstimationLM::"
67 "estimateRigidTransformation] ");
68 PCL_ERROR("Number or points in source (%zu) differs than target (%zu)!\n",
69 static_cast<std::size_t>(cloud_src.size()),
70 static_cast<std::size_t>(cloud_tgt.size()));
71 return;
72 }
73 if (cloud_src.size() < 4) // need at least 4 samples
74 {
75 PCL_ERROR("[pcl::registration::TransformationEstimationLM::"
76 "estimateRigidTransformation] ");
77 PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
78 "%zu points!\n",
79 static_cast<std::size_t>(cloud_src.size()));
80 return;
81 }
82
83 int n_unknowns = warp_point_->getDimension();
84 VectorX x(n_unknowns);
85 x.setZero();
86
87 // Set temporary pointers
88 tmp_src_ = &cloud_src;
89 tmp_tgt_ = &cloud_tgt;
90
91 OptimizationFunctor functor(static_cast<int>(cloud_src.size()), this);
92 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
93 // Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm
94 // (num_diff);
95 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm(
96 num_diff);
97 int info = lm.minimize(x);
98
99 // Compute the norm of the residuals
100 PCL_DEBUG(
101 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
102 PCL_DEBUG("LM solver finished with exit code %i, having a residual norm of %g. \n",
103 info,
104 lm.fvec.norm());
105 PCL_DEBUG("Final solution: [%f", x[0]);
106 for (int i = 1; i < n_unknowns; ++i)
107 PCL_DEBUG(" %f", x[i]);
108 PCL_DEBUG("]\n");
109
110 // Return the correct transformation
111 warp_point_->setParam(x);
112 transformation_matrix = warp_point_->getTransform();
113
114 tmp_src_ = nullptr;
115 tmp_tgt_ = nullptr;
117
118//////////////////////////////////////////////////////////////////////////////////////////////
119template <typename PointSource, typename PointTarget, typename MatScalar>
120void
123 const pcl::Indices& indices_src,
124 const pcl::PointCloud<PointTarget>& cloud_tgt,
125 Matrix4& transformation_matrix) const
126{
127 if (indices_src.size() != cloud_tgt.size()) {
128 PCL_ERROR(
129 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
130 "Number or points in source (%zu) differs than target (%zu)!\n",
131 indices_src.size(),
132 static_cast<std::size_t>(cloud_tgt.size()));
133 return;
134 }
135
136 // <cloud_src,cloud_src> is the source dataset
137 transformation_matrix.setIdentity();
138
139 const auto nr_correspondences = cloud_tgt.size();
140 pcl::Indices indices_tgt;
141 indices_tgt.resize(nr_correspondences);
142 for (std::size_t i = 0; i < nr_correspondences; ++i)
143 indices_tgt[i] = i;
144
145 estimateRigidTransformation(
146 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
147}
148
149//////////////////////////////////////////////////////////////////////////////////////////////
150template <typename PointSource, typename PointTarget, typename MatScalar>
151inline void
154 const pcl::Indices& indices_src,
155 const pcl::PointCloud<PointTarget>& cloud_tgt,
156 const pcl::Indices& indices_tgt,
157 Matrix4& transformation_matrix) const
158{
159 if (indices_src.size() != indices_tgt.size()) {
160 PCL_ERROR(
161 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
162 "Number or points in source (%lu) differs than target (%lu)!\n",
163 indices_src.size(),
164 indices_tgt.size());
165 return;
166 }
167
168 if (indices_src.size() < 4) // need at least 4 samples
169 {
170 PCL_ERROR("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
171 PCL_ERROR("Need at least 4 points to estimate a transform! Source and target have "
172 "%lu points!",
173 indices_src.size());
174 return;
175 }
176
177 int n_unknowns = warp_point_->getDimension(); // get dimension of unknown space
178 VectorX x(n_unknowns);
179 x.setConstant(n_unknowns, 0);
180
181 // Set temporary pointers
182 tmp_src_ = &cloud_src;
183 tmp_tgt_ = &cloud_tgt;
184 tmp_idx_src_ = &indices_src;
185 tmp_idx_tgt_ = &indices_tgt;
186
187 OptimizationFunctorWithIndices functor(static_cast<int>(indices_src.size()), this);
188 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff(functor);
189 // Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm
190 // (num_diff);
191 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>,
192 MatScalar>
193 lm(num_diff);
194 int info = lm.minimize(x);
195
196 // Compute the norm of the residuals
197 PCL_DEBUG(
198 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM "
199 "solver finished with exit code %i, having a residual norm of %g. \n",
200 info,
201 lm.fvec.norm());
202 PCL_DEBUG("Final solution: [%f", x[0]);
203 for (int i = 1; i < n_unknowns; ++i)
204 PCL_DEBUG(" %f", x[i]);
205 PCL_DEBUG("]\n");
206
207 // Return the correct transformation
208 warp_point_->setParam(x);
209 transformation_matrix = warp_point_->getTransform();
210
211 tmp_src_ = nullptr;
212 tmp_tgt_ = nullptr;
213 tmp_idx_src_ = tmp_idx_tgt_ = nullptr;
214}
215
216//////////////////////////////////////////////////////////////////////////////////////////////
217template <typename PointSource, typename PointTarget, typename MatScalar>
218inline void
221 const pcl::PointCloud<PointTarget>& cloud_tgt,
222 const pcl::Correspondences& correspondences,
223 Matrix4& transformation_matrix) const
224{
225 const auto nr_correspondences = correspondences.size();
226 pcl::Indices indices_src(nr_correspondences);
227 pcl::Indices indices_tgt(nr_correspondences);
228 for (std::size_t i = 0; i < nr_correspondences; ++i) {
229 indices_src[i] = correspondences[i].index_query;
230 indices_tgt[i] = correspondences[i].index_match;
231 }
232
233 estimateRigidTransformation(
234 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
235}
236
237//////////////////////////////////////////////////////////////////////////////////////////////
238template <typename PointSource, typename PointTarget, typename MatScalar>
239int
242{
243 const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
244 const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
245
246 // Initialize the warp function with the given parameters
247 estimator_->warp_point_->setParam(x);
248
249 // Transform each source point and compute its distance to the corresponding target
250 // point
251 for (int i = 0; i < values(); ++i) {
252 const PointSource& p_src = src_points[i];
253 const PointTarget& p_tgt = tgt_points[i];
254
255 // Transform the source point based on the current warp parameters
256 Vector4 p_src_warped;
257 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
258
259 // Estimate the distance (cost function)
260 fvec[i] = estimator_->computeDistance(p_src_warped, p_tgt);
261 }
262 return (0);
263}
264
265//////////////////////////////////////////////////////////////////////////////////////////////
266template <typename PointSource, typename PointTarget, typename MatScalar>
267int
270{
271 const PointCloud<PointSource>& src_points = *estimator_->tmp_src_;
272 const PointCloud<PointTarget>& tgt_points = *estimator_->tmp_tgt_;
273 const pcl::Indices& src_indices = *estimator_->tmp_idx_src_;
274 const pcl::Indices& tgt_indices = *estimator_->tmp_idx_tgt_;
275
276 // Initialize the warp function with the given parameters
277 estimator_->warp_point_->setParam(x);
278
279 // Transform each source point and compute its distance to the corresponding target
280 // point
281 for (int i = 0; i < values(); ++i) {
282 const PointSource& p_src = src_points[src_indices[i]];
283 const PointTarget& p_tgt = tgt_points[tgt_indices[i]];
284
285 // Transform the source point based on the current warp parameters
286 Vector4 p_src_warped;
287 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
288
289 // Estimate the distance (cost function)
290 fvec[i] = estimator_->computeDistance(p_src_warped, p_tgt);
291 }
292 return (0);
293}
294
295// #define PCL_INSTANTIATE_TransformationEstimationLM(T,U) template class PCL_EXPORTS
296// pcl::registration::TransformationEstimationLM<T,U>;
297
298#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.