44 #include <pcl/registration/icp.h> 45 #include <pcl/registration/bfgs.h> 59 template <
typename Po
intSource,
typename Po
intTarget>
93 typedef std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >
MatricesVector;
100 typedef boost::shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> >
Ptr;
101 typedef boost::shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> >
ConstPtr;
115 reg_name_ =
"GeneralizedIterativeClosestPoint";
121 this, _1, _2, _3, _4, _5);
127 PCL_DEPRECATED (
"[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.")
138 if (cloud->points.empty ())
140 PCL_ERROR (
"[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
getClassName ().c_str ());
143 PointCloudSource input = *cloud;
145 for (
size_t i = 0; i < input.
size (); ++i)
146 input[i].data[3] = 1.0;
194 const std::vector<int> &indices_src,
195 const PointCloudTarget &cloud_tgt,
196 const std::vector<int> &indices_tgt,
197 Eigen::Matrix4f &transformation_matrix);
308 template<
typename Po
intT>
311 MatricesVector& cloud_covariances);
321 size_t n = mat1.rows();
323 for(
size_t i = 0; i < n; i++)
324 for(
size_t j = 0; j < n; j++)
325 r += mat1 (j, i) * mat2 (i,j);
344 int k =
tree_->nearestKSearch (query, 1, index, distance);
351 void applyState(Eigen::Matrix4f &t,
const Vector6d& x)
const;
359 void df(
const Vector6d &x, Vector6d &
df);
360 void fdf(
const Vector6d &x,
double &f, Vector6d &df);
365 boost::function<void(const pcl::PointCloud<PointSource> &cloud_src,
366 const std::vector<int> &src_indices,
368 const std::vector<int> &tgt_indices,
373 #include <pcl/registration/impl/gicp.hpp> 375 #endif //#ifndef PCL_GICP_H_ Eigen::Matrix4f base_transformation_
base transformation
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
PointIndices::Ptr PointIndicesPtr
void setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
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) ...
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...
pcl::PointCloud< PointSource > PointCloudSource
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_
GeneralizedIterativeClosestPoint()
Empty constructor.
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
int getMaximumOptimizerIterations()
void setMaximumOptimizerIterations(int max)
set maximum number of iterations at the optimization step
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...
Registration< PointSource, PointTarget >::KdTree InputKdTree
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method with initial guess.
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_
double operator()(const Vector6d &x)
void fdf(const Vector6d &x, double &f, Vector6d &df)
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.
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
PointCloudSource::Ptr PointCloudSourcePtr
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.
void df(const Vector6d &x, Vector6d &df)
boost::shared_ptr< PointCloud< PointSource > > Ptr
void setInputCloud(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
KdTreePtr tree_
A pointer to the spatial search object.
const std::string & getClassName() const
Abstract class get name method.
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.
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.
double getRotationEpsilon()
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
pcl::PointCloud< PointTarget > PointCloudTarget
Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerence default: 0...
boost::shared_ptr< const PointCloud< PointSource > > ConstPtr
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
int getCorrespondenceRandomness()
Get the number of neighbors used when computing covariances as set by the user.
Registration represents the base registration class for general purpose, ICP-like methods...
double rotation_epsilon_
The epsilon constant for rotation error.
boost::shared_ptr< MatricesVector > MatricesVectorPtr
boost::shared_ptr< ::pcl::PointIndices > Ptr
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
boost::shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget > > Ptr
MatricesVectorPtr input_covariances_
Input cloud points covariances.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
boost::shared_ptr< const MatricesVector > MatricesVectorConstPtr
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) ...
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
std::string reg_name_
The registration method name.
PointIndices::ConstPtr PointIndicesConstPtr
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
PointCloudTarget::Ptr PointCloudTargetPtr
Eigen::Matrix< double, 6, 1 > Vector6d
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
boost::shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget > > ConstPtr
const Eigen::Matrix3d & mahalanobis(size_t index) const
optimization functor structure
PointCloudSource::ConstPtr PointCloudSourceConstPtr
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.
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr