30 #ifndef MCL_3DL_RAYCASTS_RAYCAST_USING_DDA_H 31 #define MCL_3DL_RAYCASTS_RAYCAST_USING_DDA_H 37 #include <unordered_map> 43 #include <pcl/common/common.h> 44 #include <pcl/point_cloud.h> 50 template <
typename POINT_TYPE>
56 RaycastUsingDDA(
const double map_grid_size_x,
const double map_grid_size_y,
const double map_grid_size_z,
57 const double dda_grid_size,
const double ray_angle_half,
const double hit_tolerance)
83 max_movement_ = std::abs(distance_index[0]) + std::abs(distance_index[1]) + std::abs(distance_index[2]);
84 step_ = Eigen::Vector3i((distance_index[0] < 0) ? -1 : 1, (distance_index[1] < 0) ? -1 : 1,
85 (distance_index[2] < 0) ? -1 : 1);
88 for (
int i = 0; i < 3; ++i)
90 if (distance_index[i] == 0)
93 t_delta_[i] = std::numeric_limits<double>::infinity();
172 const typename pcl::PointCloud<POINT_TYPE>::ConstPtr& input =
kdtree_->getInputCloud();
177 for (
int i = 0; i < 3; ++i)
186 for (
const auto& p : *input)
212 Eigen::Vector3i
toIndex(
const POINT_TYPE* point)
const 234 points_[array_index].push_back(point);
244 for (
const POINT_TYPE* target_org :
points_.find(array_index)->second)
249 const double dist_threshold_sq =
251 const double dist_sq = target_rel.
dot(target_rel) - dist_to_perpendicular_foot * dist_to_perpendicular_foot;
252 if (dist_sq < dist_threshold_sq)
262 for (
int i = 0; i < 3; ++i)
282 std::unordered_map<size_t, std::vector<const POINT_TYPE*>>
points_;
301 #endif // MCL_3DL_RAYCASTS_RAYCAST_USING_DDA_H bool isPointWithinMap(const Vec3 &point) const
Eigen::Vector3i toIndex(const Vec3 &point) const
pcl::PCLHeader point_cloud_header_
void setExists(const POINT_TYPE *point)
constexpr float dot(const Vec3 &q) const
std::shared_ptr< ChunkedKdtree > Ptr
Eigen::Vector3i begin_index_
std::vector< uint8_t > point_exists_
Eigen::Vector3i current_index_
TFSIMD_FORCE_INLINE Vector3 normalized() const
const double hit_tolerance_
Eigen::Vector3i end_index_
RaycastUsingDDA(const double map_grid_size_x, const double map_grid_size_y, const double map_grid_size_z, const double dda_grid_size, const double ray_angle_half, const double hit_tolerance)
bool incrementIndex(const int i)
size_t getArrayIndex(const Eigen::Vector3i &cell) const
const double ray_angle_half_
const double dda_grid_size_
ChunkedKdtree< POINT_TYPE >::Ptr kdtree_
void setRay(typename ChunkedKdtree< POINT_TYPE >::Ptr kdtree, const Vec3 ray_begin, const Vec3 ray_end_org) final
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
bool getNextCastResult(CastResult &result) final
Vec3 fromIndex(const Eigen::Vector3i &index) const
Vec3 ray_direction_vector_
const POINT_TYPE * hasIntersection(const Eigen::Vector3i &target_cell) const
Eigen::Vector3i map_size_
const double min_dist_thr_sq_
Eigen::Vector3i toIndex(const POINT_TYPE *point) const
std::unordered_map< size_t, std::vector< const POINT_TYPE * > > points_