43#include <pcl/registration/bfgs.h>
44#include <pcl/registration/icp.h>
57template <
typename Po
intSource,
typename Po
intTarget>
92 std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>;
99 using Ptr = shared_ptr<GeneralizedIterativeClosestPoint<PointSource, PointTarget>>;
101 shared_ptr<const GeneralizedIterativeClosestPoint<PointSource, PointTarget>>;
116 reg_name_ =
"GeneralizedIterativeClosestPoint";
124 Eigen::Matrix4f& transformation_matrix) {
126 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
137 if (cloud->points.empty()) {
139 "[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
145 for (std::size_t i = 0; i < input.
size(); ++i)
146 input[i].data[3] = 1.0;
200 Eigen::Matrix4f& transformation_matrix);
203 inline const Eigen::Matrix3d&
370 template <
typename Po
intT>
384 std::size_t n = mat1.rows();
386 for (std::size_t i = 0; i < n; i++)
387 for (std::size_t j = 0; j < n; j++)
388 r += mat1(j, i) * mat2(i, j);
399 const Eigen::Matrix4f& guess)
override;
409 std::vector<float>& distance)
411 int k =
tree_->nearestKSearch(query, 1, index, distance);
442 Eigen::Matrix4f& transformation_matrix)>
447#include <pcl/registration/impl/gicp.hpp>
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
typename PointCloudSource::Ptr PointCloudSourcePtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
typename Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
PointIndices::Ptr PointIndicesPtr
const pcl::Indices * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
void setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
int max_inner_iterations_
maximum number of optimizations
GeneralizedIterativeClosestPoint()
Empty constructor.
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
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.
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input dataset.
double getTranslationGradientTolerance() const
Return the minimal translation gradient threshold for early optimization stop.
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
int k_correspondences_
The number of neighbors used for covariances computation.
bool searchForNeighbors(const PointSource &query, pcl::Indices &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
PointIndices::ConstPtr PointIndicesConstPtr
const pcl::Indices * tmp_idx_src_
Temporary pointer to the source dataset indices.
void setMaximumOptimizerIterations(int max)
Set maximum number of iterations at the optimization step.
int getCorrespondenceRandomness() const
Get the number of neighbors used when computing covariances as set by the user.
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...
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerance default: 0....
double rotation_epsilon_
The epsilon constant for rotation error.
double rotation_gradient_tolerance_
minimal rotation gradient for early optimization stop
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
shared_ptr< MatricesVector > MatricesVectorPtr
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
MatricesVectorPtr target_covariances_
Target cloud points covariances.
shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget > > ConstPtr
pcl::PointCloud< PointTarget > PointCloudTarget
void setRotationGradientTolerance(double tolerance)
Set the minimal rotation gradient threshold for early optimization stop.
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
void setTranslationGradientTolerance(double tolerance)
Set the minimal translation gradient threshold for early optimization stop.
pcl::PointCloud< PointSource > PointCloudSource
double getRotationEpsilon() const
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
shared_ptr< const MatricesVector > MatricesVectorConstPtr
void setSourceCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input source (if computed externally!).
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
MatricesVectorPtr input_covariances_
Input cloud points covariances.
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.
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Eigen::Matrix4f &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget > > Ptr
double getRotationGradientTolerance() const
Return the minimal rotation gradient threshold for early optimization stop.
std::function< void(const pcl::PointCloud< PointSource > &cloud_src, const pcl::Indices &src_indices, const pcl::PointCloud< PointTarget > &cloud_tgt, const pcl::Indices &tgt_indices, Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_
Eigen::Matrix< double, 6, 1 > Vector6d
typename Registration< PointSource, PointTarget >::KdTree InputKdTree
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
Eigen::Matrix4f base_transformation_
base transformation
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
int getMaximumOptimizerIterations() const
Return maximum number of iterations at the optimization step.
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
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)
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)
PointCloudConstPtr input_
The input point cloud dataset.
IndicesPtr indices_
A pointer to the vector of point indices to use.
shared_ptr< PointCloud< PointSource > > Ptr
shared_ptr< const PointCloud< PointSource > > ConstPtr
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
std::function< UpdateVisualizerCallbackSignature > update_visualizer_
Callback function to update intermediate source point cloud position during it's registration to the ...
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
std::string reg_name_
The registration method name.
Matrix4 transformation_
The transformation matrix estimated by the registration method.
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.
typename KdTree::Ptr KdTreePtr
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
KdTreePtr tree_
A pointer to the spatial search object.
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
bool converged_
Holds internal convergence state, given user parameters.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
double inlier_threshold_
The inlier distance threshold for the internal RANSAC outlier rejection loop.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
PointCloudTargetConstPtr target_
The input point cloud dataset target.
const std::string & getClassName() const
Abstract class get name method.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
shared_ptr< KdTree< PointT, Tree > > Ptr
IndicesAllocator<> Indices
Type used for indices in PCL.
optimization functor structure
BFGSSpace::Status checkGradient(const Vector6d &g) override
void df(const Vector6d &x, Vector6d &df) override
double operator()(const Vector6d &x) override
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
const GeneralizedIterativeClosestPoint * gicp_
void fdf(const Vector6d &x, double &f, Vector6d &df) override
shared_ptr< ::pcl::PointIndices > Ptr
shared_ptr< const ::pcl::PointIndices > ConstPtr