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 0f3d1147..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,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(&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()}) + .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 = 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(); - 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(&dst)) { + semantic->bounding_box = entry->init_bbox; + semantic->bounding_box.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..885d872e --- /dev/null +++ b/tests/backend/test_deformation_interpolator.cpp @@ -0,0 +1,93 @@ +#include +#include + +#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