Point Cloud Library (PCL)  1.13.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 , tmp_idx_src_()
54 , tmp_idx_tgt_()
55 , warp_point_(new WarpPointRigid6D<PointSource, PointTarget, MatScalar>){};
56 
57 //////////////////////////////////////////////////////////////////////////////////////////////
58 template <typename PointSource, typename PointTarget, typename MatScalar>
59 void
60 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
61  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
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 //////////////////////////////////////////////////////////////////////////////////////////////
121 template <typename PointSource, typename PointTarget, typename MatScalar>
122 void
123 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
124  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
125  const pcl::Indices& indices_src,
126  const pcl::PointCloud<PointTarget>& cloud_tgt,
127  Matrix4& transformation_matrix) const
128 {
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 //////////////////////////////////////////////////////////////////////////////////////////////
152 template <typename PointSource, typename PointTarget, typename MatScalar>
153 inline void
154 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
155  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
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 //////////////////////////////////////////////////////////////////////////////////////////////
219 template <typename PointSource, typename PointTarget, typename MatScalar>
220 inline void
221 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
222  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
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 //////////////////////////////////////////////////////////////////////////////////////////////
240 template <typename PointSource, typename PointTarget, typename MatScalar>
241 int
242 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
243  OptimizationFunctor::operator()(const VectorX& x, VectorX& fvec) const
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 //////////////////////////////////////////////////////////////////////////////////////////////
268 template <typename PointSource, typename PointTarget, typename MatScalar>
269 int
270 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::
271  OptimizationFunctorWithIndices::operator()(const VectorX& x, VectorX& fvec) const
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);
295 }
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_ */
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:61
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:243