refactor(backend): keep the unmerged graph odometric; derive merged poses via per-node transforms

include/hydra/backend/backend_module.h

@@ -162,6 +162,12 @@ class BackendModule : public kimera_pgmo::KimeraPgmoInterface, public Module {
   uint64_t last_sequence_number_ = 0;
 
   SharedDsgInfo::Ptr private_dsg_;
+  //! Unmerged copy of the scene graph holding ODOMETRIC (never optimized) poses.
+  //! The merged private_dsg_ is derived from it via per-node odometric->optimized
+  //! transforms every optimize spin (have_loopclosures_ latches, so post-LC every
+  //! spin re-optimizes and re-deforms the active merged nodes that mergeGraph reset
+  //! to odometric clones). Nodes the interpolator skips (control-point matching
+  //! failure) or layers without a deform functor stay odometric while active.
   DynamicSceneGraph::Ptr unmerged_graph_;
   SharedModuleState::Ptr state_;
 

include/hydra/backend/deformation_interpolator.h

@@ -35,20 +35,47 @@
 #pragma once
 
 #include <memory>
+#include <optional>
 
 #include "hydra/backend/update_functions.h"
 
 namespace hydra {
 
+/**
+ * @brief Write the deformed (optimized) version of the odometric source attributes
+ * into the destination attributes.
+ *
+ * Only spatial fields are written (position, bounding box, object orientation), so
+ * merged-only state on the destination survives. Everything is derived from the
+ * odometric source, never from the destination, so repeated application with the
+ * same transform is idempotent.
+ */
+void applyNodeDeformation(const Eigen::Isometry3d& transform,
+                          const NodeAttributes& src,
+                          NodeAttributes& dst);
+
 struct NodeCache {
   struct Entry {
     NodeId id;
     uint64_t timestamp;
-    Eigen::Vector3f init_pos;
+    //! Odometric position (the unmerged graph is odometric by invariant); doubles
+    //! as the pgmo vertex for control-point matching.
+    Eigen::Vector3f pos;
+    //! Most recent odometric->optimized transform applied to this node, if any.
+    //! Lets merge hooks bring attributes rebuilt from odometric constituents into
+    //! the optimized frame.
+    std::optional<Eigen::Isometry3d> last_transform;
   };
 
   Entry* add(NodeId node, const NodeAttributes& attrs);
 
+  /**
+   * @brief Apply the node's most recent deformation transform to the given
+   * attributes (via NodeAttributes::transform).
+   * @returns true if a transform was recorded and applied.
+   */
+  bool applyLastTransform(NodeId node, NodeAttributes& attrs) const;
+
   std::map<NodeId, Entry> nodes;
 };
 
@@ -68,10 +95,14 @@ class DeformationInterpolator {
   explicit DeformationInterpolator(const Config& config);
 
   /**
-   * @brief Interpolate the node positions based on the deformation graph, using the
+   * @brief Interpolate the node poses based on the deformation graph, using the
    * temporally closest control points as reference.
    *
-   * @param unmerged Unmerged scene graph to keep up to date.
+   * Reads odometric values from the unmerged scene graph and writes the deformed
+   * (optimized) values into the private DSG only; the unmerged graph is never
+   * modified.
+   *
+   * @param unmerged Unmerged (odometric) scene graph to read from.
    * @param dsg Private DSG to update.
    * @param info Update information containing deformation graph.
    * @param view View on the unmerged scene graph selecting all nodes to update.
@@ -81,22 +112,24 @@ class DeformationInterpolator {
                    const UpdateInfo::ConstPtr& info,
                    const LayerView& view) const;
 
-  /**
-   * @brief Interpolate the node positions based on the deformation graph, using the
-   * temporally closest control points as reference.
-   *
-   * @param unmerged Unmerged scene graph to keep up to date.
-   * @param dsg Private DSG to update.
-   * @param info Update information containing deformation graph.
-   * @param view View on the unmerged scene graph selecting all nodes to update.
-   */
+  //! @copydoc DeformationInterpolator::interpolate
   void interpolateNodePositions(const DynamicSceneGraph& unmerged,
                                 DynamicSceneGraph& dsg,
                                 const UpdateInfo::ConstPtr& info,
                                 const LayerView& view) const {
     interpolate(unmerged, dsg, info, view);
   }
 
+  /**
+   * @brief Apply the node's most recent deformation transform to the given
+   * attributes (e.g. to bring merge-hook output rebuilt from odometric
+   * constituents into the optimized frame).
+   * @returns true if a transform was recorded and applied.
+   */
+  bool applyLastTransform(NodeId node, NodeAttributes& attrs) const {
+    return cache_.applyLastTransform(node, attrs);
+  }
+
  private:
   mutable NodeCache cache_;
 };

include/hydra/backend/dsg_updater.h

@@ -95,6 +95,8 @@ class DsgUpdater {
   GroupedMergeTracker merge_tracker;
   std::map<std::string, UpdateFunctor::Ptr> update_functors_;
 
+  //! Source (unmerged) graph holding odometric poses; update functors read it and
+  //! write optimized results into target_dsg_ only (see BackendModule::unmerged_graph_)
   DynamicSceneGraph::Ptr source_graph_;
   SharedDsgInfo::Ptr target_dsg_;
 };

src/backend/backend_module.cpp

@@ -260,11 +260,25 @@ bool BackendModule::spinOnce(bool force_update) {
   }
 
   timer.reset("backend/spin");
-  if ((config.optimize_on_lc && have_loopclosures_) || force_optimize_) {
+  if ((config.optimize_on_lc && have_new_loopclosures_) || force_optimize_) {
     optimize(timestamp_ns);
   } else {
     updateDsgMesh(timestamp_ns);
-    UpdateInfo::ConstPtr info(new UpdateInfo{timestamp_ns});
+    UpdateInfo::ConstPtr info;
+    if (have_loopclosures_) {
+      // no new factors to solve: deform new/active nodes and mesh with the cached
+      // optimized values; the full solve only runs when new loop closures arrive
+      info.reset(new UpdateInfo{timestamp_ns,
+                                deformation_graph_->getTempValues(),
+                                deformation_graph_->getValues(),
+                                false,
+                                {},
+                                deformation_graph_.get(),
+                                nullptr,
+                                mesh_offsets_});
+    } else {
+      info.reset(new UpdateInfo{timestamp_ns});
+    }
     dsg_updater_->callUpdateFunctions(timestamp_ns, info);
   }
 
@@ -555,8 +569,8 @@ void BackendModule::logStatus() {
   auto& status = status_log_.back();
   const auto& timer = hydra::timing::ElapsedTimeRecorder::instance();
   status.last_spin_s = timer.getLastElapsed("backend/spin");
-  status.last_opt_s = timer.getLastElapsed("backend/optimization");
-  status.last_mesh_update_s = timer.getLastElapsed("backend/mesh_update");
+  status.last_opt_s = timer.getLastElapsed("dsg_updater/optimization");
+  status.last_mesh_update_s = timer.getLastElapsed("backend/mesh_deformation");
 }
 
 }  // namespace hydra

src/backend/deformation_interpolator.cpp

@@ -60,6 +60,31 @@ std::string printTransform(const Eigen::Isometry3d& tf) {
 }  // namespace
 
 using spark_dsg::NodeSymbol;
+using spark_dsg::ObjectNodeAttributes;
+using spark_dsg::SemanticNodeAttributes;
+
+void applyNodeDeformation(const Eigen::Isometry3d& transform,
+                          const NodeAttributes& src,
+                          NodeAttributes& dst) {
+  dst.position = transform * src.position;
+
+  // transform a fresh copy of the odometric box (never the live optimized box) so
+  // repeated deformation of a node -- active or archived -- never compounds
+  const auto src_semantic = dynamic_cast<const SemanticNodeAttributes*>(&src);
+  auto dst_semantic = dynamic_cast<SemanticNodeAttributes*>(&dst);
+  if (src_semantic && dst_semantic &&
+      src_semantic->bounding_box.type != BoundingBox::Type::INVALID) {
+    dst_semantic->bounding_box = src_semantic->bounding_box;
+    dst_semantic->bounding_box.transform(transform);
+  }
+
+  const auto src_object = dynamic_cast<const ObjectNodeAttributes*>(&src);
+  auto dst_object = dynamic_cast<ObjectNodeAttributes*>(&dst);
+  if (src_object && dst_object) {
+    dst_object->world_R_object = Eigen::Quaterniond(
+        transform.linear() * src_object->world_R_object.toRotationMatrix());
+  }
+}
 
 void declare_config(DeformationInterpolator::Config& config) {
   using namespace config;
@@ -85,20 +110,31 @@ NodeCache::Entry* NodeCache::add(NodeId node_id, const NodeAttributes& attrs) {
 
   auto iter = nodes.find(node_id);
   if (iter == nodes.end()) {
-    return &nodes
-                .emplace(node_id,
-                         Entry{node_id,
-                               attrs.last_update_time_ns,
-                               attrs.position.cast<float>()})
-                .first->second;
+    iter =
+        nodes
+            .emplace(
+                node_id,
+                Entry{
+                    node_id, timestamp_ns, attrs.position.cast<float>(), std::nullopt})
+            .first;
+    return &iter->second;
   }
 
-  if (attrs.is_active) {
-    iter->second.init_pos = attrs.position.cast<float>();
-    iter->second.timestamp = attrs.last_update_time_ns;
+  // the attributes come from the unmerged graph, which is odometric by invariant,
+  // so the cached values can always refresh (archived nodes never change anyway)
+  iter->second.pos = attrs.position.cast<float>();
+  iter->second.timestamp = timestamp_ns;
+  return &iter->second;
+}
+
+bool NodeCache::applyLastTransform(NodeId node_id, NodeAttributes& attrs) const {
+  const auto iter = nodes.find(node_id);
+  if (iter == nodes.end() || !iter->second.last_transform) {
+    return false;
   }
 
-  return &iter->second;
+  attrs.transform(*iter->second.last_transform);
+  return true;
 }
 
 struct EntryList {
@@ -126,7 +162,7 @@ kimera_pgmo::traits::Pos pgmoGetVertex(const EntryList& entries,
     traits->stamp = entry->timestamp;
   }
 
-  return entry->init_pos;
+  return entry->pos;
 }
 
 uint64_t pgmoGetVertexStamp(const EntryList& entries, size_t i) {
@@ -189,14 +225,19 @@ void DeformationInterpolator::interpolate(const DynamicSceneGraph& unmerged,
       VLOG(5) << "node " << spark_dsg::NodeSymbol(entry->id).str()
               << " -> transform: " << printTransform(transform);
 
-      const auto new_pos = transform * entry->init_pos.cast<double>();
-      auto& attrs = unmerged.getNode(entry->id).attributes();
-      attrs.position = new_pos;
+      // recorded so merge hooks can bring attributes rebuilt from odometric
+      // constituents into the optimized frame
+      entry->last_transform = transform;
 
       auto node_ptr = dsg.findNode(entry->id);
-      if (node_ptr) {
-        node_ptr->attributes().position = new_pos;
+      if (!node_ptr) {
+        return;
       }
+
+      // the unmerged graph stays odometric: read the odometric source values and
+      // write the deformed results into the merged graph only
+      const auto& src = unmerged.getNode(entry->id).attributes();
+      applyNodeDeformation(transform, src, node_ptr->attributes());
     };
 
     dgraph.customDeformation(deform_func,

src/backend/dsg_updater.cpp

@@ -56,7 +56,9 @@ void findAndApplyMerges(const VerbosityConfig& config,
                         MergeTracker& tracker,
                         bool exhaustive) {
   // TODO(nathan) handle given merges
-  auto merges = hooks.find_merges(source, info);
+  // merges are found on the target (merged) graph: the source graph is odometric,
+  // and duplicates revealed by a loop closure only overlap in the optimized frame
+  auto merges = hooks.find_merges(*target.graph, info);
   auto applied = tracker.applyMerges(source, merges, target, hooks.merge);
   MLOG(1) << "pass 0: " << merges.size() << " merges (applied " << applied << ")";
   if (!exhaustive) {
@@ -164,14 +166,10 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i
     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
+  // active for at least one update. Runs before the graph merge so the nodes are
+  // also active in the target (merged) graph's views (e.g. for merge finding).
   std::vector<NodeId> active_nodes_to_restore;
   for (auto& [layer_id, layer] : source_graph_->layers()) {
     for (auto& [node_id, node] : layer->nodes()) {
@@ -183,6 +181,11 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i
     }
   }
 
+  GraphMergeConfig merge_config;
+  merge_config.previous_merges = &target_dsg_->merges;
+  merge_config.update_dynamic_attributes = false;
+  target_dsg_->graph->mergeGraph(*source_graph_, merge_config);
+
   const std::set<std::string> exhaustive_names(config.exhaustive_functors.begin(),
                                                config.exhaustive_functors.end());
   std::list<UpdateFunctor::Hooks::CleanupFunc> cleanup_hooks;
@@ -213,25 +216,33 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i
             *source_graph_, *target_dsg_->graph, hooks.merge);
       }
     }
+  }
 
-    // TODO(nathan) fix args for multithreading
-    MLOG(2) << "all merges: " << merge_tracker.print();
-    for (const auto& func : cleanup_hooks) {
-      func(info, *source_graph_, target_dsg_.get());
-    }
+  // TODO(nathan) fix args for multithreading
+  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;
-      }
+  // We reset active flags for all new nodes that were inactive after one update.
+  // The graph merge cloned the forced-active flag into the target graph, and the
+  // target copy is never re-cloned once the source node is archived again, so the
+  // flag has to be restored on both graphs.
+  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);
+    auto target_node = target_dsg_->graph->findNode(node_id);
+    if (target_node) {
+      target_node->attributes().is_active = false;
+    }
   }
+
+  // clear new node status
+  // TODO(nathan) add API for marking new nodes
+  source_graph_->getNewNodes(true);
 }
 
 }  // namespace hydra

