44 #include <pcl/registration/icp.h>
45 #include <pcl/registration/bfgs.h>
59 template <
typename Po
intSource,
typename Po
intTarget>
96 typedef boost::shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> >
Ptr;
97 typedef boost::shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> >
ConstPtr;
113 reg_name_ =
"GeneralizedIterativeClosestPoint";
119 this, _1, _2, _3, _4, _5);
134 if (cloud->points.empty ())
136 PCL_ERROR (
"[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
getClassName ().c_str ());
141 for (
size_t i = 0; i < input.
size (); ++i)
142 input[i].data[3] = 1.0;
170 const std::vector<int> &indices_src,
172 const std::vector<int> &indices_tgt,
173 Eigen::Matrix4f &transformation_matrix);
284 template<
typename Po
intT>
287 std::vector<Eigen::Matrix3d>& cloud_covariances);
297 size_t n = mat1.rows();
299 for(
size_t i = 0; i < n; i++)
300 for(
size_t j = 0; j < n; j++)
301 r += mat1 (j, i) * mat2 (i,j);
320 int k =
tree_->nearestKSearch (query, 1, index, distance);
341 boost::function<void(const pcl::PointCloud<PointSource> &cloud_src,
342 const std::vector<int> &src_indices,
344 const std::vector<int> &tgt_indices,
349 #include <pcl/registration/impl/gicp.hpp>
351 #endif //#ifndef PCL_GICP_H_
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
Eigen::Matrix< double, 6, 1 > Vector6d
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target) ...
pcl::PointCloud< PointTarget > PointCloudTarget
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
void setInputTarget(const PointCloudTargetConstPtr &target)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
const std::string & getClassName() const
Abstract class get name method.
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerence default: 0...
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
boost::shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget > > Ptr
double rotation_epsilon_
The epsilon constant for rotation error.
int getMaximumOptimizerIterations()
int max_inner_iterations_
maximum number of optimizations
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
Eigen::Matrix4f base_transformation_
base transformation
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
PointCloudSource::Ptr PointCloudSourcePtr
optimization functor structure
boost::shared_ptr< ::pcl::PointIndices > Ptr
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input dataset.
virtual void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) ...
std::string reg_name_
The registration method name.
pcl::PointCloud< PointSource > PointCloudSource
void fdf(const Vector6d &x, double &f, Vector6d &df)
std::vector< Eigen::Matrix3d > target_covariances_
Target cloud points covariances.
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method with initial guess.
double operator()(const Vector6d &x)
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
boost::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_
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
double getRotationEpsilon()
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
PointIndices::Ptr PointIndicesPtr
boost::shared_ptr< PointCloud< PointT > > Ptr
boost::shared_ptr< KdTree< PointT > > Ptr
PCL_DEPRECATED(void setInputCloud(const PointCloudSourceConstPtr &cloud),"[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.")
Provide a pointer to the input dataset.
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
boost::shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget > > ConstPtr
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...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
PointCloudTarget::Ptr PointCloudTargetPtr
const Eigen::Matrix3d & mahalanobis(size_t index) const
KdTreePtr tree_
A pointer to the spatial search object.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
int getCorrespondenceRandomness()
Get the number of neighbors used when computing covariances as set by the user.
PointCloudSource::ConstPtr PointCloudSourceConstPtr
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
int k_correspondences_
The number of neighbors used for covariances computation.
Registration represents the base registration class for general purpose, ICP-like methods...
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
const GeneralizedIterativeClosestPoint * gicp_
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
void setMaximumOptimizerIterations(int max)
set maximum number of iterations at the optimization step
std::vector< Eigen::Matrix3d > input_covariances_
Input cloud points covariances.
PointCloudConstPtr input_
The input point cloud dataset.
Registration< PointSource, PointTarget >::KdTree InputKdTree
PointIndices::ConstPtr PointIndicesConstPtr
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
void df(const Vector6d &x, Vector6d &df)
void computeCovariances(typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, std::vector< Eigen::Matrix3d > &cloud_covariances)
compute points covariances matrices according to the K nearest neighbors.
GeneralizedIterativeClosestPoint()
Empty constructor.
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...