17 #ifndef NDT__NDT_VOXEL_HPP_
18 #define NDT__NDT_VOXEL_HPP_
21 #include <experimental/optional>
31 namespace localization
49 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 using Cov = Eigen::Matrix3d;
56 static constexpr uint32_t NUM_POINT_THRESHOLD = 3U;
62 void add_observation(
const Point & pt);
70 bool8_t usable() const noexcept;
76 const
Cov & covariance() const;
81 std::experimental::optional<
Cov> inverse_covariance() const;
85 const
Point & centroid() const;
89 uint64_t count() const noexcept;
96 uint64_t m_num_points{0U};
105 using Cov = Eigen::Matrix3d;
116 Cov covariance()
const;
119 const Point & centroid()
const;
123 const Cov & inverse_covariance()
const;
127 bool8_t usable() const noexcept;
131 Cov m_inv_covariance;
138 #endif // NDT__NDT_VOXEL_HPP_