Add Rviz voxel filter option that includes occupied|unknown surfaces

ethz-asl · Dec 30, 2024 · 28803ee · 28803ee
1 parent 8f1f01b
commit 28803ee
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 3 deletions.
diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h
@@ -14,9 +14,9 @@ namespace wavemap::rviz_plugin {
 struct CellSelectionMode : public TypeSelector<CellSelectionMode> {
   using TypeSelector<CellSelectionMode>::TypeSelector;
 
-  enum Id : TypeId { kSurface, kBand };
+  enum Id : TypeId { kSurface, kBoundary, kBand };
 
-  static constexpr std::array names = {"Surface", "Band"};
+  static constexpr std::array names = {"Surface", "Boundary", "Band"};
 };
 
 class CellSelector : public QObject {
@@ -35,6 +35,7 @@ class CellSelector : public QObject {
     return std::abs(log_odds) < unknown_occupancy_threshold_;
   }
   bool hasFreeNeighbor(const OctreeIndex& cell_index) const;
+  bool hasFreeOrUnknownNeighbor(const OctreeIndex& cell_index) const;
 
  private Q_SLOTS:  // NOLINT
   // These Qt slots get connected to signals indicating changes in the

diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc
@@ -71,17 +71,29 @@ void CellSelector::setMap(const MapBase::ConstPtr& map) {
 bool CellSelector::shouldBeDrawn(const OctreeIndex& cell_index,
                                  FloatingPoint cell_log_odds) const {
   switch (cell_selection_mode_) {
+    default:
     case CellSelectionMode::kSurface:
       // Skip free cells
       if (cell_log_odds < surface_occupancy_threshold_ ||
           isUnknown(cell_log_odds)) {
         return false;
       }
-      // Skip cells that are occluded by neighbors on all sides
+      // Skip cells that would not be visible in real life
       if (!hasFreeNeighbor(cell_index)) {
         return false;
       }
       break;
+    case CellSelectionMode::kBoundary:
+      // Skip free cells
+      if (cell_log_odds < surface_occupancy_threshold_ ||
+          isUnknown(cell_log_odds)) {
+        return false;
+      }
+      // Skip cells that are occluded by neighbors on all sides in Rviz
+      if (!hasFreeOrUnknownNeighbor(cell_index)) {
+        return false;
+      }
+      break;
     case CellSelectionMode::kBand:
       // Skip cells that don't meet the occupancy threshold
       if (cell_log_odds < band_min_occupancy_threshold_ ||
@@ -109,6 +121,21 @@ bool CellSelector::hasFreeNeighbor(const OctreeIndex& cell_index) const {
   return false;
 }
 
+bool CellSelector::hasFreeOrUnknownNeighbor(
+    const OctreeIndex& cell_index) const {
+  for (const auto& offset : kNeighborOffsets) {  // NOLINT
+    const OctreeIndex neighbor_index = {cell_index.height,
+                                        cell_index.position + offset};
+    const FloatingPoint neighbor_log_odds =
+        query_accelerator_->getCellValue(neighbor_index);
+    // Check if the neighbor is free and observed
+    if (neighbor_log_odds < surface_occupancy_threshold_) {
+      return true;
+    }
+  }
+  return false;
+}
+
 void CellSelector::cellSelectionModeUpdateCallback() {
   // Update the cached cell selection mode value
   const CellSelectionMode old_cell_selection_mode = cell_selection_mode_;