fast_gicp.hpp 4.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104
  1. #ifndef FAST_GICP_FAST_GICP_HPP
  2. #define FAST_GICP_FAST_GICP_HPP
  3. #include <Eigen/Core>
  4. #include <Eigen/Geometry>
  5. #include <pcl/point_types.h>
  6. #include <pcl/point_cloud.h>
  7. #include <pcl/search/search.h>
  8. #include <pcl/registration/registration.h>
  9. // #include <fast_gicp/gicp/lsq_registration.hpp>
  10. // #include <fast_gicp/gicp/gicp_settings.hpp>
  11. #include "lsq_registration.hpp"
  12. #include "gicp_settings.hpp"
  13. namespace fast_gicp {
  14. /**
  15. * @brief Fast GICP algorithm optimized for multi threading with OpenMP
  16. */
  17. template<typename PointSource, typename PointTarget, typename SearchMethodSource = pcl::search::KdTree<PointSource>, typename SearchMethodTarget = pcl::search::KdTree<PointTarget>>
  18. class FastGICP : public LsqRegistration<PointSource, PointTarget> {
  19. public:
  20. using Scalar = float;
  21. using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
  22. using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
  23. using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  24. using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
  25. using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
  26. using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  27. using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
  28. #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
  29. using Ptr = pcl::shared_ptr<FastGICP<PointSource, PointTarget>>;
  30. using ConstPtr = pcl::shared_ptr<const FastGICP<PointSource, PointTarget>>;
  31. #else
  32. using Ptr = boost::shared_ptr<FastGICP<PointSource, PointTarget>>;
  33. using ConstPtr = boost::shared_ptr<const FastGICP<PointSource, PointTarget>>;
  34. #endif
  35. protected:
  36. using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
  37. using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
  38. using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
  39. using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
  40. public:
  41. FastGICP();
  42. virtual ~FastGICP() override;
  43. void setNumThreads(int n);
  44. void setCorrespondenceRandomness(int k);
  45. void setRegularizationMethod(RegularizationMethod method);
  46. virtual void swapSourceAndTarget() override;
  47. virtual void clearSource() override;
  48. virtual void clearTarget() override;
  49. virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
  50. virtual void setSourceCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
  51. virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
  52. virtual void setTargetCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
  53. const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getSourceCovariances() const {
  54. return source_covs_;
  55. }
  56. const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getTargetCovariances() const {
  57. return target_covs_;
  58. }
  59. protected:
  60. virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
  61. virtual void update_correspondences(const Eigen::Isometry3d& trans);
  62. virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) override;
  63. virtual double compute_error(const Eigen::Isometry3d& trans) override;
  64. template<typename PointT>
  65. 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);
  66. protected:
  67. int num_threads_;
  68. int k_correspondences_;
  69. RegularizationMethod regularization_method_;
  70. std::shared_ptr<SearchMethodSource> search_source_;
  71. std::shared_ptr<SearchMethodTarget> search_target_;
  72. std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> source_covs_;
  73. std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> target_covs_;
  74. std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> mahalanobis_;
  75. std::vector<int> correspondences_;
  76. std::vector<float> sq_distances_;
  77. };
  78. } // namespace fast_gicp
  79. #endif