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