fix(backend): deform khronos object pose and stamp nodes by observation time by harelb · Pull Request #153 · MIT-SPARK/Hydra

harelb · 2026-07-06T19:16:36Z

Found while working on the image-keyframing code. Two related bugs in DeformationInterpolator (added in #115) that leave Khronos objects mis-placed after loop-closure optimization — object positions and their 3D bounding boxes end up floating off the deformed mesh.

1. `NodeCache::add` discards the resolved timestamp

NodeCache::add computes a fallback timestamp from last_observed_ns when last_update_time_ns == 0 — which is always the case for Khronos objects — but then stores attrs.last_update_time_ns (0) in the cache Entry instead of the computed timestamp_ns (both the emplace path and the active-refresh path).

Every object is therefore stamped 0, so customDeformation matches it against the earliest control points (trajectory start) instead of the control points near its observation time. The placement error grows with how late the object was observed — on an infinite-corridor bag, error-vs-observation-time correlated at Pearson r = +0.79 (late objects ~28 m off, well beyond a 10 m camera range).

Fix: stamp the entry with the resolved timestamp_ns.

2. `interpolate()` only moved the position, not the bounding box

The deform callback set node.position but never touched the object bounding box, so once the positions were corrected the 3D boxes still floated at the un-deformed frontend pose (bbox_center up to ~20 m from the corrected position).

Fix: cache each node's original bounding box in the NodeCache entry (for nodes that dynamic_cast to SemanticNodeAttributes), and on each deform spin write a fresh copy of the cached box into the node and transform that copy; position is likewise recomputed from the cached init_pos. Because nothing is transformed in place, deformation is idempotent for active and archived nodes alike — an archived node revisited on a later loop-closure spin no longer double-applies the transform. (The earlier in-place transform() compounded on archived nodes, driving the traversability places that share this interpolator to drift to z ~40 m.) Only position + bounding box are touched, which keeps the deformation easy to reason about.

Notes / follow-ups

Scoped to position + bounding box via the SemanticNodeAttributes cast. ObjectNodeAttributes::world_R_object (only meaningful when registered) and Khronos world-frame trajectory_positions / dynamic_object_points (empty for static objects) are not transformed; a KhronosObjectAttributes::transform override is the natural follow-up. The object-local mesh is stored relative to the bbox origin, so it rides the bbox automatically.

Testing

Fix 1: object positions snap onto the mesh (r +0.79 → +0.07; 0/144 objects >2 m off).
Fix 2, validated end-to-end on the infinite-corridor bag: bbox_center → mesh median 0.065 m, 0/144 >2 m (previously 3.58 m median / 99 of 144 off); archived traversability places z median 0.97 m / max 3.66 m (previously 17.4 m / 39.8 m). The existing GenericUpdateFunctor.shouldUpdate idempotency test stays green.
Added NodeCache regression tests: the timestamp fallback, and the bounding-box caching (cached on insert + refreshed while active; stays INVALID for non-semantic nodes).

… object pose Two related bugs in DeformationInterpolator that leave Khronos objects mis-placed after loop-closure optimization. 1. NodeCache::add computed a valid fallback timestamp from last_observed_ns when last_update_time_ns == 0 (always the case for Khronos objects), but then stored last_update_time_ns (0) in the cache entry instead of the computed value. Every object was therefore stamped 0, so customDeformation matched them against the earliest control points (trajectory start) rather than the control points near their observation time. Placement error grew with observation time (Pearson r = +0.79 on an infinite-corridor bag); now stamp entries with the resolved timestamp. 2. interpolate() only updated node.position, never the object bounding box or orientation, so the 3D boxes stayed at the un-deformed frontend pose even once positions were corrected. Apply attrs.transform() instead: it moves position + bounding box + orientation together. The backend dsg node is reset to the frontend-original attributes by mergeGraph earlier in the cycle (for active nodes), so this reproduces the previous position result for the position component. Adds a NodeCache regression test for the timestamp fallback.

harelb · 2026-07-06T20:16:10Z

      if (node_ptr) {
-        node_ptr->attributes().position = new_pos;
+        // move position + bounding box + orientation)
+        node_ptr->attributes().transform(transform);


@nathanhhughes Don't merge/review this yet. This part works for objects but is problematic for place nodes

yeah, transform isn't really useable given how the backend works right now (it would work better if transform wasn't in-place or if the original odometric attributes were always available). I think you're better off just adding the original bounding boxes to the node cache if the nodes can be dynamically cast to SemanticNodeAttributes and adding the transformed version if they exist in the node cache, there's not really a good fix otherwise without burning a lot of memory

transform() was applied to the node's current attributes, which compounds when the same node is deformed on successive loop-closure spins (archived nodes are not reset to their frontend-original pose by mergeGraph). This drove archived traversability places to drift (z up to ~40m) and broke the existing GenericUpdateFunctor.shouldUpdate idempotency test. Reset position to the cached init_pos before transform() so active and archived nodes alike land at transform * original every spin.

harelb · 2026-07-07T01:04:28Z

+        // repeated deformation of an archived node stays idempotent (otherwise the
+        // transform compounds across loop-closure spins). transform() also moves
+        // the bounding box + orientation.
+        auto& dst = node_ptr->attributes();


@nathanhhughes What do you think of this? I think it will only have an issue if Khronos objects are set to is_active=False. In that case a double transformation would happen but right now it looks like is_active is always set to true

They technically should get turned inactive once their parent in the places layer gets archived (though it is configurable and you might have that behavior turned off). I think I prefer my solution for now (throwing a bounding box in the node cache and transforming a copy of it and adding it to the node attributes if the bounding box type isn't invalid) because it's easier to reason about correctness (because transform can touch a lot of the attribute fields), I can maybe try and draft something quick this afternoon.

The actual solution that probably isn't worth doing right now is to either:

Cache the last transform applied to the node and apply the difference when calling transform

(my preference / what I had been working on slowly) Rework transform to not be in-place and maintain an unmerged, odometric copy of the scene graph (and a merged, optimized copy of the scene graph) instead of the optimized+unmerged and optimized+merged copies of the scene graph that the backend currently has

Mirrors the upstream Hydra PR fixes onto the build branch: - deformation_interpolator: stamp cache entries with the computed timestamp_ns (not last_update_time_ns, which is 0 for objects) and reset to init_pos before transform() so repeated deformation of archived nodes stays idempotent (upstream PR MIT-SPARK#153). - region_growing_traversability_clustering: erase merged/stale neighbors in place with the loop iterator instead of erasing inside Region::merge, removing the dangling-reference hazard (upstream PR MIT-SPARK#152).

Cache each node's original bounding box in NodeCache and transform a fresh copy into the node on every deform spin, instead of resetting position and calling the in-place transform(). transform() mutates many attribute fields and, on archived nodes (which mergeGraph does not reset to the frontend original), compounded the bounding box across loop-closure spins even after the position reset. Caching the original box makes deformation idempotent for active and archived nodes alike and touches only position + bounding box. Verified end-to-end on the infinite-corridor bag: objects and their boxes land on the mesh (bbox_center -> mesh median 0.065 m, 0/144 > 2 m; previously 3.58 m median / 99 of 144 off) and archived traversability places no longer drift (z median 0.97 m, max 3.66 m; previously 17.4 m median / 39.8 m max).

…#153 rework) Replace the reset + in-place transform() deform approach with caching each node's original bounding box in NodeCache and transforming a fresh copy on every spin, so deformation is idempotent for active and archived nodes and touches only position + bounding box. Mirrors the reworked fix/deformation-interpolator-object-pose branch (PR MIT-SPARK#153) to keep this build copy in sync. Adds NodeCache bbox-caching regression tests + CMakeLists entry.

harelb · 2026-07-08T20:34:23Z

Heads-up: #157 supersedes this approach structurally (keeps the unmerged graph odometric so deformation can never compound, rather than caching per-node copies). Both fix the object float-off-mesh symptom; happy to close this one in favor of #157 if that direction is preferred.

nathanhhughes

Thanks for tackling this! The general approach is sound, but the actual behavior needs to be fixed

nathanhhughes · 2026-07-08T20:41:28Z

+      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);
+        }
      }


