diff --git a/include/hydra/backend/deformation_interpolator.h b/include/hydra/backend/deformation_interpolator.h index adcd5e1c..1a0b5f79 100644 --- a/include/hydra/backend/deformation_interpolator.h +++ b/include/hydra/backend/deformation_interpolator.h @@ -34,17 +34,22 @@ * -------------------------------------------------------------------------- */ #pragma once -#include - #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); diff --git a/src/backend/deformation_interpolator.cpp b/src/backend/deformation_interpolator.cpp index 0f3d1147..7f3accf3 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; @@ -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(); + attrs.position = new_pos; + if (init_bbox.type == BoundingBox::Type::INVALID) { + return; + } + + auto derived = dynamic_cast(&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(&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()}) + .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; @@ -105,7 +128,7 @@ struct EntryList { std::vector 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(); - 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, 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..d32a2317 --- /dev/null +++ b/tests/backend/test_deformation_interpolator.cpp @@ -0,0 +1,56 @@ +#include +#include + +#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