Point Cloud Library (PCL)  1.14.0-dev
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 //////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointSource, typename PointTarget, typename MatScalar>
49 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
50  TransformationEstimationLM()
51 : tmp_src_()
52 , tmp_tgt_()
53 , warp_point_(new WarpPointRigid6D<PointSource, PointTarget, MatScalar>){};
54 
55 //////////////////////////////////////////////////////////////////////////////////////////////
56 template <typename PointSource, typename PointTarget, typename MatScalar>
57 void
58 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
59  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
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;
116 }
117 
118 //////////////////////////////////////////////////////////////////////////////////////////////
119 template <typename PointSource, typename PointTarget, typename MatScalar>
120 void
121 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
122  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
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 //////////////////////////////////////////////////////////////////////////////////////////////
150 template <typename PointSource, typename PointTarget, typename MatScalar>
151 inline void
152 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
153  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
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 //////////////////////////////////////////////////////////////////////////////////////////////
217 template <typename PointSource, typename PointTarget, typename MatScalar>
218 inline void
219 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
220  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
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 //////////////////////////////////////////////////////////////////////////////////////////////
238 template <typename PointSource, typename PointTarget, typename MatScalar>
239 int
240 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
241  OptimizationFunctor::operator()(const VectorX& x, VectorX& fvec) const
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 //////////////////////////////////////////////////////////////////////////////////////////////
266 template <typename PointSource, typename PointTarget, typename MatScalar>
267 int
268 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
269  OptimizationFunctorWithIndices::operator()(const VectorX& x, VectorX& fvec) const
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_ */
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::registration::TransformationEstimationLM::estimateRigidTransformation
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.
Definition: transformation_estimation_lm.hpp:59
pcl::registration::TransformationEstimationLM::Matrix4
typename TransformationEstimation< PointSource, PointTarget, MatScalar >::Matrix4 Matrix4
Definition: transformation_estimation_lm.h:78
pcl::registration::WarpPointRigid6D
WarpPointRigid3D enables 6D (3D rotation + 3D translation) transformations for points.
Definition: warp_point_rigid_6d.h:56
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:89
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::registration::TransformationEstimationLM::OptimizationFunctor::operator()
int operator()(const VectorX &x, VectorX &fvec) const
Fill fvec from x.
Definition: transformation_estimation_lm.hpp:241