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

include/hydra/backend/deformation_interpolator.h

@@ -34,17 +34,22 @@
  * -------------------------------------------------------------------------- */
 #pragma once
 
-#include <memory>
-
 #include "hydra/backend/update_functions.h"
 
 namespace hydra {
 
 struct NodeCache {
   struct Entry {
+    //! Node entry ID
     NodeId id;
+    //! Associated update timestamp
     uint64_t timestamp;
+    //! Odometric node position
     Eigen::Vector3f init_pos;
+    //! Odometric bounding box for node
+    spark_dsg::BoundingBox init_bbox;
+
+    void update(NodeAttributes& attrs, const Eigen::Isometry3d& transform) const;
   };
 
   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;
@@ -70,6 +72,21 @@ void declare_config(DeformationInterpolator::Config& config) {
   check(config.control_point_tolerance_s, GE, 0.0, "control_point_tolerance_s");
 }
 
+void NodeCache::Entry::update(NodeAttributes& attrs,
+                              const Eigen::Isometry3d& transform) const {
+  const auto new_pos = transform * init_pos.cast<double>();
+  attrs.position = new_pos;
+  if (init_bbox.type == BoundingBox::Type::INVALID) {
+    return;
+  }
+
+  auto derived = dynamic_cast<SemanticNodeAttributes*>(&attrs);
+  if (derived) {
+    derived->bounding_box = init_bbox;
+    derived->bounding_box.transform(transform);
+  }
+}
+
 NodeCache::Entry* NodeCache::add(NodeId node_id, const NodeAttributes& attrs) {
   uint64_t timestamp_ns = attrs.last_update_time_ns;
   if (timestamp_ns == 0u) {
@@ -83,19 +100,25 @@ NodeCache::Entry* NodeCache::add(NodeId node_id, const NodeAttributes& attrs) {
     timestamp_ns = derived->last_observed_ns.back();
   }
 
+  BoundingBox bbox;
+  const auto derived = dynamic_cast<const SemanticNodeAttributes*>(&attrs);
+  if (derived) {
+    bbox = derived->bounding_box;  // Cache the original bounding box of the node
+  }
+
   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;
@@ -105,7 +128,7 @@ struct EntryList {
   std::vector<NodeCache::Entry*> entries;
 
   void sort() {
-    std::sort(entries.begin(), entries.end(), [this](const auto& lhs, const auto& rhs) {
+    std::sort(entries.begin(), entries.end(), [](const auto& lhs, const auto& rhs) {
       return lhs->timestamp < rhs->timestamp;
     });
   }
@@ -189,14 +212,13 @@ 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;
-
+      entry->update(unmerged.getNode(entry->id).attributes(), transform);
       auto node_ptr = dsg.findNode(entry->id);
-      if (node_ptr) {
-        node_ptr->attributes().position = new_pos;
+      if (!node_ptr) {
+        return;
       }
+
+      entry->update(node_ptr->attributes(), transform);
     };
 
     dgraph.customDeformation(deform_func,

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,56 @@
+#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::SemanticNodeAttributes;
+
+TEST(DeformationInterpolator, KhronosFallbackTimestampOnInsert) {
+  KhronosObjectAttributes attrs;
+  attrs.last_update_time_ns = 0;
+  attrs.last_observed_ns = {100};
+  attrs.is_active = true;
+
+  NodeCache cache;
+  auto entry = cache.add(0, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->timestamp, 100u);
+
+  entry = cache.add(0, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->timestamp, 100u);
+}
+
+TEST(DeformationInterpolator, BoundingBoxCorrect) {
+  const BoundingBox bbox1(Eigen::Vector3f(1.0f, 1.0f, 1.0f),
+                          Eigen::Vector3f(0.0f, 0.0f, 0.0f));
+  const BoundingBox bbox2(Eigen::Vector3f(2.0f, 2.0f, 2.0f),
+                          Eigen::Vector3f(3.0f, 3.0f, 3.0f));
+
+  SemanticNodeAttributes attrs;
+  attrs.last_update_time_ns = 10u;
+  attrs.is_active = true;
+  attrs.bounding_box = bbox1;
+
+  NodeCache cache;
+  auto entry = cache.add(0, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->init_bbox, bbox1);
+
+  attrs.bounding_box = bbox2;
+  entry = cache.add(0, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->init_bbox, bbox2);
+
+  attrs.is_active = false;
+  attrs.bounding_box = bbox1;
+  entry = cache.add(0, attrs);
+  ASSERT_NE(entry, nullptr);
+  EXPECT_EQ(entry->init_bbox, bbox2);
+}
+
+}  // namespace hydra