From 9f6edf74ea5b4f95be5165778b5b23fc55bea163 Mon Sep 17 00:00:00 2001 From: harelb Date: Mon, 6 Jul 2026 15:16:12 -0400 Subject: [PATCH 1/4] fix(backend): stamp khronos nodes by observation time and deform full 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. --- src/backend/deformation_interpolator.cpp | 9 +++-- tests/CMakeLists.txt | 1 + .../backend/test_deformation_interpolator.cpp | 34 +++++++++++++++++++ 3 files changed, 41 insertions(+), 3 deletions(-) create mode 100644 tests/backend/test_deformation_interpolator.cpp diff --git a/src/backend/deformation_interpolator.cpp b/src/backend/deformation_interpolator.cpp index 0f3d1147..84f8095f 100644 --- a/src/backend/deformation_interpolator.cpp +++ b/src/backend/deformation_interpolator.cpp @@ -88,14 +88,14 @@ NodeCache::Entry* NodeCache::add(NodeId node_id, const NodeAttributes& attrs) { return &nodes .emplace(node_id, Entry{node_id, - attrs.last_update_time_ns, + timestamp_ns, attrs.position.cast()}) .first->second; } if (attrs.is_active) { iter->second.init_pos = attrs.position.cast(); - iter->second.timestamp = attrs.last_update_time_ns; + iter->second.timestamp = timestamp_ns; } return &iter->second; @@ -195,7 +195,10 @@ void DeformationInterpolator::interpolate(const DynamicSceneGraph& unmerged, auto node_ptr = dsg.findNode(entry->id); if (node_ptr) { - node_ptr->attributes().position = new_pos; + // move the full pose (position + bounding box + orientation), not just + // position; dsg holds the frontend-original this cycle so this matches + // new_pos for the position component + node_ptr->attributes().transform(transform); } }; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 67315da1..4f301179 100644 --- a/tests/CMakeLists.txt +++ b/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 diff --git a/tests/backend/test_deformation_interpolator.cpp b/tests/backend/test_deformation_interpolator.cpp new file mode 100644 index 00000000..6dd332de --- /dev/null +++ b/tests/backend/test_deformation_interpolator.cpp @@ -0,0 +1,34 @@ +#include +#include + +#include "hydra/backend/deformation_interpolator.h" + +namespace hydra { + +using spark_dsg::KhronosObjectAttributes; + +// 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); +} + +} // namespace hydra From 4ab2d69a20a714fb0236c5318435879c7314eaff Mon Sep 17 00:00:00 2001 From: harelb Date: Mon, 6 Jul 2026 15:34:45 -0400 Subject: [PATCH 2/4] comment cleanup --- src/backend/deformation_interpolator.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/backend/deformation_interpolator.cpp b/src/backend/deformation_interpolator.cpp index 84f8095f..10d8c708 100644 --- a/src/backend/deformation_interpolator.cpp +++ b/src/backend/deformation_interpolator.cpp @@ -87,9 +87,7 @@ NodeCache::Entry* NodeCache::add(NodeId node_id, const NodeAttributes& attrs) { if (iter == nodes.end()) { return &nodes .emplace(node_id, - Entry{node_id, - timestamp_ns, - attrs.position.cast()}) + Entry{node_id, timestamp_ns, attrs.position.cast()}) .first->second; } @@ -195,9 +193,7 @@ void DeformationInterpolator::interpolate(const DynamicSceneGraph& unmerged, auto node_ptr = dsg.findNode(entry->id); if (node_ptr) { - // move the full pose (position + bounding box + orientation), not just - // position; dsg holds the frontend-original this cycle so this matches - // new_pos for the position component + // move position + bounding box + orientation) node_ptr->attributes().transform(transform); } }; From 2fb52d4270fe53a3287574310a7a98b984c39bff Mon Sep 17 00:00:00 2001 From: harelb Date: Mon, 6 Jul 2026 16:20:49 -0400 Subject: [PATCH 3/4] fix: reset node pose before deform so interpolation is idempotent 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. --- src/backend/deformation_interpolator.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/backend/deformation_interpolator.cpp b/src/backend/deformation_interpolator.cpp index 10d8c708..a4bc83e7 100644 --- a/src/backend/deformation_interpolator.cpp +++ b/src/backend/deformation_interpolator.cpp @@ -193,8 +193,13 @@ void DeformationInterpolator::interpolate(const DynamicSceneGraph& unmerged, auto node_ptr = dsg.findNode(entry->id); if (node_ptr) { - // move position + bounding box + orientation) - node_ptr->attributes().transform(transform); + // Reset to the cached original position before applying the transform so + // 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(); + dst.position = entry->init_pos.cast(); + dst.transform(transform); } }; From 4477bc40046b929e77adf02180a27adc708c0464 Mon Sep 17 00:00:00 2001 From: harelb Date: Tue, 7 Jul 2026 15:55:50 -0400 Subject: [PATCH 4/4] fix(backend): deform object bbox via cached copy, not in-place transform 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). --- .../hydra/backend/deformation_interpolator.h | 4 ++ src/backend/deformation_interpolator.cpp | 40 +++++++++---- .../backend/test_deformation_interpolator.cpp | 59 +++++++++++++++++++ 3 files changed, 91 insertions(+), 12 deletions(-) diff --git a/include/hydra/backend/deformation_interpolator.h b/include/hydra/backend/deformation_interpolator.h index adcd5e1c..78e73049 100644 --- a/include/hydra/backend/deformation_interpolator.h +++ b/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); diff --git a/src/backend/deformation_interpolator.cpp b/src/backend/deformation_interpolator.cpp index a4bc83e7..b8ab272e 100644 --- a/src/backend/deformation_interpolator.cpp +++ b/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,17 +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(&attrs)) { + bbox = semantic->bounding_box; + } + auto iter = nodes.find(node_id); if (iter == nodes.end()) { return &nodes - .emplace(node_id, - Entry{node_id, timestamp_ns, attrs.position.cast()}) + .emplace( + node_id, + Entry{node_id, timestamp_ns, attrs.position.cast(), bbox}) .first->second; } if (attrs.is_active) { iter->second.init_pos = attrs.position.cast(); iter->second.timestamp = timestamp_ns; + iter->second.init_bbox = bbox; } return &iter->second; @@ -188,18 +199,23 @@ void DeformationInterpolator::interpolate(const DynamicSceneGraph& unmerged, << " -> transform: " << printTransform(transform); const auto new_pos = transform * entry->init_pos.cast(); - 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) { - // Reset to the cached original position before applying the transform so - // 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(); - dst.position = entry->init_pos.cast(); - dst.transform(transform); + 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(&dst)) { + semantic->bounding_box = entry->init_bbox; + semantic->bounding_box.transform(transform); + } } }; diff --git a/tests/backend/test_deformation_interpolator.cpp b/tests/backend/test_deformation_interpolator.cpp index 6dd332de..885d872e 100644 --- a/tests/backend/test_deformation_interpolator.cpp +++ b/tests/backend/test_deformation_interpolator.cpp @@ -5,7 +5,10 @@ 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 @@ -31,4 +34,60 @@ TEST(NodeCache, KhronosFallbackTimestampOnInsert) { 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