How to fetch occupied voxels for specific resolution?

[Divelix]

Is there proper way of how to traverse octree and fetch center points (cartesian coordinates in world frame) of occupied voxels for given resolution?

Recently I managed to get occupied voxels of the map using forEachLeaf method:

Eigen::MatrixXi get_leafs(const HashedWaveletOctree::Ptr map_ptr, float threshold) {
  std::vector<Eigen::Matrix<int, 3, 1>> vec;
  vec.reserve(20000);
  map_ptr->forEachLeaf([&vec](const auto& index, FloatingPoint value) {
    if (value > 1e-3f) {
      vec.emplace_back(index.position);
    }
  });
  size_t rows = static_cast<size_t>(vec.size());
  size_t cols = 3;
  Eigen::MatrixXi occ_mat(rows, cols);
  for (size_t i = 0; i < vec.size(); i++) {
    occ_mat.row(i) = vec[i].transpose();
  }
  return occ_mat;
}

In my understanding, it fetches voxels at finest resolution, but is there any way to fetch map at coarser resolution?
P.S. @victorreijgwart can you clarify for me, please, does NdtreeIndex::Position contain cartesian coordinates I'm lookng for, or they should be converted somehow to get voxel centers in world frame?

[victorreijgwart]

Hi @Divelix, by default, the forEachLeaf method will call the lambda you give it for every leaf of the octree. These leaves will usually be at the highest resolution in areas where the occupancy is changing (e.g. around surfaces) and at a lower resolution in areas where the map is constant (e.g. in free space when the estimate has converged).
You can limit the maximum resolution for forEachLeaf by using this interface. We'll add more methods in the future to make the API more convenient, but in the meantime you could already do what you want with:

  constexpr IndexElement kMinHeight = 1;
  for (const auto& [block_index, block] : map_ptr->getBlocks()) {
    block.forEachLeaf(
        block_index,
        [](const auto& index, FloatingPoint value) {
          if (1e-3f < std::abs(value)) {
            std::cout << index.toString() << ": " << value << "\n";
          }
        },
        kMinHeight);
  }

The NdtreeIndex::Position gives the cartesian index of the cell (in integers) at the resolution level set by NdtreeIndex::Height. The height corresponds to the node's height in the octree, so higher heights correspond to lower resolutions and height=0 is the maximum resolution. You can use these conversion functions to compute the position of the center and min/max corners for such an octree index in the world frame. The min_cell_width argument is the cell width (or resolution) at height 0, which you can get from the config or map_ptr->getMinCellWidth().