fast_gicp_mp.hpp 3.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687
  1. #ifndef FAST_GICP_FAST_GICP_MP_HPP
  2. #define FAST_GICP_FAST_GICP_MP_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/kdtree.h>
  8. #include <pcl/registration/registration.h>
  9. #include <fast_gicp/gicp/gicp_settings.hpp>
  10. namespace fast_gicp {
  11. template<typename PointSource, typename PointTarget>
  12. class FastGICPMultiPoints : public pcl::Registration<PointSource, PointTarget, float> {
  13. public:
  14. using Scalar = float;
  15. using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
  16. using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
  17. using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  18. using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
  19. using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
  20. using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  21. using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
  22. using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
  23. using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
  24. using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
  25. using pcl::Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
  26. using pcl::Registration<PointSource, PointTarget, Scalar>::max_iterations_;
  27. using pcl::Registration<PointSource, PointTarget, Scalar>::final_transformation_;
  28. using pcl::Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
  29. using pcl::Registration<PointSource, PointTarget, Scalar>::converged_;
  30. using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
  31. FastGICPMultiPoints();
  32. virtual ~FastGICPMultiPoints() override;
  33. void setNumThreads(int n);
  34. void setRotationEpsilon(double eps);
  35. void setCorrespondenceRandomness(int k);
  36. void setRegularizationMethod(RegularizationMethod method);
  37. virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
  38. virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
  39. protected:
  40. virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
  41. private:
  42. bool is_converged(const Eigen::Matrix<float, 6, 1>& delta) const;
  43. void update_correspondences(const Eigen::Matrix<float, 6, 1>& x);
  44. Eigen::VectorXf loss_ls(const Eigen::Matrix<float, 6, 1>& x, Eigen::MatrixXf* J) const;
  45. template<typename PointT>
  46. bool calculate_covariances(const pcl::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);
  47. private:
  48. int num_threads_;
  49. int k_correspondences_;
  50. double rotation_epsilon_;
  51. double neighbor_search_radius_;
  52. RegularizationMethod regularization_method_;
  53. pcl::search::KdTree<PointSource> source_kdtree;
  54. pcl::search::KdTree<PointTarget> target_kdtree;
  55. std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>> source_covs;
  56. std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>> target_covs;
  57. std::vector<std::vector<int>> correspondences;
  58. std::vector<std::vector<float>> sq_distances;
  59. };
  60. } // namespace fast_gicp
  61. #endif