I like this behavior better than calling transform, but technically the unmerged graph should also have the bounding box updated (which is initially used to detect merges)

nathanhhughes · 2026-07-08T20:43:51Z

+// 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) {


Didn't catch this the first time around, but the suite name should be DeformationInterpolator (I'm slowly getting around to making all the names uniform to match the filename).

nathanhhughes · 2026-07-08T20:47:13Z

+// 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);
+}


These tests aren't super useful and I would be in favor of dropping them (given that the node cache is a temporary solution). If it tested the interpolator in a more end-to-end manner (i.e., making sure that active and archived nodes got updated attributes that were correct), I'd be in favor but also not super urgent to write until the backend actually gets fixed

nathanhhughes · 2026-07-08T20:49:22Z

+      // 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)) {


Also forget to include this in my review, but I really don't like the inline variable initialization here

harelb added 2 commits July 6, 2026 15:16

comment cleanup

4ab2d69

harelb commented Jul 6, 2026

View reviewed changes

harelb commented Jul 7, 2026

View reviewed changes

harelb marked this pull request as ready for review July 7, 2026 01:04

harelb marked this pull request as draft July 7, 2026 19:56

nathanhhughes approved these changes Jul 8, 2026

View reviewed changes

nathanhhughes reviewed Jul 8, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

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

fix(backend): deform khronos object pose and stamp nodes by observation time#153
harelb wants to merge 4 commits into
MIT-SPARK:developfrom
harelb:fix/deformation-interpolator-object-pose

harelb commented Jul 6, 2026 •

edited

Loading

Uh oh!

harelb Jul 6, 2026

Uh oh!

nathanhhughes Jul 6, 2026

Uh oh!

harelb Jul 7, 2026

Uh oh!

nathanhhughes Jul 7, 2026

Uh oh!

harelb commented Jul 8, 2026

Uh oh!

nathanhhughes left a comment

Uh oh!

nathanhhughes Jul 8, 2026

Uh oh!

nathanhhughes Jul 8, 2026

Uh oh!

nathanhhughes Jul 8, 2026

Uh oh!

nathanhhughes Jul 8, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Uh oh!

Conversation

harelb commented Jul 6, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

1. NodeCache::add discards the resolved timestamp

2. interpolate() only moved the position, not the bounding box

Notes / follow-ups

Testing

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

harelb commented Jul 8, 2026

Uh oh!

nathanhhughes left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

harelb commented Jul 6, 2026 •

edited

Loading

1. `NodeCache::add` discards the resolved timestamp

2. `interpolate()` only moved the position, not the bounding box