123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104 |
- #ifndef FAST_GICP_FAST_GICP_HPP
- #define FAST_GICP_FAST_GICP_HPP
- #include <Eigen/Core>
- #include <Eigen/Geometry>
- #include <pcl/point_types.h>
- #include <pcl/point_cloud.h>
- #include <pcl/search/search.h>
- #include <pcl/registration/registration.h>
- // #include <fast_gicp/gicp/lsq_registration.hpp>
- // #include <fast_gicp/gicp/gicp_settings.hpp>
- #include "lsq_registration.hpp"
- #include "gicp_settings.hpp"
- namespace fast_gicp {
- /**
- * @brief Fast GICP algorithm optimized for multi threading with OpenMP
- */
- template<typename PointSource, typename PointTarget, typename SearchMethodSource = pcl::search::KdTree<PointSource>, typename SearchMethodTarget = pcl::search::KdTree<PointTarget>>
- class FastGICP : public LsqRegistration<PointSource, PointTarget> {
- public:
- using Scalar = float;
- using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
- using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
- using PointCloudSourcePtr = typename PointCloudSource::Ptr;
- using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
- using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
- using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
- using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
- #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
- using Ptr = pcl::shared_ptr<FastGICP<PointSource, PointTarget>>;
- using ConstPtr = pcl::shared_ptr<const FastGICP<PointSource, PointTarget>>;
- #else
- using Ptr = boost::shared_ptr<FastGICP<PointSource, PointTarget>>;
- using ConstPtr = boost::shared_ptr<const FastGICP<PointSource, PointTarget>>;
- #endif
- protected:
- using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
- using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
- using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
- using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
- public:
- FastGICP();
- virtual ~FastGICP() override;
- void setNumThreads(int n);
- void setCorrespondenceRandomness(int k);
- void setRegularizationMethod(RegularizationMethod method);
- virtual void swapSourceAndTarget() override;
- virtual void clearSource() override;
- virtual void clearTarget() override;
- virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
- virtual void setSourceCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
- virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
- virtual void setTargetCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
- const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getSourceCovariances() const {
- return source_covs_;
- }
- const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getTargetCovariances() const {
- return target_covs_;
- }
- protected:
- virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
- virtual void update_correspondences(const Eigen::Isometry3d& trans);
- virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) override;
- virtual double compute_error(const Eigen::Isometry3d& trans) override;
- template<typename PointT>
- bool calculate_covariances(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, pcl::search::Search<PointT>& kdtree, std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covariances);
- protected:
- int num_threads_;
- int k_correspondences_;
- RegularizationMethod regularization_method_;
- std::shared_ptr<SearchMethodSource> search_source_;
- std::shared_ptr<SearchMethodTarget> search_target_;
- std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> source_covs_;
- std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> target_covs_;
- std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> mahalanobis_;
- std::vector<int> correspondences_;
- std::vector<float> sq_distances_;
- };
- } // namespace fast_gicp
- #endif
|