123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687 |
- #ifndef FAST_GICP_FAST_GICP_MP_HPP
- #define FAST_GICP_FAST_GICP_MP_HPP
- #include <Eigen/Core>
- #include <Eigen/Geometry>
- #include <pcl/point_types.h>
- #include <pcl/point_cloud.h>
- #include <pcl/search/kdtree.h>
- #include <pcl/registration/registration.h>
- #include <fast_gicp/gicp/gicp_settings.hpp>
- namespace fast_gicp {
- template<typename PointSource, typename PointTarget>
- class FastGICPMultiPoints : public pcl::Registration<PointSource, PointTarget, float> {
- 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;
- 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>::nr_iterations_;
- using pcl::Registration<PointSource, PointTarget, Scalar>::max_iterations_;
- using pcl::Registration<PointSource, PointTarget, Scalar>::final_transformation_;
- using pcl::Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
- using pcl::Registration<PointSource, PointTarget, Scalar>::converged_;
- using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
- FastGICPMultiPoints();
- virtual ~FastGICPMultiPoints() override;
- void setNumThreads(int n);
- void setRotationEpsilon(double eps);
- void setCorrespondenceRandomness(int k);
- void setRegularizationMethod(RegularizationMethod method);
- virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
- virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
- protected:
- virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
- private:
- bool is_converged(const Eigen::Matrix<float, 6, 1>& delta) const;
- void update_correspondences(const Eigen::Matrix<float, 6, 1>& x);
- Eigen::VectorXf loss_ls(const Eigen::Matrix<float, 6, 1>& x, Eigen::MatrixXf* J) const;
- template<typename PointT>
- 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);
- private:
- int num_threads_;
- int k_correspondences_;
- double rotation_epsilon_;
- double neighbor_search_radius_;
- RegularizationMethod regularization_method_;
- pcl::search::KdTree<PointSource> source_kdtree;
- pcl::search::KdTree<PointTarget> target_kdtree;
- std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>> source_covs;
- std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>> target_covs;
- std::vector<std::vector<int>> correspondences;
- std::vector<std::vector<float>> sq_distances;
- };
- } // namespace fast_gicp
- #endif
|