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);
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.
const std::string & getClassName() const
Abstract class get name method.
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 matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
void fdf(const Vector6d &x, double &f, Vector6d &df) override
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.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Eigen::Matrix< double, 6, 1 > Vector6d
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
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.
double getRotationEpsilon() const
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
shared_ptr< MatricesVector > MatricesVectorPtr
shared_ptr< ::pcl::PointIndices > Ptr
KdTreePtr tree_
A pointer to the spatial search object.
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.
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
float distance(const PointT &p1, const PointT &p2)
int getMaximumOptimizerIterations() const
Return maximum number of iterations at the optimization step.
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. ...
double getRotationGradientTolerance() const
Return the minimal rotation gradient threshold for early optimization stop.
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
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
int getCorrespondenceRandomness() const
Get the number of neighbors used when computing covariances as set by the user.
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...
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
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
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
typename Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
double getTranslationGradientTolerance() const
Return the minimal translation gradient threshold for early optimization stop.
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.