diff --git a/interface_nbvp_rotors/launch/flat_exploration.launch b/interface_nbvp_rotors/launch/flat_exploration.launch index ef7c2441..245273be 100644 --- a/interface_nbvp_rotors/launch/flat_exploration.launch +++ b/interface_nbvp_rotors/launch/flat_exploration.launch @@ -9,9 +9,9 @@ - + - + diff --git a/interface_nbvp_rotors/launch/mav_inspector.launch b/interface_nbvp_rotors/launch/mav_inspector.launch index 39b635a6..46a49d59 100644 --- a/interface_nbvp_rotors/launch/mav_inspector.launch +++ b/interface_nbvp_rotors/launch/mav_inspector.launch @@ -7,7 +7,7 @@ - + @@ -18,34 +18,37 @@ - + - + - + - + + + + - + @@ -54,15 +57,27 @@ - + + + + + + + + + + + + + - + diff --git a/interface_nbvp_rotors/worlds/flat.world b/interface_nbvp_rotors/worlds/flat.world index 9d3142c3..ffe3621c 100644 --- a/interface_nbvp_rotors/worlds/flat.world +++ b/interface_nbvp_rotors/worlds/flat.world @@ -1,5 +1,6 @@ + 1 0 0 10 0 -0 0 diff --git a/nbvplanner/CMakeLists.txt b/nbvplanner/CMakeLists.txt index 07f7f56b..b9327dc2 100644 --- a/nbvplanner/CMakeLists.txt +++ b/nbvplanner/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS tf kdtree multiagent_collision_check + voxblox_ros ) find_package(cmake_modules REQUIRED) find_package(Eigen REQUIRED) @@ -32,7 +33,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} LIBRARIES nbvplanner ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES} - CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs octomap_world tf kdtree + CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs octomap_world tf kdtree voxblox_ros ) include_directories( @@ -42,8 +43,8 @@ include_directories( ${OCTOMAP_INCLUDE_DIRS} ) -add_library(nbvPlannerLib src/mesh_structure.cpp src/nbvp.cpp src/rrt.cpp src/tree.cpp) -add_executable(nbvPlanner src/nbv_planner_node.cpp src/mesh_structure.cpp src/nbvp.cpp src/rrt.cpp src/tree.cpp) +add_library(nbvPlannerLib src/mesh_structure.cpp src/nbvp.cpp src/rrt.cpp src/tree.cpp src/voxblox_map_manager.cpp) +add_executable(nbvPlanner src/nbv_planner_node.cpp src/mesh_structure.cpp src/nbvp.cpp src/rrt.cpp src/tree.cpp src/voxblox_map_manager.cpp) add_dependencies(nbvPlannerLib ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(nbvPlannerLib diff --git a/nbvplanner/include/nbvplanner/mesh_structure.h b/nbvplanner/include/nbvplanner/mesh_structure.h index cb8f01ac..04cc1c70 100644 --- a/nbvplanner/include/nbvplanner/mesh_structure.h +++ b/nbvplanner/include/nbvplanner/mesh_structure.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include "nbvplanner/voxblox_map_manager.h" namespace mesh { @@ -42,7 +42,7 @@ class StlMesh { resolution_ = resolution; } - static void setOctomapManager(volumetric_mapping::OctomapManager * manager) + static void setVoxbloxManager(nbvInspection::VoxbloxMapManager * manager) { manager_ = manager; } @@ -73,7 +73,7 @@ class StlMesh static std::vector cameraVerticalFoV_; static double maxDist_; static std::vector > camBoundNormals_; - static volumetric_mapping::OctomapManager * manager_; + static nbvInspection::VoxbloxMapManager * manager_; static std::vector peer_vehicles_; }; } diff --git a/nbvplanner/include/nbvplanner/nbvp.h b/nbvplanner/include/nbvplanner/nbvp.h index d4205918..360efe32 100644 --- a/nbvplanner/include/nbvplanner/nbvp.h +++ b/nbvplanner/include/nbvplanner/nbvp.h @@ -29,6 +29,7 @@ #include #include #include +#include #define SQ(x) ((x)*(x)) #define SQRT2 0.70711 @@ -56,7 +57,7 @@ class nbvPlanner Params params_; mesh::StlMesh * mesh_; - volumetric_mapping::OctomapManager * manager_; + nbvInspection::VoxbloxMapManager * manager_; bool ready_; diff --git a/nbvplanner/include/nbvplanner/nbvp.hpp b/nbvplanner/include/nbvplanner/nbvp.hpp index bfcfa078..1b2a904f 100644 --- a/nbvplanner/include/nbvplanner/nbvp.hpp +++ b/nbvplanner/include/nbvplanner/nbvp.hpp @@ -36,7 +36,7 @@ nbvInspection::nbvPlanner::nbvPlanner(const ros::NodeHandle& nh, nh_private_(nh_private) { - manager_ = new volumetric_mapping::OctomapManager(nh_, nh_private_); + manager_ = new nbvInspection::VoxbloxMapManager(nh_, nh_private_); // Set up the topics and services params_.inspectionPath_ = nh_.advertise("inspectionPath", 1000); @@ -100,7 +100,7 @@ nbvInspection::nbvPlanner::nbvPlanner(const ros::NodeHandle& nh, if (stlFile.is_open()) { mesh_ = new mesh::StlMesh(stlFile); mesh_->setResolution(params_.meshResolution_); - mesh_->setOctomapManager(manager_); + mesh_->setVoxbloxManager(manager_); mesh_->setCameraParams(params_.camPitch_, params_.camHorizontal_, params_.camVertical_, params_.gainRange_); } else { @@ -175,7 +175,7 @@ bool nbvInspection::nbvPlanner::plannerCallback(nbvplanner::nbvp_srv:: ROS_ERROR_THROTTLE(1, "Planner not set up: No octomap available!"); return true; } - if (manager_->getMapSize().norm() <= 0.0) { + if (manager_->isEmpty()) { ROS_ERROR_THROTTLE(1, "Planner not set up: Octomap is empty!"); return true; } diff --git a/nbvplanner/include/nbvplanner/rrt.h b/nbvplanner/include/nbvplanner/rrt.h index 7fbf044f..28aeadc7 100644 --- a/nbvplanner/include/nbvplanner/rrt.h +++ b/nbvplanner/include/nbvplanner/rrt.h @@ -38,7 +38,7 @@ class RrtTree : public TreeBase typedef Eigen::Vector4d StateVec; RrtTree(); - RrtTree(mesh::StlMesh * mesh, volumetric_mapping::OctomapManager * manager); + RrtTree(mesh::StlMesh * mesh, VoxbloxMapManager * manager); ~RrtTree(); virtual void setStateFromPoseMsg(const geometry_msgs::PoseWithCovarianceStamped& pose); virtual void setStateFromOdometryMsg(const nav_msgs::Odometry& pose); diff --git a/nbvplanner/include/nbvplanner/tree.h b/nbvplanner/include/nbvplanner/tree.h index 3a855e18..d7dbb53a 100644 --- a/nbvplanner/include/nbvplanner/tree.h +++ b/nbvplanner/include/nbvplanner/tree.h @@ -24,6 +24,7 @@ #include #include #include +#include namespace nbvInspection { @@ -95,14 +96,14 @@ class TreeBase Node * bestNode_; Node * rootNode_; mesh::StlMesh * mesh_; - volumetric_mapping::OctomapManager * manager_; + nbvInspection::VoxbloxMapManager * manager_; stateVec root_; stateVec exact_root_; std::vector*> segments_; std::vector agentNames_; public: TreeBase(); - TreeBase(mesh::StlMesh * mesh, volumetric_mapping::OctomapManager * manager); + TreeBase(mesh::StlMesh * mesh, nbvInspection::VoxbloxMapManager * manager); ~TreeBase(); virtual void setStateFromPoseMsg(const geometry_msgs::PoseWithCovarianceStamped& pose) = 0; virtual void setStateFromOdometryMsg(const nav_msgs::Odometry& pose) = 0; diff --git a/nbvplanner/include/nbvplanner/tree.hpp b/nbvplanner/include/nbvplanner/tree.hpp index a78166de..cb8d5a02 100644 --- a/nbvplanner/include/nbvplanner/tree.hpp +++ b/nbvplanner/include/nbvplanner/tree.hpp @@ -48,7 +48,7 @@ nbvInspection::TreeBase::TreeBase() template nbvInspection::TreeBase::TreeBase(mesh::StlMesh * mesh, - volumetric_mapping::OctomapManager * manager) + nbvInspection::VoxbloxMapManager * manager) { mesh_ = mesh; manager_ = manager; @@ -106,7 +106,7 @@ template void nbvInspection::TreeBase::insertPointcloudWithTf( const sensor_msgs::PointCloud2::ConstPtr& pointcloud) { - manager_->insertPointcloudWithTf(pointcloud); + // manager_->insertPointcloudWithTf(pointcloud); } template diff --git a/nbvplanner/include/nbvplanner/voxblox_map_manager.h b/nbvplanner/include/nbvplanner/voxblox_map_manager.h new file mode 100644 index 00000000..9376307e --- /dev/null +++ b/nbvplanner/include/nbvplanner/voxblox_map_manager.h @@ -0,0 +1,61 @@ +#ifndef NBVPLANNER_VOXBLOX_MAP_MANAGER_H_ +#define NBVPLANNER_VOXBLOX_MAP_MANAGER_H_ + +#include +#include + +namespace nbvInspection { + +// A small class that uses a voxblox TSDF server underneath to emulate the +// functionality of Octomap. +// NOTE: for now uses only *T*SDFs, so doesn't compute the full ESDFs and +// doesn't take advantage of faster collision checking! +class VoxbloxMapManager { + public: + enum VoxelStatus { kUnknown = 0, kOccupied, kFree }; + + VoxbloxMapManager(const ros::NodeHandle& nh, + const ros::NodeHandle& nh_private); + + VoxelStatus getVisibility(const Eigen::Vector3d& view_point, + const Eigen::Vector3d& voxel_to_test, + bool stop_at_unknown_voxel) const; + + VoxelStatus getVoxelStatus(const Eigen::Vector3d& position) const; + + // Project an axis-aligned bounding box along a line. + VoxelStatus getLineStatusBoundingBox(const Eigen::Vector3d& start, + const Eigen::Vector3d& end, + const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const; + + // Get the voxel status of an axis-aligned bounding box centered at the given + // position. + VoxelStatus getBoundingBoxStatus(const Eigen::Vector3d& center, + const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const; + + double getResolution() const { return tsdf_layer_->voxel_size(); } + + bool isEmpty() const { + return tsdf_layer_->getNumberOfAllocatedBlocks() == 0; + } + + private: + VoxelStatus getBoundingBoxStatusInVoxels( + const voxblox::LongIndex& bounding_box_center, + const voxblox::AnyIndex& bounding_box_voxels, + bool stop_at_unknown_voxel) const; + + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + + voxblox::TsdfServer tsdf_server_; + + // Cached: + voxblox::Layer* tsdf_layer_; +}; + +} // namespace nbvInspection + +#endif // NBVPLANNER_VOXBLOX_MAP_MANAGER_H_ diff --git a/nbvplanner/package.xml b/nbvplanner/package.xml index 9816dcc1..66372553 100644 --- a/nbvplanner/package.xml +++ b/nbvplanner/package.xml @@ -11,7 +11,7 @@ Andreas Bircher catkin - + roscpp tf message_generation @@ -20,7 +20,8 @@ octomap_world kdtree multiagent_collision_check - + voxblox_ros + roscpp tf message_runtime @@ -29,4 +30,5 @@ octomap_world kdtree multiagent_collision_check + voxblox_ros diff --git a/nbvplanner/src/mesh_structure.cpp b/nbvplanner/src/mesh_structure.cpp index c1d5315d..ebc4c57b 100644 --- a/nbvplanner/src/mesh_structure.cpp +++ b/nbvplanner/src/mesh_structure.cpp @@ -430,7 +430,7 @@ bool mesh::StlMesh::getVisibility(const tf::Transform& transform, bool& partialV || manager_->getVisibility( Eigen::Vector3d(originTransf.x(), originTransf.y(), originTransf.z()), Eigen::Vector3d(x1_.x(), x1_.y(), x1_.z()), stop_at_unknown_cell) - != volumetric_mapping::OctomapWorld::CellStatus::kFree) { + != nbvInspection::VoxbloxMapManager::kFree) { return false; } else { bool visibility1 = true; @@ -454,7 +454,7 @@ bool mesh::StlMesh::getVisibility(const tf::Transform& transform, bool& partialV || manager_->getVisibility( Eigen::Vector3d(originTransf.x(), originTransf.y(), originTransf.z()), Eigen::Vector3d(x2_.x(), x2_.y(), x2_.z()), stop_at_unknown_cell) - != volumetric_mapping::OctomapWorld::CellStatus::kFree) { + != nbvInspection::VoxbloxMapManager::kFree) { ret = false; } else { bool visibility2 = true; @@ -482,7 +482,7 @@ bool mesh::StlMesh::getVisibility(const tf::Transform& transform, bool& partialV || manager_->getVisibility( Eigen::Vector3d(originTransf.x(), originTransf.y(), originTransf.z()), Eigen::Vector3d(x3_.x(), x3_.y(), x3_.z()), stop_at_unknown_cell) - != volumetric_mapping::OctomapWorld::CellStatus::kFree) { + != nbvInspection::VoxbloxMapManager::kFree) { ret = false; } else { bool visibility3 = true; @@ -514,7 +514,7 @@ std::vector mesh::StlMesh::cameraHorizontalFoV_ = { }; std::vector mesh::StlMesh::cameraVerticalFoV_ = { }; double mesh::StlMesh::maxDist_ = 5; std::vector > mesh::StlMesh::camBoundNormals_ = { }; -volumetric_mapping::OctomapManager * mesh::StlMesh::manager_ = NULL; +nbvInspection::VoxbloxMapManager* mesh::StlMesh::manager_ = NULL; std::vector mesh::StlMesh::peer_vehicles_ = { }; #endif // _MESH_STRUCTURE_CPP_ diff --git a/nbvplanner/src/rrt.cpp b/nbvplanner/src/rrt.cpp index f9d65018..d341a70f 100644 --- a/nbvplanner/src/rrt.cpp +++ b/nbvplanner/src/rrt.cpp @@ -51,7 +51,7 @@ nbvInspection::RrtTree::RrtTree() } } -nbvInspection::RrtTree::RrtTree(mesh::StlMesh * mesh, volumetric_mapping::OctomapManager * manager) +nbvInspection::RrtTree::RrtTree(mesh::StlMesh * mesh, nbvInspection::VoxbloxMapManager * manager) { mesh_ = mesh; manager_ = manager; @@ -338,10 +338,10 @@ void nbvInspection::RrtTree::iterate(int iterations) newState[0] = origin[0] + direction[0]; newState[1] = origin[1] + direction[1]; newState[2] = origin[2] + direction[2]; - if (volumetric_mapping::OctomapManager::CellStatus::kFree + if (nbvInspection::VoxbloxMapManager::kFree == manager_->getLineStatusBoundingBox( origin, direction + origin + direction.normalized() * params_.dOvershoot_, - params_.boundingBox_) + params_.boundingBox_, true) && !multiagent::isInCollision(newParent->state_, newState, params_.boundingBox_, segments_)) { // Sample the new orientation newState[3] = 2.0 * M_PI * (((double) rand()) / ((double) RAND_MAX) - 0.5); @@ -434,10 +434,10 @@ void nbvInspection::RrtTree::initialize() newState[0] = origin[0] + direction[0]; newState[1] = origin[1] + direction[1]; newState[2] = origin[2] + direction[2]; - if (volumetric_mapping::OctomapManager::CellStatus::kFree + if (nbvInspection::VoxbloxMapManager::kFree == manager_->getLineStatusBoundingBox( origin, direction + origin + direction.normalized() * params_.dOvershoot_, - params_.boundingBox_) + params_.boundingBox_, true) && !multiagent::isInCollision(newParent->state_, newState, params_.boundingBox_, segments_)) { // Create new node and insert into tree @@ -554,19 +554,19 @@ double nbvInspection::RrtTree::gain(StateVec state) } // Check cell status and add to the gain considering the corresponding factor. double probability; - volumetric_mapping::OctomapManager::CellStatus node = manager_->getCellProbabilityPoint( - vec, &probability); - if (node == volumetric_mapping::OctomapManager::CellStatus::kUnknown) { + nbvInspection::VoxbloxMapManager::VoxelStatus node = manager_->getVoxelStatus( + vec); + if (node == VoxbloxMapManager::kUnknown) { // Rayshooting to evaluate inspectability of cell - if (volumetric_mapping::OctomapManager::CellStatus::kOccupied + if (nbvInspection::VoxbloxMapManager::kOccupied != this->manager_->getVisibility(origin, vec, false)) { gain += params_.igUnmapped_; // TODO: Add probabilistic gain // gain += params_.igProbabilistic_ * PROBABILISTIC_MODEL(probability); } - } else if (node == volumetric_mapping::OctomapManager::CellStatus::kOccupied) { + } else if (node == nbvInspection::VoxbloxMapManager::kOccupied) { // Rayshooting to evaluate inspectability of cell - if (volumetric_mapping::OctomapManager::CellStatus::kOccupied + if (VoxbloxMapManager::kOccupied != this->manager_->getVisibility(origin, vec, false)) { gain += params_.igOccupied_; // TODO: Add probabilistic gain @@ -574,7 +574,7 @@ double nbvInspection::RrtTree::gain(StateVec state) } } else { // Rayshooting to evaluate inspectability of cell - if (volumetric_mapping::OctomapManager::CellStatus::kOccupied + if (VoxbloxMapManager::kOccupied != this->manager_->getVisibility(origin, vec, false)) { gain += params_.igFree_; // TODO: Add probabilistic gain diff --git a/nbvplanner/src/voxblox_map_manager.cpp b/nbvplanner/src/voxblox_map_manager.cpp new file mode 100644 index 00000000..097d3b85 --- /dev/null +++ b/nbvplanner/src/voxblox_map_manager.cpp @@ -0,0 +1,156 @@ +#include "nbvplanner/voxblox_map_manager.h" + +namespace nbvInspection { + +VoxbloxMapManager::VoxbloxMapManager(const ros::NodeHandle& nh, + const ros::NodeHandle& nh_private) + : nh_(nh), nh_private_(nh_private), tsdf_server_(nh, nh_private) { + tsdf_layer_ = tsdf_server_.getTsdfMapPtr()->getTsdfLayerPtr(); + CHECK_NOTNULL(tsdf_layer_); +} + +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVoxelStatus( + const Eigen::Vector3d& position) const { + voxblox::TsdfVoxel* voxel = tsdf_layer_->getVoxelPtrByCoordinates( + position.cast()); + + if (voxel == nullptr) { + return VoxelStatus::kUnknown; + } + if (voxel->weight < 1e-6) { + return VoxelStatus::kUnknown; + } + if (voxel->distance > 0.0) { + return VoxelStatus::kFree; + } + return VoxelStatus::kOccupied; +} + +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVisibility( + const Eigen::Vector3d& view_point, const Eigen::Vector3d& voxel_to_test, + bool stop_at_unknown_voxel) const { + // This involves doing a raycast from view point to voxel to test. + // Let's get the global voxel coordinates of both. + float voxel_size = tsdf_layer_->voxel_size(); + float voxel_size_inv = 1.0 / voxel_size; + + const voxblox::Point start_scaled = + view_point.cast() * voxel_size_inv; + const voxblox::Point end_scaled = + voxel_to_test.cast() * voxel_size_inv; + + voxblox::LongIndexVector global_voxel_indices; + voxblox::castRay(start_scaled, end_scaled, &global_voxel_indices); + + // Iterate over the ray. + for (const voxblox::GlobalIndex& global_index : global_voxel_indices) { + voxblox::TsdfVoxel* voxel = + tsdf_layer_->getVoxelPtrByGlobalIndex(global_index); + if (voxel == nullptr || voxel->weight < 1e-6) { + if (stop_at_unknown_voxel) { + return VoxelStatus::kUnknown; + } + } else if (voxel->distance <= 0.0) { + return VoxelStatus::kOccupied; + } + } + return VoxelStatus::kFree; +} + +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getBoundingBoxStatus( + const Eigen::Vector3d& center, const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const { + float voxel_size = tsdf_layer_->voxel_size(); + float voxel_size_inv = 1.0 / voxel_size; + + // Get the center of the bounding box as a global index. + voxblox::LongIndex center_voxel_index = + voxblox::getGridIndexFromPoint( + center.cast(), voxel_size_inv); + + // Get the bounding box size in terms of voxels. + voxblox::AnyIndex bounding_box_voxels( + std::ceil(bounding_box_size.x() * voxel_size_inv), + std::ceil(bounding_box_size.y() * voxel_size_inv), + std::ceil(bounding_box_size.z() * voxel_size_inv)); + + // Iterate over all voxels in the bounding box. + return getBoundingBoxStatusInVoxels(center_voxel_index, bounding_box_voxels, + stop_at_unknown_voxel); +} + +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getBoundingBoxStatusInVoxels( + const voxblox::LongIndex& bounding_box_center, + const voxblox::AnyIndex& bounding_box_voxels, + bool stop_at_unknown_voxel) const { + VoxelStatus current_status = VoxelStatus::kFree; + + voxblox::LongIndex voxel_index = bounding_box_center; + for (voxel_index.x() = bounding_box_center.x() - bounding_box_voxels.x() / 2; + voxel_index.x() <= bounding_box_center.x() + bounding_box_voxels.x() / 2; + voxel_index.x()++) { + for (voxel_index.y() = + bounding_box_center.y() - bounding_box_voxels.y() / 2; + voxel_index.y() <= + bounding_box_center.y() + bounding_box_voxels.y() / 2; + voxel_index.y()++) { + for (voxel_index.z() = + bounding_box_center.z() - bounding_box_voxels.z() / 2; + voxel_index.z() <= + bounding_box_center.z() + bounding_box_voxels.z() / 2; + voxel_index.z()++) { + voxblox::TsdfVoxel* voxel = + tsdf_layer_->getVoxelPtrByGlobalIndex(voxel_index); + if (voxel == nullptr || voxel->weight < 1e-6) { + if (stop_at_unknown_voxel) { + return VoxelStatus::kUnknown; + } + current_status = VoxelStatus::kUnknown; + } else if (voxel->distance <= 0.0) { + return VoxelStatus::kOccupied; + } + } + } + } + return current_status; +} + +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getLineStatusBoundingBox( + const Eigen::Vector3d& start, const Eigen::Vector3d& end, + const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const { + // Cast ray along the center to make sure we don't miss anything. + float voxel_size = tsdf_layer_->voxel_size(); + float voxel_size_inv = 1.0 / voxel_size; + + const voxblox::Point start_scaled = + start.cast() * voxel_size_inv; + const voxblox::Point end_scaled = + end.cast() * voxel_size_inv; + + voxblox::LongIndexVector global_voxel_indices; + voxblox::castRay(start_scaled, end_scaled, &global_voxel_indices); + + // Get the bounding box size in terms of voxels. + voxblox::AnyIndex bounding_box_voxels( + std::ceil(bounding_box_size.x() * voxel_size_inv), + std::ceil(bounding_box_size.y() * voxel_size_inv), + std::ceil(bounding_box_size.z() * voxel_size_inv)); + + // Iterate over the ray. + VoxelStatus current_status = VoxelStatus::kFree; + for (const voxblox::GlobalIndex& global_index : global_voxel_indices) { + VoxelStatus box_status = getBoundingBoxStatusInVoxels( + global_index, bounding_box_voxels, stop_at_unknown_voxel); + if (box_status == VoxelStatus::kOccupied) { + return box_status; + } + if (stop_at_unknown_voxel && box_status == VoxelStatus::kUnknown) { + return box_status; + } + current_status = box_status; + } + return current_status; +} + +} // namespace nbvInspection