Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
transformation_estimation_lm.hpp
Go to the documentation of this file.
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  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id: transformation_estimation_lm.hpp 5155 2012-03-17 21:45:48Z rusu $
37  *
38  */
39 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
40 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
41 
42 #include <pcl/registration/warp_point_rigid.h>
43 #include <pcl/registration/warp_point_rigid_6d.h>
44 #include <pcl/registration/distances.h>
45 #include <unsupported/Eigen/NonLinearOptimization>
47 template <typename PointSource, typename PointTarget> void
48 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
49  const pcl::PointCloud<PointSource> &cloud_src,
50  const pcl::PointCloud<PointTarget> &cloud_tgt,
51  Eigen::Matrix4f &transformation_matrix)
52 {
53 
54  // <cloud_src,cloud_src> is the source dataset
55  if (cloud_src.points.size () != cloud_tgt.points.size ())
56  {
57  PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
58  PCL_ERROR ("Number or points in source (%zu) differs than target (%zu)!\n",
59  cloud_src.points.size (), cloud_tgt.points.size ());
60  return;
61  }
62  if (cloud_src.points.size () < 4) // need at least 4 samples
63  {
64  PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
65  PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!\n",
66  cloud_src.points.size ());
67  return;
68  }
69 
70  // If no warp function has been set, use the default (WarpPointRigid6D)
71  if (!warp_point_)
72  warp_point_.reset (new WarpPointRigid6D<PointSource, PointTarget>);
73 
74  int n_unknowns = warp_point_->getDimension ();
75  Eigen::VectorXd x (n_unknowns);
76  x.setZero ();
77 
78  // Set temporary pointers
79  tmp_src_ = &cloud_src;
80  tmp_tgt_ = &cloud_tgt;
81 
82  OptimizationFunctor functor (static_cast<int> (cloud_src.points.size ()), this);
83  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
84  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
85  int info = lm.minimize (x);
86 
87  // Compute the norm of the residuals
88  PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
89  PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
90  PCL_DEBUG ("Final solution: [%f", x[0]);
91  for (int i = 1; i < n_unknowns; ++i)
92  PCL_DEBUG (" %f", x[i]);
93  PCL_DEBUG ("]\n");
94 
95  // Return the correct transformation
96  Eigen::VectorXf params = x.cast<float> ();
97  warp_point_->setParam (params);
98  transformation_matrix = warp_point_->getTransform ();
99 
100  tmp_src_ = NULL;
101  tmp_tgt_ = NULL;
102 }
103 
105 template <typename PointSource, typename PointTarget> void
106 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
107  const pcl::PointCloud<PointSource> &cloud_src,
108  const std::vector<int> &indices_src,
109  const pcl::PointCloud<PointTarget> &cloud_tgt,
110  Eigen::Matrix4f &transformation_matrix)
111 {
112  if (indices_src.size () != cloud_tgt.points.size ())
113  {
114  PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
115  return;
116  }
117 
118  // <cloud_src,cloud_src> is the source dataset
119  transformation_matrix.setIdentity ();
120 
121  const int nr_correspondences = static_cast<const int> (cloud_tgt.points.size ());
122  std::vector<int> indices_tgt;
123  indices_tgt.resize(nr_correspondences);
124  for (int i = 0; i < nr_correspondences; ++i)
125  indices_tgt[i] = i;
126 
127  estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
128 }
129 
131 template <typename PointSource, typename PointTarget> inline void
132 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
133  const pcl::PointCloud<PointSource> &cloud_src,
134  const std::vector<int> &indices_src,
135  const pcl::PointCloud<PointTarget> &cloud_tgt,
136  const std::vector<int> &indices_tgt,
137  Eigen::Matrix4f &transformation_matrix)
138 {
139  if (indices_src.size () != indices_tgt.size ())
140  {
141  PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
142  return;
143  }
144 
145  if (indices_src.size () < 4) // need at least 4 samples
146  {
147  PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
148  PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!",
149  indices_src.size ());
150  return;
151  }
152 
153  // If no warp function has been set, use the default (WarpPointRigid6D)
154  if (!warp_point_)
155  warp_point_.reset (new WarpPointRigid6D<PointSource, PointTarget>);
156 
157  int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space
158  Eigen::VectorXd x (n_unknowns);
159  x.setConstant (n_unknowns, 0);
160 
161  // Set temporary pointers
162  tmp_src_ = &cloud_src;
163  tmp_tgt_ = &cloud_tgt;
164  tmp_idx_src_ = &indices_src;
165  tmp_idx_tgt_ = &indices_tgt;
166 
167  OptimizationFunctorWithIndices functor (static_cast<int> (indices_src.size ()), this);
168  Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
169  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm (num_diff);
170  int info = lm.minimize (x);
171 
172  // Compute the norm of the residuals
173  PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
174  PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
175  PCL_DEBUG ("Final solution: [%f", x[0]);
176  for (int i = 1; i < n_unknowns; ++i)
177  PCL_DEBUG (" %f", x[i]);
178  PCL_DEBUG ("]\n");
179 
180  // Return the correct transformation
181  Eigen::VectorXf params = x.cast<float> ();
182  warp_point_->setParam (params);
183  transformation_matrix = warp_point_->getTransform ();
184 
185  tmp_src_ = NULL;
186  tmp_tgt_ = NULL;
187  tmp_idx_src_ = tmp_idx_tgt_ = NULL;
188 }
189 
191 template <typename PointSource, typename PointTarget> inline void
192 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
193  const pcl::PointCloud<PointSource> &cloud_src,
194  const pcl::PointCloud<PointTarget> &cloud_tgt,
195  const pcl::Correspondences &correspondences,
196  Eigen::Matrix4f &transformation_matrix)
197 {
198  const int nr_correspondences = static_cast<const int> (correspondences.size ());
199  std::vector<int> indices_src (nr_correspondences);
200  std::vector<int> indices_tgt (nr_correspondences);
201  for (int i = 0; i < nr_correspondences; ++i)
202  {
203  indices_src[i] = correspondences[i].index_query;
204  indices_tgt[i] = correspondences[i].index_match;
205  }
206 
207  estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
208 }
209 
211 template <typename PointSource, typename PointTarget> int
212 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::OptimizationFunctor::operator () (
213  const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
214 {
215  const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
216  const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
217 
218  // Initialize the warp function with the given parameters
219  Eigen::VectorXf params = x.cast<float> ();
220  estimator_->warp_point_->setParam (params);
221 
222  // Transform each source point and compute its distance to the corresponding target point
223  for (int i = 0; i < values (); ++i)
224  {
225  const PointSource & p_src = src_points.points[i];
226  const PointTarget & p_tgt = tgt_points.points[i];
227 
228  // Transform the source point based on the current warp parameters
229  PointSource p_src_warped;
230  estimator_->warp_point_->warpPoint (p_src, p_src_warped);
231 
232  // Estimate the distance (cost function)
233  fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);
234  }
235  return (0);
236 }
237 
239 template <typename PointSource, typename PointTarget> int
240 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::OptimizationFunctorWithIndices::operator() (
241  const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
242 {
243  const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
244  const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
245  const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
246  const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
247 
248  // Initialize the warp function with the given parameters
249  Eigen::VectorXf params = x.cast<float> ();
250  estimator_->warp_point_->setParam (params);
251 
252  // Transform each source point and compute its distance to the corresponding target point
253  for (int i = 0; i < values (); ++i)
254  {
255  const PointSource & p_src = src_points.points[src_indices[i]];
256  const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]];
257 
258  // Transform the source point based on the current warp parameters
259  PointSource p_src_warped;
260  estimator_->warp_point_->warpPoint (p_src, p_src_warped);
261 
262  // Estimate the distance (cost function)
263  fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);
264  }
265  return (0);
266 }
267 /*
269 template <typename PointSource, typename PointTarget> inline double
270 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::computeMedian (double *fvec, int m)
271 {
272  double median;
273  // Copy the values to vectors for faster sorting
274  std::vector<double> data (m);
275  memcpy (&data[0], fvec, sizeof (double) * m);
276 
277  std::sort (data.begin (), data.end ());
278 
279  int mid = data.size () / 2;
280  if (data.size () % 2 == 0)
281  median = (data[mid-1] + data[mid]) / 2.0;
282  else
283  median = data[mid];
284  return (median);
285 }
286 */
287 
288 //#define PCL_INSTANTIATE_TransformationEstimationLM(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationLM<T,U>;
289 
290 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ */
Generated on Sat Aug 24 2013 17:28:36 for Point Cloud Library (PCL) by   doxygen 1.8.4