43 #include <pcl/registration/icp.h>
44 #include <pcl/registration/bfgs.h>
58 template <
typename Po
intSource,
typename Po
intTarget>
92 using MatricesVector = std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >;
99 using Ptr = shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> >;
100 using ConstPtr = shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> >;
116 reg_name_ =
"GeneralizedIterativeClosestPoint";
121 const std::vector<int>& indices_src,
123 const std::vector<int>& indices_tgt,
124 Eigen::Matrix4f& transformation_matrix)
137 if (cloud->points.empty ())
139 PCL_ERROR (
"[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
getClassName ().c_str ());
144 for (std::size_t i = 0; i < input.
size (); ++i)
145 input[i].data[3] = 1.0;
193 const std::vector<int> &indices_src,
195 const std::vector<int> &indices_tgt,
196 Eigen::Matrix4f &transformation_matrix);
199 inline const Eigen::Matrix3d&
mahalanobis(std::size_t index)
const
335 template<
typename Po
intT>
348 std::size_t n = mat1.rows();
350 for(std::size_t i = 0; i < n; i++)
351 for(std::size_t j = 0; j < n; j++)
352 r += mat1 (j, i) * mat2 (i,j);
371 int k =
tree_->nearestKSearch (query, 1, index, distance);
393 std::function<void(const pcl::PointCloud<PointSource> &cloud_src,
394 const std::vector<int> &src_indices,
396 const std::vector<int> &tgt_indices,
401 #include <pcl/registration/impl/gicp.hpp>
Eigen::Matrix4f base_transformation_
base transformation
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
shared_ptr< KdTree< PointT, Tree > > Ptr
void setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
void setRotationGradientTolerance(double tolerance)
Set the minimal rotation gradient threshold for early optimization stop.
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...
shared_ptr< PointCloud< PointSource > > Ptr
GeneralizedIterativeClosestPoint()
Empty constructor.
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
void setMaximumOptimizerIterations(int max)
Set maximum number of iterations at the optimization step.
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_
void df(const Vector6d &x, Vector6d &df) override
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
int max_inner_iterations_
maximum number of optimizations
const GeneralizedIterativeClosestPoint * gicp_
void setTranslationGradientTolerance(double tolerance)
Set the minimal translation gradient threshold for early optimization stop.
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
void fdf(const Vector6d &x, double &f, Vector6d &df) override
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
BFGSSpace::Status checkGradient(const Vector6d &g) override
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.
double getRotationEpsilon() const
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Eigen::Matrix< double, 6, 1 > Vector6d
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
int getMaximumOptimizerIterations() const
Return maximum number of iterations at the optimization step.
void setSourceCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input source (if computed externally!).
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
shared_ptr< MatricesVector > MatricesVectorPtr
shared_ptr< ::pcl::PointIndices > Ptr
KdTreePtr tree_
A pointer to the spatial search object.
const std::string & getClassName() const
Abstract class get name method.
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
int k_correspondences_
The number of neighbors used for covariances computation.
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) ...
typename PointCloudTarget::Ptr PointCloudTargetPtr
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
typename PointCloudSource::Ptr PointCloudSourcePtr
pcl::PointCloud< PointSource > PointCloudSource
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerance default: 0...
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input dataset.
Registration represents the base registration class for general purpose, ICP-like methods...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget > > Ptr
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target) ...
double rotation_epsilon_
The epsilon constant for rotation error.
pcl::PointCloud< PointTarget > PointCloudTarget
PointIndices::Ptr PointIndicesPtr
shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget > > ConstPtr
MatricesVectorPtr input_covariances_
Input cloud points covariances.
double rotation_gradient_tolerance_
minimal rotation gradient for early optimization stop
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
shared_ptr< const ::pcl::PointIndices > ConstPtr
shared_ptr< const MatricesVector > MatricesVectorConstPtr
shared_ptr< const PointCloud< PointSource > > ConstPtr
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
std::string reg_name_
The registration method name.
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
PointIndices::ConstPtr PointIndicesConstPtr
double getRotationGradientTolerance() const
Return the minimal rotation gradient threshold for early optimization stop.
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
double operator()(const Vector6d &x) override
void setInputTarget(const PointCloudTargetConstPtr &target) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
typename Registration< PointSource, PointTarget >::KdTree InputKdTree
double getTranslationGradientTolerance() const
Return the minimal translation gradient threshold for early optimization stop.
int getCorrespondenceRandomness() const
Get the number of neighbors used when computing covariances as set by the user.
typename Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
optimization functor structure
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
MatricesVectorPtr target_covariances_
Target cloud points covariances.