fast_occupancy_grid.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2025  Kenji Koide (k.koide@aist.go.jp)
#pragma once
 
#include <vector>
#include <bitset>
#include <numeric>
#include <gtsam_points/types/point_cloud.hpp>
 
namespace gtsam_points {
 
struct FastOccupancyBlock {
public:
  FastOccupancyBlock() : cells(0) {}
 
  static constexpr int stride = 8;                            
  static constexpr int num_cells = stride * stride * stride;  
 
  int cell_index(const Eigen::Vector3i& coord) const { return coord.x() + coord.y() * stride + coord.z() * stride * stride; }
 
  bool occupied(const Eigen::Vector3i& coord) const { return cells[cell_index(coord)]; }
 
  bool free(const Eigen::Vector3i& coord) const { return !occupied(coord); }
 
  void set_occupied(const Eigen::Vector3i& coord) { cells.set(cell_index(coord)); }
 
  void set_free(const Eigen::Vector3i& coord) { cells.reset(cell_index(coord)); }
 
  int count() const { return cells.count(); }
 
public:
  std::bitset<num_cells> cells;  
};
 
class FastOccupancyGrid {
public:
  using Ptr = std::shared_ptr<FastOccupancyGrid>;
  using ConstPtr = std::shared_ptr<const FastOccupancyGrid>;
 
  FastOccupancyGrid(double resolution);
  ~FastOccupancyGrid();
 
  template <typename PointCloud>
  void insert(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity());
 
  template <typename PointCloud>
  int calc_overlap(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity()) const;
 
  template <typename PointCloud>
  double calc_overlap_rate(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity()) const;
 
  template <typename PointCloud>
  std::vector<unsigned char> get_overlaps(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity()) const;
 
  int num_occupied_cells() const;
 
private:
  std::uint64_t calc_index(const Eigen::Vector4i& coord) const;
 
  Eigen::Vector3i calc_coord(std::uint64_t index) const;
 
  std::uint64_t calc_hash(std::uint64_t index) const;
 
  std::uint64_t find_block(std::uint64_t block_index) const;
 
  std::uint64_t find_or_insert_block(std::uint64_t block_index);
 
  void rehash(size_t hash_size);
 
private:
  static constexpr std::uint64_t INVALID_INDEX = std::numeric_limits<std::uint64_t>::max();
  static constexpr int coord_bit_size = 21;                          
  static constexpr std::uint64_t coord_bit_mask = (1ull << 21) - 1;  
  static constexpr std::uint64_t coord_offset = 1ull << (coord_bit_size - 1);  
 
  double inv_resolution;                                             
  int max_seek_count;                                                
  std::vector<std::pair<std::uint64_t, FastOccupancyBlock>> blocks;  
};
 
}  // namespace gtsam_points