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);
125 PCL_DEPRECATED (
void setInputCloud (
const PointCloudSourceConstPtr &cloud),
"[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.");
134 if (cloud->points.empty ())
136 PCL_ERROR (
"[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
getClassName ().c_str ());
139 PointCloudSource input = *cloud;
141 for (
size_t i = 0; i < input.
size (); ++i)
142 input[i].data[3] = 1.0;
170 const std::vector<int> &indices_src,
171 const PointCloudTarget &cloud_tgt,
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);
327 void applyState(Eigen::Matrix4f &t,
const Vector6d& x)
const;
335 void df(
const Vector6d &x, Vector6d &
df);
336 void fdf(
const Vector6d &x,
double &f, Vector6d &
df);
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_
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
int k_correspondences_
The number of neighbors used for covariances computation.
boost::shared_ptr< KdTree< PointT > > Ptr
boost::shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget > > Ptr
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Registration represents the base registration class for general purpose, ICP-like methods...
int getMaximumOptimizerIterations()
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
Eigen::Matrix4f base_transformation_
base transformation
Registration< PointSource, PointTarget >::KdTree InputKdTree
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
PointCloudSource::Ptr PointCloudSourcePtr
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) ...
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input dataset.
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.
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
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...
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
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...
const std::string & getClassName() const
Abstract class get name method.
optimization functor structure
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
std::vector< Eigen::Matrix3d > target_covariances_
Target cloud points covariances.
double operator()(const Vector6d &x)
PointCloudConstPtr input_
The input point cloud dataset.
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
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_
Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
double rotation_epsilon_
The epsilon constant for rotation error.
int max_inner_iterations_
maximum number of optimizations
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
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
boost::shared_ptr< const PointCloud< PointSource > > ConstPtr
void df(const Vector6d &x, Vector6d &df)
PointCloudTarget::Ptr PointCloudTargetPtr
const Eigen::Matrix3d & mahalanobis(size_t index) const
boost::shared_ptr< ::pcl::PointIndices > Ptr
PointCloudSource::ConstPtr PointCloudSourceConstPtr
pcl::PointCloud< PointSource > PointCloudSource
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
KdTreePtr tree_
A pointer to the spatial search object.
void setMaximumOptimizerIterations(int max)
set maximum number of iterations at the optimization step
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method with initial guess.
std::vector< Eigen::Matrix3d > input_covariances_
Input cloud points covariances.
std::string reg_name_
The registration method name.
double getRotationEpsilon()
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
PointIndices::Ptr PointIndicesPtr
PointIndices::ConstPtr PointIndicesConstPtr
GeneralizedIterativeClosestPoint()
Empty constructor.
const GeneralizedIterativeClosestPoint * gicp_
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.
boost::shared_ptr< PointCloud< PointSource > > Ptr
Eigen::Matrix< double, 6, 1 > Vector6d
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< ::pcl::PointIndices const > ConstPtr
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) ...
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
void fdf(const Vector6d &x, double &f, Vector6d &df)
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
int getCorrespondenceRandomness()
Get the number of neighbors used when computing covariances as set by the user.