Feature/active dsg port

include/hydra/backend/dsg_updater.h

@@ -37,20 +37,14 @@
 #include <kimera_pgmo/kimera_pgmo_interface.h>
 #include <spark_dsg/scene_graph_logger.h>
 
-#include <filesystem>
 #include <map>
 #include <memory>
-#include <mutex>
-#include <thread>
 
-#include "hydra/backend/backend_input.h"
-#include "hydra/backend/external_loop_closure_receiver.h"
 #include "hydra/backend/merge_tracker.h"
-#include "hydra/backend/pgmo_configs.h"
 #include "hydra/common/output_sink.h"
 #include "hydra/common/shared_dsg_info.h"
-#include "hydra/common/shared_module_state.h"
 #include "hydra/utils/data_directory.h"
+#include "hydra/utils/logging.h"
 
 namespace hydra {
 
@@ -62,16 +56,20 @@ class DsgUpdater {
                           const kimera_pgmo::DeformationGraph&>;
   using NodeToRobotMap = std::unordered_map<NodeId, size_t>;
 
-  struct Config {
+  struct Config : VerbosityConfig {
+    using FunctorConfig = config::VirtualConfig<UpdateFunctor, true>;
+
+    Config();
+    //! Verbosity controls for functors
+    VerbosityConfig functor_logging;
     //! Enable combining multiple nodes together
     bool enable_node_merging = true;
-    //! Repeatedly run merge detection until no more merges are detected
-    bool enable_exhaustive_merging = false;
     //! If true, reset the private DSG with the unmerged graph on every loop closure
     bool reset_dsg_on_loop_closure = false;
     //! Update functors that get applied in the specified order
-    config::OrderedMap<std::string, config::VirtualConfig<UpdateFunctor, true>>
-        update_functors;
+    config::OrderedMap<std::string, FunctorConfig> update_functors;
+    //! Names of functors to use exhaustive merging for
+    std::vector<std::string> exhaustive_functors;
   } const config;
 
   DsgUpdater(const Config& config,
@@ -91,7 +89,7 @@ class DsgUpdater {
   void callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr info);
 
  private:
-  MergeTracker merge_tracker;
+  GroupedMergeTracker merge_tracker;
   std::map<std::string, UpdateFunctor::Ptr> update_functors_;
 
   DynamicSceneGraph::Ptr source_graph_;

include/hydra/backend/merge_tracker.h

@@ -43,12 +43,30 @@ struct MergeTracker {
                      SharedDsgInfo& dsg,
                      const MergeFunc& merge_attrs = MergeFunc());
 
+  void updateAllMergeAttributes(const DynamicSceneGraph& unmerged,
+                                DynamicSceneGraph& merged,
+                                const MergeFunc& merge_attrs);
+
   void clear();
 
+  void erase_nodes(std::vector<NodeId> nodes_to_erase);
+  std::string print() const;
+
  private:
   void updateParents(std::map<NodeId, NodeId>& prior_merges, const Merge& merge);
 
   std::map<NodeId, std::set<NodeId>> merge_sets_;
 };
 
+struct GroupedMergeTracker {
+  void initializeTracker(std::string name);
+  void clear();
+  void erase_nodes(std::vector<NodeId> nodes);
+  std::string print() const;
+  MergeTracker& getMergeGroup(std::string name);
+
+ private:
+  std::map<std::string, MergeTracker> group_to_tracker_;
+};
+
 }  // namespace hydra

include/hydra/backend/update_buildings_functor.h

@@ -33,8 +33,6 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
-#include <config_utilities/factory.h>
-
 #include "hydra/backend/update_functions.h"
 
 namespace hydra {
@@ -50,11 +48,6 @@ struct UpdateBuildingsFunctor : public UpdateFunctor {
   void call(const DynamicSceneGraph& unmerged,
             SharedDsgInfo& dsg,
             const UpdateInfo::ConstPtr& info) const override;
-
- private:
-  inline static const auto registration_ =
-      config::RegistrationWithConfig<UpdateFunctor, UpdateBuildingsFunctor, Config>(
-          "UpdateBuildingsFunctor");
 };
 
 void declare_config(UpdateBuildingsFunctor::Config& config);

include/hydra/backend/update_frontiers_functor.h

@@ -41,11 +41,12 @@ namespace hydra {
 
 struct UpdateFrontiersFunctor : public UpdateFunctor {
   struct Config {
-    //! Frontiers with in this distance of the robot will be checked against places for
-    //! potential removal
-    double frontier_removal_check_threshold = 4;
+    //! Frontiers within this distance will be checked against places for removal
+    double frontier_removal_check_threshold = 4.0;
     //! Frontiers within this distance of the robot will be removed
-    double frontier_removal_threshold = 1;
+    double frontier_removal_threshold = 1.0;
+    //! Distance around existing frontiers that blocks adding new frontiers
+    double frontier_exclusion_radius = 0.0;
   } const config;
 
   explicit UpdateFrontiersFunctor(const Config& config) : config(config) {}
@@ -54,12 +55,18 @@ struct UpdateFrontiersFunctor : public UpdateFunctor {
             SharedDsgInfo&,
             const UpdateInfo::ConstPtr&) const override;
 
-  void cleanup(uint64_t timestamp_ns, SharedDsgInfo& dsg) const;
+  void cleanup(uint64_t timestamp_ns,
+               DynamicSceneGraph& unmerged_dsg,
+               SharedDsgInfo& dsg) const;
 
  private:
   inline static const auto registration_ =
       config::RegistrationWithConfig<UpdateFunctor, UpdateFrontiersFunctor, Config>(
           "UpdateFrontiersFunctor");
+  mutable NodeSymbol next_node_id_ = NodeSymbol('G', 0);
+  mutable std::set<NodeId> deleted_frontiers_;
 };
 
+void declare_config(UpdateFrontiersFunctor::Config& config);
+
 }  // namespace hydra

include/hydra/backend/update_functions.h

@@ -65,16 +65,15 @@ struct UpdateInfo {
   kimera_pgmo::MeshOffsetInfo mesh_offsets = {};
 };
 
-using LayerCleanupFunc =
-    std::function<void(const UpdateInfo::ConstPtr&, SharedDsgInfo*)>;
+using LayerCleanupFunc = std::function<void(
+    const UpdateInfo::ConstPtr&, DynamicSceneGraph&, SharedDsgInfo*)>;
 using FindMergeFunc =
     std::function<MergeList(const DynamicSceneGraph&, const UpdateInfo::ConstPtr&)>;
 using MergeFunc = std::function<NodeAttributes::Ptr(const DynamicSceneGraph&,
                                                     const std::vector<NodeId>&)>;
 
 struct UpdateFunctor {
   using Ptr = std::shared_ptr<UpdateFunctor>;
-
   struct Hooks {
     LayerCleanupFunc cleanup;
     FindMergeFunc find_merges;

include/hydra/backend/update_rooms_functor.h

@@ -33,8 +33,6 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
-#include <config_utilities/factory.h>
-
 #include "hydra/backend/update_functions.h"
 #include "hydra/common/output_sink.h"
 #include "hydra/rooms/room_finder.h"
@@ -60,9 +58,6 @@ struct UpdateRoomsFunctor : public UpdateFunctor {
   std::unique_ptr<RoomFinder> room_finder;
 
  private:
-  inline static const auto registration_ =
-      config::RegistrationWithConfig<UpdateFunctor, UpdateRoomsFunctor, Config>(
-          "UpdateRoomsFunctor");
   Sink::List sinks_;
 };
 

include/hydra/backend/update_surface_places_functor.h

@@ -88,10 +88,6 @@ struct Update2dPlacesFunctor : public UpdateFunctor {
 
  private:
   mutable NodeSymbol next_node_id_ = NodeSymbol('S', 0);
-
-  inline static const auto registration_ =
-      config::RegistrationWithConfig<UpdateFunctor, Update2dPlacesFunctor, Config>(
-          "Update2dPlacesFunctor");
 };
 
 void declare_config(Update2dPlacesFunctor::Config& conf);

include/hydra/utils/logging.h

@@ -44,6 +44,9 @@ struct VerbosityConfig {
   VerbosityConfig() = default;
   VerbosityConfig(int verbosity);
   VerbosityConfig(const std::string& prefix, int verbosity = 0);
+
+  //! Make a copy of the verbosity config with a prefix derived from the provided name
+  VerbosityConfig with_name(const std::string& name) const;
 };
 
 void declare_config(VerbosityConfig& config);

src/backend/dsg_updater.cpp

@@ -41,26 +41,53 @@
 #include <glog/stl_logging.h>
 #include <kimera_pgmo/utils/mesh_io.h>
 
-#include "hydra/common/global_info.h"
-#include "hydra/common/launch_callbacks.h"
-#include "hydra/common/pipeline_queues.h"
-#include "hydra/utils/minimum_spanning_tree.h"
-#include "hydra/utils/pgmo_mesh_traits.h"
+#include "hydra/utils/pgmo_mesh_traits.h"  // IWYU pragma: keep
 #include "hydra/utils/timing_utilities.h"
 
 namespace hydra {
+namespace {
+
+void findAndApplyMerges(const VerbosityConfig& config,
+                        const UpdateFunctor::Hooks& hooks,
+                        const UpdateInfo::ConstPtr& info,
+                        const DynamicSceneGraph& source,
+                        SharedDsgInfo& target,
+                        MergeTracker& tracker,
+                        bool exhaustive) {
+  // TODO(nathan) handle given merges
+  auto merges = hooks.find_merges(source, info);
+  auto applied = tracker.applyMerges(source, merges, target, hooks.merge);
+  MLOG(1) << "pass 0: " << merges.size() << " merges (applied " << applied << ")";
+  if (!exhaustive) {
+    return;
+  }
+
+  applied = 0;
+  size_t iter = 0;
+  do {
+    merges = hooks.find_merges(*target.graph, info);
+    applied = tracker.applyMerges(source, merges, target, hooks.merge);
+    MLOG(1) << "pass " << iter << ": " << merges.size() << " merges (applied "
+            << applied << ")";
+    ++iter;
+  } while (applied > 0);
+}
+
+}  // namespace
 
 using hydra::timing::ScopedTimer;
-using kimera_pgmo::KimeraPgmoInterface;
-using pose_graph_tools::PoseGraph;
+
+DsgUpdater::Config::Config() : VerbosityConfig("[dsg_updated] ") {}
 
 void declare_config(DsgUpdater::Config& config) {
   using namespace config;
   name("DsgUpdaterConfig");
+  base<VerbosityConfig>(config);
+  field(config.functor_logging, "functor_logging");
   field(config.enable_node_merging, "enable_node_merging");
-  field(config.enable_exhaustive_merging, "enable_exhaustive_merging");
   field(config.reset_dsg_on_loop_closure, "reset_dsg_on_loop_closure");
   field(config.update_functors, "update_functors");
+  field(config.exhaustive_functors, "exhaustive_functors");
 }
 
 DsgUpdater::DsgUpdater(const Config& config,
@@ -69,6 +96,7 @@ DsgUpdater::DsgUpdater(const Config& config,
     : config(config::checkValid(config)), source_graph_(source), target_dsg_(target) {
   for (const auto& [name, functor] : config.update_functors) {
     update_functors_.emplace(name, functor.create());
+    merge_tracker.initializeTracker(name);
   }
 }
 
@@ -116,11 +144,28 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i
   if (config.reset_dsg_on_loop_closure && info->loop_closure_detected) {
     resetBackendDsg(timestamp_ns);
   }
+
   GraphMergeConfig merge_config;
   merge_config.previous_merges = &target_dsg_->merges;
   merge_config.update_dynamic_attributes = false;
   target_dsg_->graph->mergeGraph(*source_graph_, merge_config);
 
+  // Nodes occasionally get added to the backend after they've left the active window,
+  // which means they never get deformed or updated correctly. This forces them to be
+  // active for at least one update
+  std::vector<NodeId> active_nodes_to_restore;
+  for (auto& [layer_id, layer] : source_graph_->layers()) {
+    for (auto& [node_id, node] : layer->nodes()) {
+      auto& attrs = node->attributes();
+      if (source_graph_->checkNode(node_id) == NodeStatus::NEW && !attrs.is_active) {
+        attrs.is_active = true;
+        active_nodes_to_restore.push_back(node_id);
+      }
+    }
+  }
+
+  const std::set<std::string> exhaustive_names(config.exhaustive_functors.begin(),
+                                               config.exhaustive_functors.end());
   std::list<LayerCleanupFunc> cleanup_hooks;
   for (const auto& [name, functor] : update_functors_) {
     if (!functor) {
@@ -134,30 +179,39 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i
 
     functor->call(*source_graph_, *target_dsg_, info);
     if (hooks.find_merges && enable_merging) {
-      // TODO(nathan) handle given merges
-      const auto merges = hooks.find_merges(*source_graph_, info);
-      const auto applied =
-          merge_tracker.applyMerges(*source_graph_, merges, *target_dsg_, hooks.merge);
-      VLOG(1) << "[Backend: " << name << "] Found " << merges.size()
-              << " merges (applied " << applied << ")";
-
-      if (config.enable_exhaustive_merging) {
-        size_t merge_iter = 0;
-        size_t num_applied = 0;
-        do {
-          const auto new_merges = hooks.find_merges(*target_dsg_->graph, info);
-          num_applied = merge_tracker.applyMerges(
-              *source_graph_, new_merges, *target_dsg_, hooks.merge);
-          VLOG(1) << "[Backend: " << name << "] Found " << new_merges.size()
-                  << " merges at pass " << merge_iter << " (" << num_applied
-                  << " applied)";
-          ++merge_iter;
-        } while (num_applied > 0);
+      auto& tracker = merge_tracker.getMergeGroup(name);
+      findAndApplyMerges(config.functor_logging.with_name(name),
+                         hooks,
+                         info,
+                         *source_graph_,
+                         *target_dsg_,
+                         tracker,
+                         exhaustive_names.count(name));
+      if (info->loop_closure_detected && hooks.merge) {
+        MLOG(3) << "updating all merge attributes for " << name;
+        MLOG(3) << "current tracker: " << tracker.print();
+        tracker.updateAllMergeAttributes(
+            *source_graph_, *target_dsg_->graph, hooks.merge);
       }
     }
-  }
 
-  launchCallbacks(cleanup_hooks, info, target_dsg_.get());
+    MLOG(2) << "all merges: " << merge_tracker.print();
+    for (const auto& func : cleanup_hooks) {
+      func(info, *source_graph_, target_dsg_.get());
+    }
+
+    // We reset active flags for all new nodes that were inactive after one update
+    for (auto node_id : active_nodes_to_restore) {
+      auto node = source_graph_->findNode(node_id);
+      if (node) {
+        node->attributes().is_active = false;
+      }
+    }
+
+    // clear new node status
+    // TODO(nathan) add API for marking new nodes
+    source_graph_->getNewNodes(true);
+  }
 }
 
 }  // namespace hydra