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