src/backend/update_places_functor.cpp

@@ -116,9 +116,11 @@ size_t UpdatePlacesFunctor::updateFromValues(const LayerView& view,
 
     // TODO(nathan) consider updating distance via parents + deformation graph
     ++num_changed;
-    auto& attrs = node.attributes();
-    attrs.position = places_values.at<gtsam::Pose3>(node.id).translation();
-    dsg.graph->setNodeAttributes(node.id, attrs.clone());
+    // write the optimized position into the merged graph only; the unmerged view
+    // stays odometric
+    auto new_attrs = node.attributes().clone();
+    new_attrs->position = places_values.at<gtsam::Pose3>(node.id).translation();
+    dsg.graph->setNodeAttributes(node.id, std::move(new_attrs));
   }
 
   // TODO(nathan) fix this

src/backend/update_region_growing_traversability_functor.cpp

@@ -77,7 +77,14 @@ UpdateFunctor::Hooks UpdateRegionGrowingTraversabilityFunctor::hooks() const {
   };
   my_hooks.merge = [this](const DynamicSceneGraph& dsg,
                           const std::vector<NodeId>& merge_ids) {
-    return mergeNodes(dsg, merge_ids);
+    auto attrs = mergeNodes(dsg, merge_ids);
+    if (attrs) {
+      // the merged attributes are cloned from the odometric unmerged graph;
+      // re-apply the surviving node's last deformation so the merge result stays
+      // in the optimized frame
+      deformation_interpolator_.applyLastTransform(merge_ids.front(), *attrs);
+    }
+    return attrs;
   };
   my_hooks.cleanup = [this](const UpdateInfo::ConstPtr& info,
                             DynamicSceneGraph&,

tests/CMakeLists.txt

@@ -10,6 +10,8 @@ add_executable(
   test_${PROJECT_NAME}
   main.cpp
   src/resources.cpp
+  backend/test_deformation_interpolator.cpp
+  backend/test_dsg_updater.cpp
   backend/test_external_loop_closure.cpp
   backend/test_generic_update_functor.cpp
   backend/test_update_agents_functor.cpp