Point Cloud Library (PCL)  1.11.1
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  , use_correspondence_weights_ (true)
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::TransformationEstimationPointToPlaneWeighted::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::TransformationEstimationPointToPlaneWeighted::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  if (correspondence_weights_.size () != cloud_src.size ())
87  {
88  PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
89  PCL_ERROR("Number of weights (%zu) differs than number of points (%zu)!\n",
90  correspondence_weights_.size(),
91  static_cast<std::size_t>(cloud_src.size()));
92  return;
93  }
94 
95  int n_unknowns = warp_point_->getDimension ();
96  VectorX x (n_unknowns);
97  x.setZero ();
98 
99  // Set temporary pointers
100  tmp_src_ = &cloud_src;
101  tmp_tgt_ = &cloud_tgt;
102 
103  OptimizationFunctor functor (static_cast<int> (cloud_src.size ()), this);
104  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
105  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
106  int info = lm.minimize (x);
107 
108  // Compute the norm of the residuals
109  PCL_DEBUG ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation]");
110  PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
111  PCL_DEBUG ("Final solution: [%f", x[0]);
112  for (int i = 1; i < n_unknowns; ++i)
113  PCL_DEBUG (" %f", x[i]);
114  PCL_DEBUG ("]\n");
115 
116  // Return the correct transformation
117  warp_point_->setParam (x);
118  transformation_matrix = warp_point_->getTransform ();
119 
120  tmp_src_ = NULL;
121  tmp_tgt_ = NULL;
122 }
123 
124 //////////////////////////////////////////////////////////////////////////////////////////////
125 template <typename PointSource, typename PointTarget, typename MatScalar> void
127  const pcl::PointCloud<PointSource> &cloud_src,
128  const std::vector<int> &indices_src,
129  const pcl::PointCloud<PointTarget> &cloud_tgt,
130  Matrix4 &transformation_matrix) const
131 {
132  if (indices_src.size () != cloud_tgt.size ())
133  {
134  PCL_ERROR("[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
135  "estimateRigidTransformation] Number or points in source (%zu) differs "
136  "than target (%zu)!\n",
137  indices_src.size(),
138  static_cast<std::size_t>(cloud_tgt.size()));
139  return;
140  }
141 
142  if (correspondence_weights_.size () != indices_src.size ())
143  {
144  PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
145  PCL_ERROR("Number of weights (%zu) differs than number of points (%zu)!\n",
146  correspondence_weights_.size(),
147  indices_src.size());
148  return;
149  }
150 
151 
152  // <cloud_src,cloud_src> is the source dataset
153  transformation_matrix.setIdentity ();
154 
155  const auto nr_correspondences = cloud_tgt.size ();
156  std::vector<int> indices_tgt;
157  indices_tgt.resize(nr_correspondences);
158  for (std::size_t i = 0; i < nr_correspondences; ++i)
159  indices_tgt[i] = i;
160 
161  estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
162 }
163 
164 //////////////////////////////////////////////////////////////////////////////////////////////
165 template <typename PointSource, typename PointTarget, typename MatScalar> inline void
167  const pcl::PointCloud<PointSource> &cloud_src,
168  const std::vector<int> &indices_src,
169  const pcl::PointCloud<PointTarget> &cloud_tgt,
170  const std::vector<int> &indices_tgt,
171  Matrix4 &transformation_matrix) const
172 {
173  if (indices_src.size () != indices_tgt.size ())
174  {
175  PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
176  return;
177  }
178 
179  if (indices_src.size () < 4) // need at least 4 samples
180  {
181  PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
182  PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
183  indices_src.size ());
184  return;
185  }
186 
187  if (correspondence_weights_.size () != indices_src.size ())
188  {
189  PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
190  PCL_ERROR ("Number of weights (%lu) differs than number of points (%lu)!\n",
191  correspondence_weights_.size (), indices_src.size ());
192  return;
193  }
194 
195 
196  int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space
197  VectorX x (n_unknowns);
198  x.setConstant (n_unknowns, 0);
199 
200  // Set temporary pointers
201  tmp_src_ = &cloud_src;
202  tmp_tgt_ = &cloud_tgt;
203  tmp_idx_src_ = &indices_src;
204  tmp_idx_tgt_ = &indices_tgt;
205 
206  OptimizationFunctorWithIndices functor (static_cast<int> (indices_src.size ()), this);
207  Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
208  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff);
209  int info = lm.minimize (x);
210 
211  // Compute the norm of the residuals
212  PCL_DEBUG ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
213  PCL_DEBUG ("Final solution: [%f", x[0]);
214  for (int i = 1; i < n_unknowns; ++i)
215  PCL_DEBUG (" %f", x[i]);
216  PCL_DEBUG ("]\n");
217 
218  // Return the correct transformation
219  warp_point_->setParam (x);
220  transformation_matrix = warp_point_->getTransform ();
221 
222  tmp_src_ = NULL;
223  tmp_tgt_ = NULL;
224  tmp_idx_src_ = tmp_idx_tgt_ = NULL;
225 }
226 
227 //////////////////////////////////////////////////////////////////////////////////////////////
228 template <typename PointSource, typename PointTarget, typename MatScalar> inline void
230  const pcl::PointCloud<PointSource> &cloud_src,
231  const pcl::PointCloud<PointTarget> &cloud_tgt,
232  const pcl::Correspondences &correspondences,
233  Matrix4 &transformation_matrix) const
234 {
235  const int nr_correspondences = static_cast<int> (correspondences.size ());
236  std::vector<int> indices_src (nr_correspondences);
237  std::vector<int> indices_tgt (nr_correspondences);
238  for (int i = 0; i < nr_correspondences; ++i)
239  {
240  indices_src[i] = correspondences[i].index_query;
241  indices_tgt[i] = correspondences[i].index_match;
242  }
243 
244  if (use_correspondence_weights_)
245  {
246  correspondence_weights_.resize (nr_correspondences);
247  for (std::size_t i = 0; i < nr_correspondences; ++i)
248  correspondence_weights_[i] = correspondences[i].weight;
249  }
250 
251  estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
252 }
253 
254 //////////////////////////////////////////////////////////////////////////////////////////////
255 template <typename PointSource, typename PointTarget, typename MatScalar> int
257  const VectorX &x, VectorX &fvec) const
258 {
259  const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
260  const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
261 
262  // Initialize the warp function with the given parameters
263  estimator_->warp_point_->setParam (x);
264 
265  // Transform each source point and compute its distance to the corresponding target point
266  for (int i = 0; i < values (); ++i)
267  {
268  const PointSource & p_src = src_points[i];
269  const PointTarget & p_tgt = tgt_points[i];
270 
271  // Transform the source point based on the current warp parameters
272  Vector4 p_src_warped;
273  estimator_->warp_point_->warpPoint (p_src, p_src_warped);
274 
275  // Estimate the distance (cost function)
276  fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);
277  }
278  return (0);
279 }
280 
281 //////////////////////////////////////////////////////////////////////////////////////////////
282 template <typename PointSource, typename PointTarget, typename MatScalar> int
284  const VectorX &x, VectorX &fvec) const
285 {
286  const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
287  const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
288  const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
289  const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
290 
291  // Initialize the warp function with the given parameters
292  estimator_->warp_point_->setParam (x);
293 
294  // Transform each source point and compute its distance to the corresponding target point
295  for (int i = 0; i < values (); ++i)
296  {
297  const PointSource & p_src = src_points[src_indices[i]];
298  const PointTarget & p_tgt = tgt_points[tgt_indices[i]];
299 
300  // Transform the source point based on the current warp parameters
301  Vector4 p_src_warped;
302  estimator_->warp_point_->warpPoint (p_src, p_src_warped);
303 
304  // Estimate the distance (cost function)
305  fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);
306  }
307  return (0);
308 }
309 
310 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ */
std::size_t size() const
Definition: point_cloud.h:459
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const
Estimate a rigid rotation transformation between a source and a target point cloud using LM...
typename TransformationEstimation< PointSource, PointTarget, MatScalar >::Matrix4 Matrix4
WarpPointRigid3D enables 6D (3D rotation + 3D translation) transformations for points.
Eigen::Matrix< MatScalar, Eigen::Dynamic, 1 > VectorX
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences