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