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