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