Point Cloud Library (PCL)  1.8.1
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  MatricesVector& cloud_covariances)
60 {
61  if (k_correspondences_ > int (cloud->size ()))
62  {
63  PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\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  MatricesVector::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  result = BFGSSpace::Running;
231  do
232  {
233  inner_iterations_++;
234  result = bfgs.minimizeOneStep (x);
235  if(result)
236  {
237  break;
238  }
239  result = bfgs.testGradient(gradient_tol);
240  } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
241  if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
242  {
243  PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
244  PCL_DEBUG ("BFGS solver finished with exit code %i \n", result);
245  transformation_matrix.setIdentity();
246  applyState(transformation_matrix, x);
247  }
248  else
249  PCL_THROW_EXCEPTION(SolverDidntConvergeException,
250  "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
251 }
252 
253 ////////////////////////////////////////////////////////////////////////////////////////
254 template <typename PointSource, typename PointTarget> inline double
256 {
257  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
258  gicp_->applyState(transformation_matrix, x);
259  double f = 0;
260  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
261  for (int i = 0; i < m; ++i)
262  {
263  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
264  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
265  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
266  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
267  Eigen::Vector4f pp (transformation_matrix * p_src);
268  // Estimate the distance (cost function)
269  // The last coordiante is still guaranteed to be set to 1.0
270  Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
271  Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
272  //increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes)
273  f+= double(res.transpose() * temp);
274  }
275  return f/m;
276 }
277 
278 ////////////////////////////////////////////////////////////////////////////////////////
279 template <typename PointSource, typename PointTarget> inline void
281 {
282  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
283  gicp_->applyState(transformation_matrix, x);
284  //Zero out g
285  g.setZero ();
286  //Eigen::Vector3d g_t = g.head<3> ();
287  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
288  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
289  for (int i = 0; i < m; ++i)
290  {
291  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
292  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
293  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
294  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
295 
296  Eigen::Vector4f pp (transformation_matrix * p_src);
297  // The last coordiante is still guaranteed to be set to 1.0
298  Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
299  // temp = M*res
300  Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
301  // Increment translation gradient
302  // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
303  g.head<3> ()+= temp;
304  // Increment rotation gradient
305  pp = gicp_->base_transformation_ * p_src;
306  Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
307  R+= p_src3 * temp.transpose();
308  }
309  g.head<3> ()*= 2.0/m;
310  R*= 2.0/m;
311  gicp_->computeRDerivative(x, R, g);
312 }
313 
314 ////////////////////////////////////////////////////////////////////////////////////////
315 template <typename PointSource, typename PointTarget> inline void
317 {
318  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
319  gicp_->applyState(transformation_matrix, x);
320  f = 0;
321  g.setZero ();
322  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
323  const int m = static_cast<const int> (gicp_->tmp_idx_src_->size ());
324  for (int i = 0; i < m; ++i)
325  {
326  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
327  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
328  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
329  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
330  Eigen::Vector4f pp (transformation_matrix * p_src);
331  // The last coordiante is still guaranteed to be set to 1.0
332  Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
333  // temp = M*res
334  Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
335  // Increment total error
336  f+= double(res.transpose() * temp);
337  // Increment translation gradient
338  // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
339  g.head<3> ()+= temp;
340  pp = gicp_->base_transformation_ * p_src;
341  Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
342  // Increment rotation gradient
343  R+= p_src3 * temp.transpose();
344  }
345  f/= double(m);
346  g.head<3> ()*= double(2.0/m);
347  R*= 2.0/m;
348  gicp_->computeRDerivative(x, R, g);
349 }
350 
351 ////////////////////////////////////////////////////////////////////////////////////////
352 template <typename PointSource, typename PointTarget> inline void
354 {
356  using namespace std;
357  // Difference between consecutive transforms
358  double delta = 0;
359  // Get the size of the target
360  const size_t N = indices_->size ();
361  // Set the mahalanobis matrices to identity
362  mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
363  // Compute target cloud covariance matrices
364  if ((!target_covariances_) || (target_covariances_->empty ()))
365  {
366  target_covariances_.reset (new MatricesVector);
367  computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
368  }
369  // Compute input cloud covariance matrices
370  if ((!input_covariances_) || (input_covariances_->empty ()))
371  {
373  computeCovariances<PointSource> (input_, tree_reciprocal_, *input_covariances_);
374  }
375 
376  base_transformation_ = Eigen::Matrix4f::Identity();
377  nr_iterations_ = 0;
378  converged_ = false;
379  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
380  std::vector<int> nn_indices (1);
381  std::vector<float> nn_dists (1);
382 
383  pcl::transformPointCloud(output, output, guess);
384 
385  while(!converged_)
386  {
387  size_t cnt = 0;
388  std::vector<int> source_indices (indices_->size ());
389  std::vector<int> target_indices (indices_->size ());
390 
391  // guess corresponds to base_t and transformation_ to t
392  Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
393  for(size_t i = 0; i < 4; i++)
394  for(size_t j = 0; j < 4; j++)
395  for(size_t k = 0; k < 4; k++)
396  transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
397 
398  Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
399 
400  for (size_t i = 0; i < N; i++)
401  {
402  PointSource query = output[i];
403  query.getVector4fMap () = transformation_ * query.getVector4fMap ();
404 
405  if (!searchForNeighbors (query, nn_indices, nn_dists))
406  {
407  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]);
408  return;
409  }
410 
411  // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
412  if (nn_dists[0] < dist_threshold)
413  {
414  Eigen::Matrix3d &C1 = (*input_covariances_)[i];
415  Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
416  Eigen::Matrix3d &M = mahalanobis_[i];
417  // M = R*C1
418  M = R * C1;
419  // temp = M*R' + C2 = R*C1*R' + C2
420  Eigen::Matrix3d temp = M * R.transpose();
421  temp+= C2;
422  // M = temp^-1
423  M = temp.inverse ();
424  source_indices[cnt] = static_cast<int> (i);
425  target_indices[cnt] = nn_indices[0];
426  cnt++;
427  }
428  }
429  // Resize to the actual number of valid correspondences
430  source_indices.resize(cnt); target_indices.resize(cnt);
431  /* optimize transformation using the current assignment and Mahalanobis metrics*/
433  //optimization right here
434  try
435  {
436  rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
437  /* compute the delta from this iteration */
438  delta = 0.;
439  for(int k = 0; k < 4; k++) {
440  for(int l = 0; l < 4; l++) {
441  double ratio = 1;
442  if(k < 3 && l < 3) // rotation part of the transform
443  ratio = 1./rotation_epsilon_;
444  else
445  ratio = 1./transformation_epsilon_;
446  double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
447  if(c_delta > delta)
448  delta = c_delta;
449  }
450  }
451  }
452  catch (PCLException &e)
453  {
454  PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
455  break;
456  }
457  nr_iterations_++;
458  // Check for convergence
459  if (nr_iterations_ >= max_iterations_ || delta < 1)
460  {
461  converged_ = true;
463  PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
464  getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
465  }
466  else
467  PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
468  }
470 
471  // Transform the point cloud
473 }
474 
475 template <typename PointSource, typename PointTarget> void
477 {
478  // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
479  Eigen::Matrix3f R;
480  R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
481  * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
482  * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
483  t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
484  Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
485  t.col (3) += T;
486 }
487 
488 #endif //PCL_REGISTRATION_IMPL_GICP_HPP_
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.
Definition: registration.h:488
Eigen::Matrix4f base_transformation_
base transformation
Definition: gicp.h:275
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
const std::string & getClassName() const
Abstract class get name method.
Definition: registration.h:422
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const std::vector< int > &indices_src, const PointCloudTarget &cloud_tgt, const std::vector< int > &indices_tgt, Eigen::Matrix4f &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
Definition: gicp.hpp:190
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
boost::function< void(const pcl::PointCloud< PointSource > &cloud_src, const std::vector< int > &src_indices, const pcl::PointCloud< PointTarget > &cloud_tgt, const std::vector< int > &tgt_indices, Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_
Definition: gicp.h:369
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
Definition: gicp.h:297
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method with initial guess.
Definition: gicp.hpp:353
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
Definition: gicp.h:284
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
Definition: gicp.h:342
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Definition: registration.h:491
const GeneralizedIterativeClosestPoint * gicp_
Definition: gicp.h:362
iterator end()
Definition: point_cloud.h:443
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:153
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for the k-nearest neighbors for the given query point.
Definition: kdtree.hpp:88
void fdf(const Vector6d &x, double &f, Vector6d &df)
Definition: gicp.hpp:316
void computeCovariances(typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, MatricesVector &cloud_covariances)
compute points covariances matrices according to the K nearest neighbors.
Definition: gicp.hpp:57
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
Definition: gicp.h:278
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
Definition: gicp.h:287
void setInputCloud(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: gicp.hpp:48
Parameters parameters
Definition: bfgs.h:155
KdTreePtr tree_
A pointer to the spatial search object.
Definition: registration.h:485
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
Definition: registration.h:511
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Definition: registration.h:508
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:496
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:79
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
Definition: registration.h:505
PointCloudTargetConstPtr target_
The input point cloud dataset target.
Definition: registration.h:502
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:42
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
Definition: gicp.h:281
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Definition: gicp.hpp:476
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
PointCloud represents the base class in PCL for storing collections of 3D points. ...
An exception that is thrown when the number of correspondants is not equal to the minimum required...
Definition: exceptions.h:65
double rotation_epsilon_
The epsilon constant for rotation error.
Definition: gicp.h:272
BFGSSpace::Status testGradient(Scalar epsilon)
Definition: bfgs.h:412
An exception that is thrown when the non linear solver didn&#39;t converge.
Definition: exceptions.h:50
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
Definition: gicp.h:93
MatricesVectorPtr input_covariances_
Input cloud points covariances.
Definition: gicp.h:291
bool converged_
Holds internal convergence state, given user parameters.
Definition: registration.h:536
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition: registration.h:516
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
Definition: gicp.hpp:135
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:527
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
iterator begin()
Definition: point_cloud.h:442
const Eigen::Matrix3d & mahalanobis(size_t index) const
Definition: gicp.h:200
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: gicp.h:104
A point structure representing Euclidean xyz coordinates, and the RGB color.
BFGSSpace::Status minimizeOneStep(FVectorType &x)
Definition: bfgs.h:332
BFGSSpace::Status minimizeInit(FVectorType &x)
Definition: bfgs.h:305
PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: gicp.h:84
MatricesVectorPtr target_covariances_
Target cloud points covariances.
Definition: gicp.h:294
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear...
Definition: bfgs.h:114
size_t size() const
Definition: point_cloud.h:448
VectorType::const_iterator const_iterator
Definition: point_cloud.h:441