Point Cloud Library (PCL)
1.10.1
|
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> >;
114 reg_name_ =
"GeneralizedIterativeClosestPoint";
119 const std::vector<int>& indices_src,
121 const std::vector<int>& indices_tgt,
122 Eigen::Matrix4f& transformation_matrix)
135 if (cloud->points.empty ())
137 PCL_ERROR (
"[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
getClassName ().c_str ());
142 for (std::size_t i = 0; i < input.
size (); ++i)
143 input[i].data[3] = 1.0;
191 const std::vector<int> &indices_src,
193 const std::vector<int> &indices_tgt,
194 Eigen::Matrix4f &transformation_matrix);
197 inline const Eigen::Matrix3d&
mahalanobis(std::size_t index)
const
305 template<
typename Po
intT>
318 std::size_t n = mat1.rows();
320 for(std::size_t i = 0; i < n; i++)
321 for(std::size_t j = 0; j < n; j++)
322 r += mat1 (j, i) * mat2 (i,j);
363 const std::vector<int> &src_indices,
365 const std::vector<int> &tgt_indices,
370 #include <pcl/registration/impl/gicp.hpp>
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
MatricesVectorPtr target_covariances_
Target cloud points covariances.
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input dataset.
This file defines compatibility wrappers for low level I/O functions.
Eigen::Matrix4f base_transformation_
base transformation
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
Registration represents the base registration class for general purpose, ICP-like methods.
float distance(const PointT &p1, const PointT &p2)
Eigen::Matrix< double, 6, 1 > Vector6d
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerance default: 0....
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...
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
typename Registration< PointSource, PointTarget >::KdTree InputKdTree
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
KdTreePtr tree_
A pointer to the spatial search object.
void setSourceCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input source (if computed externally!).
shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget > > ConstPtr
const std::string & getClassName() const
Abstract class get name method.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
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)
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< MatricesVector > MatricesVectorPtr
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...
int max_inner_iterations_
maximum number of optimizations
pcl::PointCloud< PointTarget > PointCloudTarget
PointIndices::Ptr PointIndicesPtr
shared_ptr< KdTree< PointT, Tree > > Ptr
shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget > > Ptr
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
shared_ptr< const ::pcl::PointIndices > ConstPtr
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
optimization functor structure
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)
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
pcl::PointCloud< PointSource > PointCloudSource
double operator()(const Vector6d &x) override
typename Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
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 setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
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_
shared_ptr< ::pcl::PointIndices > Ptr
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
double rotation_epsilon_
The epsilon constant for rotation error.
shared_ptr< PointCloud< PointSource > > Ptr
typename PointCloudTarget::Ptr PointCloudTargetPtr
int getCorrespondenceRandomness()
Get the number of neighbors used when computing covariances as set by the user.
double getRotationEpsilon()
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
PointIndices::ConstPtr PointIndicesConstPtr
shared_ptr< const PointCloud< PointSource > > ConstPtr
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
const GeneralizedIterativeClosestPoint * gicp_
void df(const Vector6d &x, Vector6d &df) override
MatricesVectorPtr input_covariances_
Input cloud points covariances.
int getMaximumOptimizerIterations()
void setMaximumOptimizerIterations(int max)
set maximum number of iterations at the optimization step
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
typename PointCloudSource::Ptr PointCloudSourcePtr
shared_ptr< const MatricesVector > MatricesVectorConstPtr
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
int k_correspondences_
The number of neighbors used for covariances computation.
std::string reg_name_
The registration method name.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
GeneralizedIterativeClosestPoint()
Empty constructor.
void fdf(const Vector6d &x, double &f, Vector6d &df) override