Point Cloud Library (PCL)  1.9.0-dev
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>
48 template<typename PointT> void
50  const typename pcl::search::KdTree<PointT>::Ptr kdtree,
51  MatricesVector& cloud_covariances)
52 {
53  if (k_correspondences_ > int (cloud->size ()))
54  {
55  PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
56  return;
57  }
58 
59  Eigen::Vector3d mean;
60  std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
61  std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
62 
63  // We should never get there but who knows
64  if(cloud_covariances.size () < cloud->size ())
65  cloud_covariances.resize (cloud->size ());
66 
67  typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
68  MatricesVector::iterator matrices_iterator = cloud_covariances.begin ();
69  for(;
70  points_iterator != cloud->end ();
71  ++points_iterator, ++matrices_iterator)
72  {
73  const PointT &query_point = *points_iterator;
74  Eigen::Matrix3d &cov = *matrices_iterator;
75  // Zero out the cov and mean
76  cov.setZero ();
77  mean.setZero ();
78 
79  // Search for the K nearest neighbours
80  kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
81 
82  // Find the covariance matrix
83  for(int j = 0; j < k_correspondences_; j++) {
84  const PointT &pt = (*cloud)[nn_indecies[j]];
85 
86  mean[0] += pt.x;
87  mean[1] += pt.y;
88  mean[2] += pt.z;
89 
90  cov(0,0) += pt.x*pt.x;
91 
92  cov(1,0) += pt.y*pt.x;
93  cov(1,1) += pt.y*pt.y;
94 
95  cov(2,0) += pt.z*pt.x;
96  cov(2,1) += pt.z*pt.y;
97  cov(2,2) += pt.z*pt.z;
98  }
99 
100  mean /= static_cast<double> (k_correspondences_);
101  // Get the actual covariance
102  for (int k = 0; k < 3; k++)
103  for (int l = 0; l <= k; l++)
104  {
105  cov(k,l) /= static_cast<double> (k_correspondences_);
106  cov(k,l) -= mean[k]*mean[l];
107  cov(l,k) = cov(k,l);
108  }
109 
110  // Compute the SVD (covariance matrix is symmetric so U = V')
111  Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
112  cov.setZero ();
113  Eigen::Matrix3d U = svd.matrixU ();
114  // Reconstitute the covariance matrix with modified singular values using the column // vectors in V.
115  for(int k = 0; k < 3; k++) {
116  Eigen::Vector3d col = U.col(k);
117  double v = 1.; // biggest 2 singular values replaced by 1
118  if(k == 2) // smallest singular value replaced by gicp_epsilon
119  v = gicp_epsilon_;
120  cov+= v * col * col.transpose();
121  }
122  }
123 }
124 
125 ////////////////////////////////////////////////////////////////////////////////////////
126 template <typename PointSource, typename PointTarget> void
128 {
129  Eigen::Matrix3d dR_dPhi;
130  Eigen::Matrix3d dR_dTheta;
131  Eigen::Matrix3d dR_dPsi;
132 
133  double phi = x[3], theta = x[4], psi = x[5];
134 
135  double cphi = cos(phi), sphi = sin(phi);
136  double ctheta = cos(theta), stheta = sin(theta);
137  double cpsi = cos(psi), spsi = sin(psi);
138 
139  dR_dPhi(0,0) = 0.;
140  dR_dPhi(1,0) = 0.;
141  dR_dPhi(2,0) = 0.;
142 
143  dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
144  dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
145  dR_dPhi(2,1) = cphi*ctheta;
146 
147  dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
148  dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
149  dR_dPhi(2,2) = -ctheta*sphi;
150 
151  dR_dTheta(0,0) = -cpsi*stheta;
152  dR_dTheta(1,0) = -spsi*stheta;
153  dR_dTheta(2,0) = -ctheta;
154 
155  dR_dTheta(0,1) = cpsi*ctheta*sphi;
156  dR_dTheta(1,1) = ctheta*sphi*spsi;
157  dR_dTheta(2,1) = -sphi*stheta;
158 
159  dR_dTheta(0,2) = cphi*cpsi*ctheta;
160  dR_dTheta(1,2) = cphi*ctheta*spsi;
161  dR_dTheta(2,2) = -cphi*stheta;
162 
163  dR_dPsi(0,0) = -ctheta*spsi;
164  dR_dPsi(1,0) = cpsi*ctheta;
165  dR_dPsi(2,0) = 0.;
166 
167  dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
168  dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
169  dR_dPsi(2,1) = 0.;
170 
171  dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
172  dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
173  dR_dPsi(2,2) = 0.;
174 
175  g[3] = matricesInnerProd(dR_dPhi, R);
176  g[4] = matricesInnerProd(dR_dTheta, R);
177  g[5] = matricesInnerProd(dR_dPsi, R);
178 }
179 
180 ////////////////////////////////////////////////////////////////////////////////////////
181 template <typename PointSource, typename PointTarget> void
183  const std::vector<int> &indices_src,
184  const PointCloudTarget &cloud_tgt,
185  const std::vector<int> &indices_tgt,
186  Eigen::Matrix4f &transformation_matrix)
187 {
188  if (indices_src.size () < 4) // need at least 4 samples
189  {
190  PCL_THROW_EXCEPTION (NotEnoughPointsException,
191  "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!");
192  return;
193  }
194  // Set the initial solution
195  Vector6d x = Vector6d::Zero ();
196  x[0] = transformation_matrix (0,3);
197  x[1] = transformation_matrix (1,3);
198  x[2] = transformation_matrix (2,3);
199  x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
200  x[4] = asin (-transformation_matrix (2,0));
201  x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
202 
203  // Set temporary pointers
204  tmp_src_ = &cloud_src;
205  tmp_tgt_ = &cloud_tgt;
206  tmp_idx_src_ = &indices_src;
207  tmp_idx_tgt_ = &indices_tgt;
208 
209  // Optimize using forward-difference approximation LM
210  const double gradient_tol = 1e-2;
211  OptimizationFunctorWithIndices functor(this);
213  bfgs.parameters.sigma = 0.01;
214  bfgs.parameters.rho = 0.01;
215  bfgs.parameters.tau1 = 9;
216  bfgs.parameters.tau2 = 0.05;
217  bfgs.parameters.tau3 = 0.5;
218  bfgs.parameters.order = 3;
219 
220  int inner_iterations_ = 0;
221  int result = bfgs.minimizeInit (x);
222  result = BFGSSpace::Running;
223  do
224  {
225  inner_iterations_++;
226  result = bfgs.minimizeOneStep (x);
227  if(result)
228  {
229  break;
230  }
231  result = bfgs.testGradient(gradient_tol);
232  } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
233  if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
234  {
235  PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
236  PCL_DEBUG ("BFGS solver finished with exit code %i \n", result);
237  transformation_matrix.setIdentity();
238  applyState(transformation_matrix, x);
239  }
240  else
241  PCL_THROW_EXCEPTION(SolverDidntConvergeException,
242  "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
243 }
244 
245 ////////////////////////////////////////////////////////////////////////////////////////
246 template <typename PointSource, typename PointTarget> inline double
248 {
249  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
250  gicp_->applyState(transformation_matrix, x);
251  double f = 0;
252  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
253  for (int i = 0; i < m; ++i)
254  {
255  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
256  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
257  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
258  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
259  Eigen::Vector4f pp (transformation_matrix * p_src);
260  // Estimate the distance (cost function)
261  // The last coordinate is still guaranteed to be set to 1.0
262  Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
263  Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
264  //increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes)
265  f+= double(res.transpose() * temp);
266  }
267  return f/m;
268 }
269 
270 ////////////////////////////////////////////////////////////////////////////////////////
271 template <typename PointSource, typename PointTarget> inline void
273 {
274  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
275  gicp_->applyState(transformation_matrix, x);
276  //Zero out g
277  g.setZero ();
278  //Eigen::Vector3d g_t = g.head<3> ();
279  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
280  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
281  for (int i = 0; i < m; ++i)
282  {
283  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
284  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
285  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
286  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
287 
288  Eigen::Vector4f pp (transformation_matrix * p_src);
289  // The last coordinate is still guaranteed to be set to 1.0
290  Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
291  // temp = M*res
292  Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
293  // Increment translation gradient
294  // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
295  g.head<3> ()+= temp;
296  // Increment rotation gradient
297  pp = gicp_->base_transformation_ * p_src;
298  Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
299  R+= p_src3 * temp.transpose();
300  }
301  g.head<3> ()*= 2.0/m;
302  R*= 2.0/m;
303  gicp_->computeRDerivative(x, R, g);
304 }
305 
306 ////////////////////////////////////////////////////////////////////////////////////////
307 template <typename PointSource, typename PointTarget> inline void
309 {
310  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
311  gicp_->applyState(transformation_matrix, x);
312  f = 0;
313  g.setZero ();
314  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
315  const int m = static_cast<const int> (gicp_->tmp_idx_src_->size ());
316  for (int i = 0; i < m; ++i)
317  {
318  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
319  Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
320  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
321  Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
322  Eigen::Vector4f pp (transformation_matrix * p_src);
323  // The last coordinate is still guaranteed to be set to 1.0
324  Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
325  // temp = M*res
326  Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
327  // Increment total error
328  f+= double(res.transpose() * temp);
329  // Increment translation gradient
330  // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
331  g.head<3> ()+= temp;
332  pp = gicp_->base_transformation_ * p_src;
333  Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
334  // Increment rotation gradient
335  R+= p_src3 * temp.transpose();
336  }
337  f/= double(m);
338  g.head<3> ()*= double(2.0/m);
339  R*= 2.0/m;
340  gicp_->computeRDerivative(x, R, g);
341 }
342 
343 ////////////////////////////////////////////////////////////////////////////////////////
344 template <typename PointSource, typename PointTarget> inline void
346 {
348  using namespace std;
349  // Difference between consecutive transforms
350  double delta = 0;
351  // Get the size of the target
352  const size_t N = indices_->size ();
353  // Set the mahalanobis matrices to identity
354  mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
355  // Compute target cloud covariance matrices
356  if ((!target_covariances_) || (target_covariances_->empty ()))
357  {
358  target_covariances_.reset (new MatricesVector);
359  computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
360  }
361  // Compute input cloud covariance matrices
362  if ((!input_covariances_) || (input_covariances_->empty ()))
363  {
365  computeCovariances<PointSource> (input_, tree_reciprocal_, *input_covariances_);
366  }
367 
368  base_transformation_ = Eigen::Matrix4f::Identity();
369  nr_iterations_ = 0;
370  converged_ = false;
371  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
372  std::vector<int> nn_indices (1);
373  std::vector<float> nn_dists (1);
374 
375  pcl::transformPointCloud(output, output, guess);
376 
377  while(!converged_)
378  {
379  size_t cnt = 0;
380  std::vector<int> source_indices (indices_->size ());
381  std::vector<int> target_indices (indices_->size ());
382 
383  // guess corresponds to base_t and transformation_ to t
384  Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
385  for(size_t i = 0; i < 4; i++)
386  for(size_t j = 0; j < 4; j++)
387  for(size_t k = 0; k < 4; k++)
388  transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
389 
390  Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
391 
392  for (size_t i = 0; i < N; i++)
393  {
394  PointSource query = output[i];
395  query.getVector4fMap () = transformation_ * query.getVector4fMap ();
396 
397  if (!searchForNeighbors (query, nn_indices, nn_dists))
398  {
399  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]);
400  return;
401  }
402 
403  // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
404  if (nn_dists[0] < dist_threshold)
405  {
406  Eigen::Matrix3d &C1 = (*input_covariances_)[i];
407  Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
408  Eigen::Matrix3d &M = mahalanobis_[i];
409  // M = R*C1
410  M = R * C1;
411  // temp = M*R' + C2 = R*C1*R' + C2
412  Eigen::Matrix3d temp = M * R.transpose();
413  temp+= C2;
414  // M = temp^-1
415  M = temp.inverse ();
416  source_indices[cnt] = static_cast<int> (i);
417  target_indices[cnt] = nn_indices[0];
418  cnt++;
419  }
420  }
421  // Resize to the actual number of valid correspondences
422  source_indices.resize(cnt); target_indices.resize(cnt);
423  /* optimize transformation using the current assignment and Mahalanobis metrics*/
425  //optimization right here
426  try
427  {
428  rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
429  /* compute the delta from this iteration */
430  delta = 0.;
431  for(int k = 0; k < 4; k++) {
432  for(int l = 0; l < 4; l++) {
433  double ratio = 1;
434  if(k < 3 && l < 3) // rotation part of the transform
435  ratio = 1./rotation_epsilon_;
436  else
437  ratio = 1./transformation_epsilon_;
438  double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
439  if(c_delta > delta)
440  delta = c_delta;
441  }
442  }
443  }
444  catch (PCLException &e)
445  {
446  PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
447  break;
448  }
449  nr_iterations_++;
450  // Check for convergence
451  if (nr_iterations_ >= max_iterations_ || delta < 1)
452  {
453  converged_ = true;
455  PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
456  getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
457  }
458  else
459  PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
460  }
462 
463  // Transform the point cloud
465 }
466 
467 template <typename PointSource, typename PointTarget> void
469 {
470  // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
471  Eigen::Matrix3f R;
472  R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
473  * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
474  * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
475  t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
476  Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
477  t.col (3) += T;
478 }
479 
480 #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:268
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
size_t size() const
Definition: point_cloud.h:448
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:182
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:362
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
Definition: gicp.h:290
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:345
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
Definition: gicp.h:277
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:335
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Definition: registration.h:491
const GeneralizedIterativeClosestPoint * gicp_
Definition: gicp.h:355
iterator end()
Definition: point_cloud.h:443
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:153
void fdf(const Vector6d &x, double &f, Vector6d &df)
Definition: gicp.hpp:308
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:49
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
Definition: gicp.h:271
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Definition: gicp.hpp:468
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
Definition: gicp.h:280
Parameters parameters
Definition: bfgs.h:155
KdTreePtr tree_
A pointer to the spatial search object.
Definition: registration.h:485
const std::string & getClassName() const
Abstract class get name method.
Definition: registration.h:422
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
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
Definition: gicp.hpp:127
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:215
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
Definition: gicp.h:274
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 correspondents is not equal to the minimum required...
Definition: exceptions.h:65
double rotation_epsilon_
The epsilon constant for rotation error.
Definition: gicp.h:265
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:284
bool converged_
Holds internal convergence state, given user parameters.
Definition: registration.h:541
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition: registration.h:516
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:532
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
iterator begin()
Definition: point_cloud.h:442
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
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
const Eigen::Matrix3d & mahalanobis(size_t index) const
Definition: gicp.h:193
BFGSSpace::Status minimizeInit(FVectorType &x)
Definition: bfgs.h:305
MatricesVectorPtr target_covariances_
Target cloud points covariances.
Definition: gicp.h:287
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear...
Definition: bfgs.h:114
VectorType::const_iterator const_iterator
Definition: point_cloud.h:441