Point Cloud Library (PCL)  1.7.0
gicp.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 #ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
41 #define PCL_REGISTRATION_IMPL_GICP_HPP_
42 
43 #include <pcl/registration/boost.h>
44 #include <pcl/registration/exceptions.h>
45 
46 ///////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointSource, typename PointTarget> void
50 {
51  setInputSource (cloud);
52 }
53 
54 ////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointSource, typename PointTarget>
56 template<typename PointT> void
58  const typename pcl::search::KdTree<PointT>::Ptr kdtree,
59  std::vector<Eigen::Matrix3d>& cloud_covariances)
60 {
61  if (k_correspondences_ > int (cloud->size ()))
62  {
63  PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%zu) is less than k_correspondences_ (%zu)!\n", cloud->size (), k_correspondences_);
64  return;
65  }
66 
67  Eigen::Vector3d mean;
68  std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
69  std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
70 
71  // We should never get there but who knows
72  if(cloud_covariances.size () < cloud->size ())
73  cloud_covariances.resize (cloud->size ());
74 
75  typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
76  std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
77  for(;
78  points_iterator != cloud->end ();
79  ++points_iterator, ++matrices_iterator)
80  {
81  const PointT &query_point = *points_iterator;
82  Eigen::Matrix3d &cov = *matrices_iterator;
83  // Zero out the cov and mean
84  cov.setZero ();
85  mean.setZero ();
86 
87  // Search for the K nearest neighbours
88  kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
89 
90  // Find the covariance matrix
91  for(int j = 0; j < k_correspondences_; j++) {
92  const PointT &pt = (*cloud)[nn_indecies[j]];
93 
94  mean[0] += pt.x;
95  mean[1] += pt.y;
96  mean[2] += pt.z;
97 
98  cov(0,0) += pt.x*pt.x;
99 
100  cov(1,0) += pt.y*pt.x;
101  cov(1,1) += pt.y*pt.y;
102 
103  cov(2,0) += pt.z*pt.x;
104  cov(2,1) += pt.z*pt.y;
105  cov(2,2) += pt.z*pt.z;
106  }
107 
108  mean /= static_cast<double> (k_correspondences_);
109  // Get the actual covariance
110  for (int k = 0; k < 3; k++)
111  for (int l = 0; l <= k; l++)
112  {
113  cov(k,l) /= static_cast<double> (k_correspondences_);
114  cov(k,l) -= mean[k]*mean[l];
115  cov(l,k) = cov(k,l);
116  }
117 
118  // Compute the SVD (covariance matrix is symmetric so U = V')
119  Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
120  cov.setZero ();
121  Eigen::Matrix3d U = svd.matrixU ();
122  // Reconstitute the covariance matrix with modified singular values using the column // vectors in V.
123  for(int k = 0; k < 3; k++) {
124  Eigen::Vector3d col = U.col(k);
125  double v = 1.; // biggest 2 singular values replaced by 1
126  if(k == 2) // smallest singular value replaced by gicp_epsilon
127  v = gicp_epsilon_;
128  cov+= v * col * col.transpose();
129  }
130  }
131 }
132 
133 ////////////////////////////////////////////////////////////////////////////////////////
134 template <typename PointSource, typename PointTarget> void
136 {
137  Eigen::Matrix3d dR_dPhi;
138  Eigen::Matrix3d dR_dTheta;
139  Eigen::Matrix3d dR_dPsi;
140 
141  double phi = x[3], theta = x[4], psi = x[5];
142 
143  double cphi = cos(phi), sphi = sin(phi);
144  double ctheta = cos(theta), stheta = sin(theta);
145  double cpsi = cos(psi), spsi = sin(psi);
146 
147  dR_dPhi(0,0) = 0.;
148  dR_dPhi(1,0) = 0.;
149  dR_dPhi(2,0) = 0.;
150 
151  dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
152  dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
153  dR_dPhi(2,1) = cphi*ctheta;
154 
155  dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
156  dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
157  dR_dPhi(2,2) = -ctheta*sphi;
158 
159  dR_dTheta(0,0) = -cpsi*stheta;
160  dR_dTheta(1,0) = -spsi*stheta;
161  dR_dTheta(2,0) = -ctheta;
162 
163  dR_dTheta(0,1) = cpsi*ctheta*sphi;
164  dR_dTheta(1,1) = ctheta*sphi*spsi;
165  dR_dTheta(2,1) = -sphi*stheta;
166 
167  dR_dTheta(0,2) = cphi*cpsi*ctheta;
168  dR_dTheta(1,2) = cphi*ctheta*spsi;
169  dR_dTheta(2,2) = -cphi*stheta;
170 
171  dR_dPsi(0,0) = -ctheta*spsi;
172  dR_dPsi(1,0) = cpsi*ctheta;
173  dR_dPsi(2,0) = 0.;
174 
175  dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
176  dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
177  dR_dPsi(2,1) = 0.;
178 
179  dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
180  dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
181  dR_dPsi(2,2) = 0.;
182 
183  g[3] = matricesInnerProd(dR_dPhi, R);
184  g[4] = matricesInnerProd(dR_dTheta, R);
185  g[5] = matricesInnerProd(dR_dPsi, R);
186 }
187 
188 ////////////////////////////////////////////////////////////////////////////////////////
189 template <typename PointSource, typename PointTarget> void
191  const std::vector<int> &indices_src,
192  const PointCloudTarget &cloud_tgt,
193  const std::vector<int> &indices_tgt,
194  Eigen::Matrix4f &transformation_matrix)
195 {
196  if (indices_src.size () < 4) // need at least 4 samples
197  {
198  PCL_THROW_EXCEPTION (NotEnoughPointsException,
199  "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!");
200  return;
201  }
202  // Set the initial solution
203  Vector6d x = Vector6d::Zero ();
204  x[0] = transformation_matrix (0,3);
205  x[1] = transformation_matrix (1,3);
206  x[2] = transformation_matrix (2,3);
207  x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
208  x[4] = asin (-transformation_matrix (2,0));
209  x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
210 
211  // Set temporary pointers
212  tmp_src_ = &cloud_src;
213  tmp_tgt_ = &cloud_tgt;
214  tmp_idx_src_ = &indices_src;
215  tmp_idx_tgt_ = &indices_tgt;
216 
217  // Optimize using forward-difference approximation LM
218  const double gradient_tol = 1e-2;
219  OptimizationFunctorWithIndices functor(this);
221  bfgs.parameters.sigma = 0.01;
222  bfgs.parameters.rho = 0.01;
223  bfgs.parameters.tau1 = 9;
224  bfgs.parameters.tau2 = 0.05;
225  bfgs.parameters.tau3 = 0.5;
226  bfgs.parameters.order = 3;
227 
228  int inner_iterations_ = 0;
229  int result = bfgs.minimizeInit (x);
230  do
231  {
232  inner_iterations_++;
233  result = bfgs.minimizeOneStep (x);
234  if(result)
235  {
236  break;
237  }
238  result = bfgs.testGradient(gradient_tol);
239  } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
240  if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
241  {
242  PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
243  PCL_DEBUG ("BFGS solver finished with exit code %i \n", result);
244  transformation_matrix.setIdentity();
245  applyState(transformation_matrix, x);
246  }
247  else
248  PCL_THROW_EXCEPTION(SolverDidntConvergeException,
249  "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
250 }
251 
252 ////////////////////////////////////////////////////////////////////////////////////////
253 template <typename PointSource, typename PointTarget> inline double
255 {
256  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
257  gicp_->applyState(transformation_matrix, x);
258  double f = 0;
259  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
260  for (int i = 0; i < m; ++i)
261  {
262  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
263  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
264  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
265  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
266  Eigen::Vector4f pp (transformation_matrix * p_src);
267  // Estimate the distance (cost function)
268  // The last coordiante is still guaranteed to be set to 1.0
269  Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
270  Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
271  //increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes)
272  f+= double(res.transpose() * temp);
273  }
274  return f/m;
275 }
276 
277 ////////////////////////////////////////////////////////////////////////////////////////
278 template <typename PointSource, typename PointTarget> inline void
280 {
281  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
282  gicp_->applyState(transformation_matrix, x);
283  //Zero out g
284  g.setZero ();
285  //Eigen::Vector3d g_t = g.head<3> ();
286  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
287  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
288  for (int i = 0; i < m; ++i)
289  {
290  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
291  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
292  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
293  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
294 
295  Eigen::Vector4f pp (transformation_matrix * p_src);
296  // The last coordiante is still guaranteed to be set to 1.0
297  Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
298  // temp = M*res
299  Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
300  // Increment translation gradient
301  // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
302  g.head<3> ()+= temp;
303  // Increment rotation gradient
304  pp = gicp_->base_transformation_ * p_src;
305  Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
306  R+= p_src3 * temp.transpose();
307  }
308  g.head<3> ()*= 2.0/m;
309  R*= 2.0/m;
310  gicp_->computeRDerivative(x, R, g);
311 }
312 
313 ////////////////////////////////////////////////////////////////////////////////////////
314 template <typename PointSource, typename PointTarget> inline void
316 {
317  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
318  gicp_->applyState(transformation_matrix, x);
319  f = 0;
320  g.setZero ();
321  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
322  const int m = static_cast<const int> (gicp_->tmp_idx_src_->size ());
323  for (int i = 0; i < m; ++i)
324  {
325  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
326  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
327  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
328  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
329  Eigen::Vector4f pp (transformation_matrix * p_src);
330  // The last coordiante is still guaranteed to be set to 1.0
331  Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
332  // temp = M*res
333  Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
334  // Increment total error
335  f+= double(res.transpose() * temp);
336  // Increment translation gradient
337  // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
338  g.head<3> ()+= temp;
339  pp = gicp_->base_transformation_ * p_src;
340  Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
341  // Increment rotation gradient
342  R+= p_src3 * temp.transpose();
343  }
344  f/= double(m);
345  g.head<3> ()*= double(2.0/m);
346  R*= 2.0/m;
347  gicp_->computeRDerivative(x, R, g);
348 }
349 
350 ////////////////////////////////////////////////////////////////////////////////////////
351 template <typename PointSource, typename PointTarget> inline void
353 {
355  using namespace std;
356  // Difference between consecutive transforms
357  double delta = 0;
358  // Get the size of the target
359  const size_t N = indices_->size ();
360  // Set the mahalanobis matrices to identity
361  mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
362  // Compute target cloud covariance matrices
363  computeCovariances<PointTarget> (target_, tree_, target_covariances_);
364  // Compute input cloud covariance matrices
365  computeCovariances<PointSource> (input_, tree_reciprocal_, input_covariances_);
366 
367  base_transformation_ = guess;
368  nr_iterations_ = 0;
369  converged_ = false;
370  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
371  std::vector<int> nn_indices (1);
372  std::vector<float> nn_dists (1);
373 
374  while(!converged_)
375  {
376  size_t cnt = 0;
377  std::vector<int> source_indices (indices_->size ());
378  std::vector<int> target_indices (indices_->size ());
379 
380  // guess corresponds to base_t and transformation_ to t
381  Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
382  for(size_t i = 0; i < 4; i++)
383  for(size_t j = 0; j < 4; j++)
384  for(size_t k = 0; k < 4; k++)
385  transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
386 
387  Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
388 
389  for (size_t i = 0; i < N; i++)
390  {
391  PointSource query = output[i];
392  query.getVector4fMap () = guess * query.getVector4fMap ();
393  query.getVector4fMap () = transformation_ * query.getVector4fMap ();
394 
395  if (!searchForNeighbors (query, nn_indices, nn_dists))
396  {
397  PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]);
398  return;
399  }
400 
401  // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
402  if (nn_dists[0] < dist_threshold)
403  {
404  Eigen::Matrix3d &C1 = input_covariances_[i];
405  Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]];
406  Eigen::Matrix3d &M = mahalanobis_[i];
407  // M = R*C1
408  M = R * C1;
409  // temp = M*R' + C2 = R*C1*R' + C2
410  Eigen::Matrix3d temp = M * R.transpose();
411  temp+= C2;
412  // M = temp^-1
413  M = temp.inverse ();
414  source_indices[cnt] = static_cast<int> (i);
415  target_indices[cnt] = nn_indices[0];
416  cnt++;
417  }
418  }
419  // Resize to the actual number of valid correspondences
420  source_indices.resize(cnt); target_indices.resize(cnt);
421  /* optimize transformation using the current assignment and Mahalanobis metrics*/
423  //optimization right here
424  try
425  {
426  rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
427  /* compute the delta from this iteration */
428  delta = 0.;
429  for(int k = 0; k < 4; k++) {
430  for(int l = 0; l < 4; l++) {
431  double ratio = 1;
432  if(k < 3 && l < 3) // rotation part of the transform
433  ratio = 1./rotation_epsilon_;
434  else
435  ratio = 1./transformation_epsilon_;
436  double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
437  if(c_delta > delta)
438  delta = c_delta;
439  }
440  }
441  }
442  catch (PCLException &e)
443  {
444  PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
445  break;
446  }
447  nr_iterations_++;
448  // Check for convergence
449  if (nr_iterations_ >= max_iterations_ || delta < 1)
450  {
451  converged_ = true;
453  PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
454  getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
455  }
456  else
457  PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
458  }
459  //for some reason the static equivalent methode raises an error
460  // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0));
461  // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3);
462  final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3);
463  final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3);
464  final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3);
465  final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3);
466 }
467 
468 template <typename PointSource, typename PointTarget> void
470 {
471  // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
472  Eigen::Matrix3f R;
473  R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
474  * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
475  * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
476  t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
477  Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
478  t.col (3) += T;
479 }
480 
481 #endif //PCL_REGISTRATION_IMPL_GICP_HPP_