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