43 #include <pcl/registration/bfgs.h>
44 #include <pcl/registration/icp.h>
58 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar =
float>
72 previous_transformation_;
76 transformation_epsilon_;
81 min_number_correspondences_;
96 std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>;
105 shared_ptr<GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
109 using Vector3 =
typename Eigen::Matrix<Scalar, 3, 1>;
110 using Vector4 =
typename Eigen::Matrix<Scalar, 4, 1>;
112 using Matrix3 =
typename Eigen::Matrix<Scalar, 3, 3>;
124 reg_name_ =
"GeneralizedIterativeClosestPoint";
132 Matrix4& transformation_matrix) {
134 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
145 if (cloud->points.empty()) {
147 "[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
153 for (std::size_t i = 0; i < input.
size(); ++i)
154 input[i].data[3] = 1.0;
210 Matrix4& transformation_matrix);
227 Matrix4& transformation_matrix);
230 inline const Eigen::Matrix3d&
247 const Eigen::Matrix3d& dCost_dR_T,
300 Matrix4& transformation_matrix) {
302 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
415 template <
typename Po
intT>
428 if (mat1.cols() != mat2.rows()) {
430 "The two matrices' shapes don't match. "
432 << mat1.rows() <<
", " << mat1.cols() <<
") and ("
433 << mat2.rows() <<
", " << mat2.cols() <<
")");
435 return (mat1 * mat2).trace();
454 std::vector<float>& distance)
456 int k =
tree_->nearestKSearch(query, 1, index, distance);
485 std::function<void(const pcl::PointCloud<PointSource>& cloud_src,
489 Matrix4& transformation_matrix)>
494 getRDerivatives(
double phi,
497 Eigen::Matrix3d& dR_dPhi,
498 Eigen::Matrix3d& dR_dTheta,
499 Eigen::Matrix3d& dR_dPsi)
const;
502 getR2ndDerivatives(
double phi,
505 Eigen::Matrix3d& ddR_dPhi_dPhi,
506 Eigen::Matrix3d& ddR_dPhi_dTheta,
507 Eigen::Matrix3d& ddR_dPhi_dPsi,
508 Eigen::Matrix3d& ddR_dTheta_dTheta,
509 Eigen::Matrix3d& ddR_dTheta_dPsi,
510 Eigen::Matrix3d& ddR_dPsi_dPsi)
const;
514 #include <pcl/registration/impl/gicp.hpp>
shared_ptr< KdTree< PointT, Tree > > Ptr
double rotation_epsilon_
The epsilon constant for rotation error.
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Matrix4 &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d >> MatricesVector
shared_ptr< PointCloud< PointSource > > Ptr
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...
Matrix4 base_transformation_
base transformation
typename Registration< PointXYZRGBA, PointXYZRGBA, float >::KdTree InputKdTree
shared_ptr< GeneralizedIterativeClosestPoint< PointXYZRGBA, PointXYZRGBA, float >> Ptr
double rotation_gradient_tolerance_
minimal rotation gradient for early optimization stop
typename IterativeClosestPoint< PointXYZRGBA, PointXYZRGBA, float >::Matrix4 Matrix4
A base class for all pcl exceptions which inherits from std::runtime_error.
void setSourceCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input source (if computed externally!).
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
PointIndices::Ptr PointIndicesPtr
pcl::PointCloud< PointSource > PointCloudSource
double getTranslationGradientTolerance() const
Return the minimal translation gradient threshold for early optimization stop.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
typename Registration< PointXYZRGBA, PointXYZRGBA, float >::KdTreePtr InputKdTreePtr
void df(const Vector6d &x, Vector6d &df) override
int k_correspondences_
The number of neighbors used for covariances computation.
PCL_MAKE_ALIGNED_OPERATOR_NEW GeneralizedIterativeClosestPoint()
Empty constructor.
double operator()(const Vector6d &x) override
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
void fdf(const Vector6d &x, double &f, Vector6d &df) override
shared_ptr< const MatricesVector > MatricesVectorConstPtr
unsigned int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
const GeneralizedIterativeClosestPoint * gicp_
void dfddf(const Vector6d &x, Vector6d &df, Matrix6d &ddf)
typename Eigen::Matrix< float, 3, 3 > Matrix3
void applyState(Matrix4 &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
MatricesVectorPtr target_covariances_
Target cloud points covariances.
shared_ptr< ::pcl::PointIndices > Ptr
KdTreePtr tree_
A pointer to the spatial search object.
const std::string & getClassName() const
Abstract class get name method.
typename Eigen::Matrix< float, 3, 1 > Vector3
typename PointCloudSource::Ptr PointCloudSourcePtr
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
void useBFGS()
Use BFGS optimizer instead of default Newton optimizer.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
const pcl::Indices * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
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 setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
Eigen::Matrix< double, 6, 6 > Matrix6d
typename Eigen::AngleAxis< float > AngleAxis
typename Eigen::Matrix< float, 4, 1 > Vector4
pcl::PointCloud< PointTarget > PointCloudTarget
int getMaximumOptimizerIterations() const
Return maximum number of iterations at the optimization step.
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
const pcl::Indices * tmp_idx_src_
Temporary pointer to the source dataset indices.
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
IndicesAllocator<> Indices
Type used for indices in PCL.
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 Eigen::Matrix3d & mahalanobis(std::size_t index) const
Eigen::Matrix< double, 6, 1 > Vector6d
void estimateRigidTransformationNewton(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Matrix4 &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
void setMaximumOptimizerIterations(int max)
Set maximum number of iterations at the optimization step.
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
typename KdTree::Ptr KdTreePtr
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input dataset.
MatricesVectorPtr input_covariances_
Input cloud points covariances.
shared_ptr< const GeneralizedIterativeClosestPoint< PointXYZRGBA, PointXYZRGBA, float >> ConstPtr
shared_ptr< MatricesVector > MatricesVectorPtr
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
shared_ptr< const ::pcl::PointIndices > ConstPtr
shared_ptr< const PointCloud< PointSource > > ConstPtr
std::string reg_name_
The registration method name.
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
bool searchForNeighbors(const PointSource &query, pcl::Indices &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
void setTranslationGradientTolerance(double tolerance)
Set the minimal translation gradient threshold for early optimization stop.
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerance default: 0...
int max_inner_iterations_
maximum number of optimizations
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
BFGSSpace::Status checkGradient(const Vector6d &g) override
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &dCost_dR_T, Vector6d &g) const
Computes the derivative of the cost function w.r.t rotation angles.
optimization functor structure
std::function< void(const pcl::PointCloud< PointSource > &cloud_src, const pcl::Indices &src_indices, const pcl::PointCloud< PointTarget > &cloud_tgt, const pcl::Indices &tgt_indices, Matrix4 &transformation_matrix)> rigid_transformation_estimation_
int getCorrespondenceRandomness() const
Get the number of neighbors used when computing covariances as set by the user.
PointIndices::ConstPtr PointIndicesConstPtr
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
typename PointCloudTarget::Ptr PointCloudTargetPtr
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
void setRotationGradientTolerance(double tolerance)
Set the minimal rotation gradient threshold for early optimization stop.
double getRotationEpsilon() const
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
double getRotationGradientTolerance() const
Return the minimal rotation gradient threshold for early optimization stop.