Feat/traversability cleanup

hydra/include/hydra/frontend/traversability_place_extractor.h

@@ -54,8 +54,10 @@ class TraversabilityPlaceExtractor : public GraphBuilderFunctor {
     std::string layer = spark_dsg::DsgLayers::TRAVERSABILITY;
     //! Estimator for maintaining traversability state
     config::VirtualConfig<TraversabilityEstimator> estimator;
-    //! Postprocessing filters for the traversability state (before clustering)
-    TraversabilityProcessors::Config postprocessing;
+    //! Processors applied to the stored traversability state after estimation
+    PersistentTraversabilityProcessors::Config persistent_processing;
+    //! Processors applied to a copy of the traversability state before clustering
+    TransientTraversabilityProcessors::Config transient_processing;
     //! Clustering that produces the places layer from the traversability state
     config::VirtualConfig<TraversabilityClustering> clustering;
     //! Visualization sinks for the current traversaiblity information
@@ -77,7 +79,9 @@ class TraversabilityPlaceExtractor : public GraphBuilderFunctor {
 
  protected:
   TraversabilityEstimator::Ptr estimator_;
-  const TraversabilityProcessors postprocessing_;
+  std::unique_ptr<TraversabilityLayer> layer_;
+  const PersistentTraversabilityProcessors persistent_processing_;
+  const TransientTraversabilityProcessors transient_processing_;
   TraversabilityClustering::Ptr clustering_;
   Sink::List sinks_;
 };

hydra/include/hydra/places/traversability_estimator.h

@@ -111,12 +111,10 @@ class TraversabilityEstimator {
    * @brief Update the traversability based on the active window output and mesh delta.
    * @param msg The active window output containing the latest sensor data and
    * volumetric map updates.
+   * @param layer The traversability layer to update.
    */
-  virtual void updateTraversability(const ActiveWindowOutput& msg) = 0;
-
-  virtual const TraversabilityLayer& getTraversabilityLayer() const {
-    return *traversability_layer_;
-  }
+  virtual void updateTraversability(const ActiveWindowOutput& msg,
+                                    TraversabilityLayer& layer) = 0;
 
   /**
    * @brief Classify a traversability voxel based on its confidence and traversability.
@@ -125,9 +123,6 @@ class TraversabilityEstimator {
    * logic.
    */
   virtual void classifyTraversabilityVoxel(TraversabilityVoxel& voxel) const;
-
- protected:
-  std::unique_ptr<TraversabilityLayer> traversability_layer_;
 };
 
 void declare_config(TraversabilityEstimator::Config& config);
@@ -154,16 +149,17 @@ class HeightTraversabilityEstimator : public TraversabilityEstimator {
   HeightTraversabilityEstimator(const Config& config);
   ~HeightTraversabilityEstimator() override = default;
 
-  void updateTraversability(const ActiveWindowOutput& msg) override;
+  void updateTraversability(const ActiveWindowOutput& msg,
+                            TraversabilityLayer& layer) override;
 
   const Config config;
 
  protected:
   TsdfLayer::Ptr tsdf_layer_;
 
   // Processing steps.
-  void updateTsdf(const ActiveWindowOutput& msg);
-  void computeTraversability(const ActiveWindowOutput& msg);
+  void updateTsdf(const ActiveWindowOutput& msg, TraversabilityLayer& layer);
+  void computeTraversability(const ActiveWindowOutput& msg, TraversabilityLayer& layer);
 
   // Helper functions.
   BlockIndexSet get2DBlockIndices(const BlockIndices& blocks) const;
@@ -210,7 +206,8 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator {
   GradientTraversabilityEstimator(const Config& config);
   ~GradientTraversabilityEstimator() override = default;
 
-  void updateTraversability(const ActiveWindowOutput& msg) override;
+  void updateTraversability(const ActiveWindowOutput& msg,
+                            TraversabilityLayer& layer) override;
 
   const Config config;
   Sink::List sinks_;
@@ -219,8 +216,8 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator {
   TsdfLayer::Ptr tsdf_layer_;
 
   // Processing steps (reuse updateTsdf from HeightTraversabilityEstimator).
-  void updateTsdf(const ActiveWindowOutput& msg);
-  void computeTraversability(const ActiveWindowOutput& msg);
+  void updateTsdf(const ActiveWindowOutput& msg, TraversabilityLayer& layer);
+  void computeTraversability(const ActiveWindowOutput& msg, TraversabilityLayer& layer);
 };
 
 void declare_config(GradientTraversabilityEstimator::Config& config);

hydra/include/hydra/places/traversability_postprocessing.h

@@ -36,16 +36,15 @@
 
 #include <config_utilities/virtual_config.h>
 
+#include <Eigen/Core>
 #include <memory>
 
+#include "hydra/active_window/active_window_output.h"
 #include "hydra/places/traversability_layer.h"
 
 namespace hydra::places {
 
-/**
- * @brief Interface for various traversability post processing functions that can be
- * applied to the traversability layer.
- */
+//! Interface for post-processing functions applied to the traversability layer.
 class TraversabilityProcessor {
  public:
   using Ptr = std::shared_ptr<TraversabilityProcessor>;
@@ -56,42 +55,72 @@ class TraversabilityProcessor {
 
   /**
    * @brief Apply the post processing to the traversability layer.
+   * @param msg The active window output that triggered the update.
    * @param layer The traversability layer to process.
    */
-  virtual void apply(TraversabilityLayer& layer) = 0;
+  virtual void apply(const ActiveWindowOutput& msg, TraversabilityLayer& layer) = 0;
 };
 
-/**
- * @brief Utility container for several processors.
- */
+//! Processor whose changes do not persist, i.e., applied to a copy of the layer.
+class TransientTraversabilityProcessor : public TraversabilityProcessor {
+ public:
+  using Ptr = std::shared_ptr<TransientTraversabilityProcessor>;
+  using ConstPtr = std::shared_ptr<const TransientTraversabilityProcessor>;
+};
+
+//! Processor whose changes persist, i.e., applied to the stored layer.
+class PersistentTraversabilityProcessor : public TraversabilityProcessor {
+ public:
+  using Ptr = std::shared_ptr<PersistentTraversabilityProcessor>;
+  using ConstPtr = std::shared_ptr<const PersistentTraversabilityProcessor>;
+};
+
+//! Utility container for several processors.
+template <typename ProcessorT>
 struct TraversabilityProcessors {
-  using Config = std::vector<config::VirtualConfig<TraversabilityProcessor>>;
+  using Config = std::vector<config::VirtualConfig<ProcessorT>>;
 
-  explicit TraversabilityProcessors(const Config& config);
+  explicit TraversabilityProcessors(const Config& config) {
+    for (const auto& processor_config : config) {
+      auto processor = processor_config.create();
+      if (processor) {
+        processors_.emplace_back(std::move(processor));
+      }
+    }
+  }
 
-  /** @brief Apply all processors to the traversability layer.
+  /**
+   * @brief Apply all processors to the traversability layer.
+   * @param msg The active window output that triggered the update.
    * @param layer The traversability layer to process.
    */
-  void apply(TraversabilityLayer& layer) const;
+  void apply(const ActiveWindowOutput& msg, TraversabilityLayer& layer) const {
+    for (const auto& processor : processors_) {
+      processor->apply(msg, layer);
+    }
+  }
 
  private:
-  std::vector<TraversabilityProcessor::Ptr> processors_;
+  std::vector<std::shared_ptr<ProcessorT>> processors_;
 };
 
-/**
- * @brief Dilate intraversable space in the traversability layer.
- */
-class ErosionDilation : public TraversabilityProcessor {
+using TransientTraversabilityProcessors =
+    TraversabilityProcessors<TransientTraversabilityProcessor>;
+using PersistentTraversabilityProcessors =
+    TraversabilityProcessors<PersistentTraversabilityProcessor>;
+
+//! Dilate intraversable space in the traversability layer.
+class ErosionDilation : public TransientTraversabilityProcessor {
  public:
   struct Config {
-    //! @brief Number of dilation steps to apply.
+    //! Number of dilation steps to apply.
     size_t num_dilations = 1;
   };
 
   ErosionDilation(const Config& config);
   ~ErosionDilation() override = default;
 
-  void apply(TraversabilityLayer& layer) override;
+  void apply(const ActiveWindowOutput& msg, TraversabilityLayer& layer) override;
 
   const Config config;
 
@@ -101,4 +130,42 @@ class ErosionDilation : public TraversabilityProcessor {
 
 void declare_config(ErosionDilation::Config& config);
 
+//! Mark all voxels the robot footprint intersected as traversable. Footprint voxels
+//! are accumulated while their block is in the active window and re-applied every
+//! update, since the estimator resets the state of updated blocks.
+class RobotFootprintTraversability : public PersistentTraversabilityProcessor {
+ public:
+  struct Config {
+    //! Minimum xy corner of the robot footprint in body frame [m].
+    Eigen::Vector2f bbox_min = Eigen::Vector2f::Zero();
+    //! Maximum xy corner of the robot footprint in body frame [m].
+    Eigen::Vector2f bbox_max = Eigen::Vector2f::Zero();
+  };
+
+  RobotFootprintTraversability(const Config& config);
+  ~RobotFootprintTraversability() override = default;
+
+  void apply(const ActiveWindowOutput& msg, TraversabilityLayer& layer) override;
+
+  //! Drop footprint voxels whose block is no longer in the layer, i.e., archived.
+  void archive(const TraversabilityLayer& layer);
+
+  //! Add all voxels intersecting the footprint at the given pose.
+  void addFootprint(const Eigen::Isometry3f& world_T_body,
+                    const TraversabilityLayer& layer);
+
+  //! Mark all accumulated footprint voxels as traversable.
+  void markTraversable(TraversabilityLayer& layer) const;
+
+  const Index2DSet& footprint() const { return footprint_; }
+
+  const Config config;
+
+ protected:
+  //! Global 2D voxel indices that were covered by the footprint at any time.
+  Index2DSet footprint_;
+};
+
+void declare_config(RobotFootprintTraversability::Config& config);
+
 }  // namespace hydra::places