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