fix(backend): deform khronos object pose and stamp nodes by observation time

include/hydra/backend/deformation_interpolator.h

@@ -45,6 +45,10 @@ struct NodeCache {
     NodeId id;
     uint64_t timestamp;
     Eigen::Vector3f init_pos;
+    //! Original (odometric) bounding box, INVALID if the node has none. Cached so
+    //! the deform callback can transform a fresh copy each spin rather than
+    //! mutating the box in place (which would compound on archived nodes).
+    spark_dsg::BoundingBox init_bbox;
   };
 
   Entry* add(NodeId node, const NodeAttributes& attrs);

src/backend/deformation_interpolator.cpp

@@ -59,7 +59,9 @@ std::string printTransform(const Eigen::Isometry3d& tf) {
 
 }  // namespace
 
+using spark_dsg::BoundingBox;
 using spark_dsg::NodeSymbol;
+using spark_dsg::SemanticNodeAttributes;
 
 void declare_config(DeformationInterpolator::Config& config) {
   using namespace config;
@@ -83,19 +85,26 @@ NodeCache::Entry* NodeCache::add(NodeId node_id, const NodeAttributes& attrs) {
     timestamp_ns = derived->last_observed_ns.back();
   }
 
+  // Cache the original bounding box (if any) so the deform callback can transform a
+  // fresh copy each spin instead of mutating the live box in place.
+  BoundingBox bbox;  // default INVALID
+  if (const auto semantic = dynamic_cast<const SemanticNodeAttributes*>(&attrs)) {
+    bbox = semantic->bounding_box;
+  }
+
   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>()})
+                .emplace(
+                    node_id,
+                    Entry{node_id, timestamp_ns, attrs.position.cast<float>(), bbox})
                 .first->second;
   }
 
   if (attrs.is_active) {
     iter->second.init_pos = attrs.position.cast<float>();
-    iter->second.timestamp = attrs.last_update_time_ns;
+    iter->second.timestamp = timestamp_ns;
+    iter->second.init_bbox = bbox;
   }
 
   return &iter->second;
@@ -190,12 +199,23 @@ void DeformationInterpolator::interpolate(const DynamicSceneGraph& unmerged,
               << " -> transform: " << printTransform(transform);
 
       const auto new_pos = transform * entry->init_pos.cast<double>();
-      auto& attrs = unmerged.getNode(entry->id).attributes();
-      attrs.position = new_pos;
+      unmerged.getNode(entry->id).attributes().position = new_pos;
 
       auto node_ptr = dsg.findNode(entry->id);
-      if (node_ptr) {
-        node_ptr->attributes().position = new_pos;
+      if (!node_ptr) {
+        return;
+      }
+
+      auto& dst = node_ptr->attributes();
+      dst.position = new_pos;
+
+      // Transform a fresh copy of the cached original box (never the live box) so
+      // repeated deformation of a node -- active or archived -- never compounds.
+      if (entry->init_bbox.type != BoundingBox::Type::INVALID) {
+        if (auto semantic = dynamic_cast<SemanticNodeAttributes*>(&dst)) {
+          semantic->bounding_box = entry->init_bbox;
+          semantic->bounding_box.transform(transform);
+        }
       }
     };
 

tests/CMakeLists.txt

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

tests/backend/test_deformation_interpolator.cpp

@@ -0,0 +1,93 @@
+#include <gtest/gtest.h>
+#include <spark_dsg/node_attributes.h>
+
+#include "hydra/backend/deformation_interpolator.h"
+
+namespace hydra {
+
+using spark_dsg::BoundingBox;
+using spark_dsg::KhronosObjectAttributes;
+using spark_dsg::NodeAttributes;
+using spark_dsg::SemanticNodeAttributes;
+
+// Khronos objects never populate last_update_time_ns (it stays 0), so NodeCache
+// must fall back to last_observed_ns. Otherwise the cached entry is stamped 0
+// and the deformation solver matches it against the earliest control points
+// instead of the ones near the object's observation time.
+TEST(NodeCache, KhronosFallbackTimestampOnInsert) {
+  constexpr uint64_t observed_ns = 1782847154557208075ULL;
+  KhronosObjectAttributes attrs;
+  attrs.last_update_time_ns = 0;
+  attrs.last_observed_ns = {observed_ns};
+  attrs.is_active = true;
+
+  NodeCache cache;
+  const NodeId node_id = 42;
+
+  auto* entry = cache.add(node_id, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->timestamp, observed_ns);
+
+  // re-adding an active node must keep the fallback stamp, not reset it to 0
+  entry = cache.add(node_id, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->timestamp, observed_ns);
+}
+
+// The deform callback transforms a copy of the cached original box rather than
+// mutating the live box in place, so NodeCache must capture the box for any node
+// that carries one (i.e. dynamic-casts to SemanticNodeAttributes).
+TEST(NodeCache, CachesBoundingBoxForSemanticNodes) {
+  SemanticNodeAttributes attrs;
+  attrs.last_update_time_ns = 10u;
+  attrs.is_active = true;
+  attrs.bounding_box =
+      BoundingBox(Eigen::Vector3f(1.0f, 2.0f, 3.0f), Eigen::Vector3f(4.0f, 5.0f, 6.0f));
+
+  NodeCache cache;
+  const NodeId node_id = 7;
+
+  auto* entry = cache.add(node_id, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->init_bbox.type, BoundingBox::Type::AABB);
+  EXPECT_TRUE(entry->init_bbox.dimensions.isApprox(attrs.bounding_box.dimensions));
+  EXPECT_TRUE(
+      entry->init_bbox.world_P_center.isApprox(attrs.bounding_box.world_P_center));
+}
+
+// Re-adding an active node must refresh the cached box to the current frontend
+// original (mergeGraph re-clones the undeformed box onto active nodes each cycle).
+TEST(NodeCache, RefreshesCachedBoundingBoxWhileActive) {
+  SemanticNodeAttributes attrs;
+  attrs.last_update_time_ns = 10u;
+  attrs.is_active = true;
+  attrs.bounding_box =
+      BoundingBox(Eigen::Vector3f(1.0f, 1.0f, 1.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f));
+
+  NodeCache cache;
+  const NodeId node_id = 8;
+  cache.add(node_id, attrs);
+
+  attrs.bounding_box =
+      BoundingBox(Eigen::Vector3f(2.0f, 2.0f, 2.0f), Eigen::Vector3f(3.0f, 3.0f, 3.0f));
+  auto* entry = cache.add(node_id, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_TRUE(entry->init_bbox.dimensions.isApprox(attrs.bounding_box.dimensions));
+  EXPECT_TRUE(
+      entry->init_bbox.world_P_center.isApprox(attrs.bounding_box.world_P_center));
+}
+
+// Nodes without a bounding box (plain NodeAttributes, e.g. traversability places)
+// leave the cached box INVALID, so the deform callback skips the box entirely.
+TEST(NodeCache, LeavesBoundingBoxInvalidForNonSemanticNodes) {
+  NodeAttributes attrs;
+  attrs.last_update_time_ns = 10u;
+  attrs.is_active = true;
+
+  NodeCache cache;
+  auto* entry = cache.add(9, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->init_bbox.type, BoundingBox::Type::INVALID);
+}
+
+}  // namespace hydra