diff --git a/include/hydra/backend/backend_module.h b/include/hydra/backend/backend_module.h index f9f2e182..61cf4a19 100644 --- a/include/hydra/backend/backend_module.h +++ b/include/hydra/backend/backend_module.h @@ -162,6 +162,12 @@ class BackendModule : public kimera_pgmo::KimeraPgmoInterface, public Module { uint64_t last_sequence_number_ = 0; SharedDsgInfo::Ptr private_dsg_; + //! Unmerged copy of the scene graph holding ODOMETRIC (never optimized) poses. + //! The merged private_dsg_ is derived from it via per-node odometric->optimized + //! transforms every optimize spin (have_loopclosures_ latches, so post-LC every + //! spin re-optimizes and re-deforms the active merged nodes that mergeGraph reset + //! to odometric clones). Nodes the interpolator skips (control-point matching + //! failure) or layers without a deform functor stay odometric while active. DynamicSceneGraph::Ptr unmerged_graph_; SharedModuleState::Ptr state_; diff --git a/include/hydra/backend/deformation_interpolator.h b/include/hydra/backend/deformation_interpolator.h index adcd5e1c..e765f2f9 100644 --- a/include/hydra/backend/deformation_interpolator.h +++ b/include/hydra/backend/deformation_interpolator.h @@ -35,20 +35,47 @@ #pragma once #include +#include #include "hydra/backend/update_functions.h" namespace hydra { +/** + * @brief Write the deformed (optimized) version of the odometric source attributes + * into the destination attributes. + * + * Only spatial fields are written (position, bounding box, object orientation), so + * merged-only state on the destination survives. Everything is derived from the + * odometric source, never from the destination, so repeated application with the + * same transform is idempotent. + */ +void applyNodeDeformation(const Eigen::Isometry3d& transform, + const NodeAttributes& src, + NodeAttributes& dst); + struct NodeCache { struct Entry { NodeId id; uint64_t timestamp; - Eigen::Vector3f init_pos; + //! Odometric position (the unmerged graph is odometric by invariant); doubles + //! as the pgmo vertex for control-point matching. + Eigen::Vector3f pos; + //! Most recent odometric->optimized transform applied to this node, if any. + //! Lets merge hooks bring attributes rebuilt from odometric constituents into + //! the optimized frame. + std::optional last_transform; }; Entry* add(NodeId node, const NodeAttributes& attrs); + /** + * @brief Apply the node's most recent deformation transform to the given + * attributes (via NodeAttributes::transform). + * @returns true if a transform was recorded and applied. + */ + bool applyLastTransform(NodeId node, NodeAttributes& attrs) const; + std::map nodes; }; @@ -68,10 +95,14 @@ class DeformationInterpolator { explicit DeformationInterpolator(const Config& config); /** - * @brief Interpolate the node positions based on the deformation graph, using the + * @brief Interpolate the node poses based on the deformation graph, using the * temporally closest control points as reference. * - * @param unmerged Unmerged scene graph to keep up to date. + * Reads odometric values from the unmerged scene graph and writes the deformed + * (optimized) values into the private DSG only; the unmerged graph is never + * modified. + * + * @param unmerged Unmerged (odometric) scene graph to read from. * @param dsg Private DSG to update. * @param info Update information containing deformation graph. * @param view View on the unmerged scene graph selecting all nodes to update. @@ -81,15 +112,7 @@ class DeformationInterpolator { const UpdateInfo::ConstPtr& info, const LayerView& view) const; - /** - * @brief Interpolate the node positions based on the deformation graph, using the - * temporally closest control points as reference. - * - * @param unmerged Unmerged scene graph to keep up to date. - * @param dsg Private DSG to update. - * @param info Update information containing deformation graph. - * @param view View on the unmerged scene graph selecting all nodes to update. - */ + //! @copydoc DeformationInterpolator::interpolate void interpolateNodePositions(const DynamicSceneGraph& unmerged, DynamicSceneGraph& dsg, const UpdateInfo::ConstPtr& info, @@ -97,6 +120,16 @@ class DeformationInterpolator { interpolate(unmerged, dsg, info, view); } + /** + * @brief Apply the node's most recent deformation transform to the given + * attributes (e.g. to bring merge-hook output rebuilt from odometric + * constituents into the optimized frame). + * @returns true if a transform was recorded and applied. + */ + bool applyLastTransform(NodeId node, NodeAttributes& attrs) const { + return cache_.applyLastTransform(node, attrs); + } + private: mutable NodeCache cache_; }; diff --git a/include/hydra/backend/dsg_updater.h b/include/hydra/backend/dsg_updater.h index ada7e8fa..f7bd1856 100644 --- a/include/hydra/backend/dsg_updater.h +++ b/include/hydra/backend/dsg_updater.h @@ -95,6 +95,8 @@ class DsgUpdater { GroupedMergeTracker merge_tracker; std::map update_functors_; + //! Source (unmerged) graph holding odometric poses; update functors read it and + //! write optimized results into target_dsg_ only (see BackendModule::unmerged_graph_) DynamicSceneGraph::Ptr source_graph_; SharedDsgInfo::Ptr target_dsg_; }; diff --git a/src/backend/backend_module.cpp b/src/backend/backend_module.cpp index bce80d9e..586d2c49 100644 --- a/src/backend/backend_module.cpp +++ b/src/backend/backend_module.cpp @@ -260,11 +260,25 @@ bool BackendModule::spinOnce(bool force_update) { } timer.reset("backend/spin"); - if ((config.optimize_on_lc && have_loopclosures_) || force_optimize_) { + if ((config.optimize_on_lc && have_new_loopclosures_) || force_optimize_) { optimize(timestamp_ns); } else { updateDsgMesh(timestamp_ns); - UpdateInfo::ConstPtr info(new UpdateInfo{timestamp_ns}); + UpdateInfo::ConstPtr info; + if (have_loopclosures_) { + // no new factors to solve: deform new/active nodes and mesh with the cached + // optimized values; the full solve only runs when new loop closures arrive + info.reset(new UpdateInfo{timestamp_ns, + deformation_graph_->getTempValues(), + deformation_graph_->getValues(), + false, + {}, + deformation_graph_.get(), + nullptr, + mesh_offsets_}); + } else { + info.reset(new UpdateInfo{timestamp_ns}); + } dsg_updater_->callUpdateFunctions(timestamp_ns, info); } @@ -555,8 +569,8 @@ void BackendModule::logStatus() { auto& status = status_log_.back(); const auto& timer = hydra::timing::ElapsedTimeRecorder::instance(); status.last_spin_s = timer.getLastElapsed("backend/spin"); - status.last_opt_s = timer.getLastElapsed("backend/optimization"); - status.last_mesh_update_s = timer.getLastElapsed("backend/mesh_update"); + status.last_opt_s = timer.getLastElapsed("dsg_updater/optimization"); + status.last_mesh_update_s = timer.getLastElapsed("backend/mesh_deformation"); } } // namespace hydra diff --git a/src/backend/deformation_interpolator.cpp b/src/backend/deformation_interpolator.cpp index 0f3d1147..e9cbd095 100644 --- a/src/backend/deformation_interpolator.cpp +++ b/src/backend/deformation_interpolator.cpp @@ -60,6 +60,31 @@ std::string printTransform(const Eigen::Isometry3d& tf) { } // namespace using spark_dsg::NodeSymbol; +using spark_dsg::ObjectNodeAttributes; +using spark_dsg::SemanticNodeAttributes; + +void applyNodeDeformation(const Eigen::Isometry3d& transform, + const NodeAttributes& src, + NodeAttributes& dst) { + dst.position = transform * src.position; + + // transform a fresh copy of the odometric box (never the live optimized box) so + // repeated deformation of a node -- active or archived -- never compounds + const auto src_semantic = dynamic_cast(&src); + auto dst_semantic = dynamic_cast(&dst); + if (src_semantic && dst_semantic && + src_semantic->bounding_box.type != BoundingBox::Type::INVALID) { + dst_semantic->bounding_box = src_semantic->bounding_box; + dst_semantic->bounding_box.transform(transform); + } + + const auto src_object = dynamic_cast(&src); + auto dst_object = dynamic_cast(&dst); + if (src_object && dst_object) { + dst_object->world_R_object = Eigen::Quaterniond( + transform.linear() * src_object->world_R_object.toRotationMatrix()); + } +} void declare_config(DeformationInterpolator::Config& config) { using namespace config; @@ -85,20 +110,31 @@ NodeCache::Entry* NodeCache::add(NodeId node_id, const NodeAttributes& attrs) { 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()}) - .first->second; + iter = + nodes + .emplace( + node_id, + Entry{ + node_id, timestamp_ns, attrs.position.cast(), std::nullopt}) + .first; + return &iter->second; } - if (attrs.is_active) { - iter->second.init_pos = attrs.position.cast(); - iter->second.timestamp = attrs.last_update_time_ns; + // the attributes come from the unmerged graph, which is odometric by invariant, + // so the cached values can always refresh (archived nodes never change anyway) + iter->second.pos = attrs.position.cast(); + iter->second.timestamp = timestamp_ns; + return &iter->second; +} + +bool NodeCache::applyLastTransform(NodeId node_id, NodeAttributes& attrs) const { + const auto iter = nodes.find(node_id); + if (iter == nodes.end() || !iter->second.last_transform) { + return false; } - return &iter->second; + attrs.transform(*iter->second.last_transform); + return true; } struct EntryList { @@ -126,7 +162,7 @@ kimera_pgmo::traits::Pos pgmoGetVertex(const EntryList& entries, traits->stamp = entry->timestamp; } - return entry->init_pos; + return entry->pos; } uint64_t pgmoGetVertexStamp(const EntryList& entries, size_t i) { @@ -189,14 +225,19 @@ 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; + // recorded so merge hooks can bring attributes rebuilt from odometric + // constituents into the optimized frame + entry->last_transform = transform; auto node_ptr = dsg.findNode(entry->id); - if (node_ptr) { - node_ptr->attributes().position = new_pos; + if (!node_ptr) { + return; } + + // the unmerged graph stays odometric: read the odometric source values and + // write the deformed results into the merged graph only + const auto& src = unmerged.getNode(entry->id).attributes(); + applyNodeDeformation(transform, src, node_ptr->attributes()); }; dgraph.customDeformation(deform_func, diff --git a/src/backend/dsg_updater.cpp b/src/backend/dsg_updater.cpp index 2e721a17..3e9123c2 100644 --- a/src/backend/dsg_updater.cpp +++ b/src/backend/dsg_updater.cpp @@ -56,7 +56,9 @@ void findAndApplyMerges(const VerbosityConfig& config, MergeTracker& tracker, bool exhaustive) { // TODO(nathan) handle given merges - auto merges = hooks.find_merges(source, info); + // merges are found on the target (merged) graph: the source graph is odometric, + // and duplicates revealed by a loop closure only overlap in the optimized frame + auto merges = hooks.find_merges(*target.graph, info); auto applied = tracker.applyMerges(source, merges, target, hooks.merge); MLOG(1) << "pass 0: " << merges.size() << " merges (applied " << applied << ")"; if (!exhaustive) { @@ -164,14 +166,10 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i resetBackendDsg(timestamp_ns); } - GraphMergeConfig merge_config; - merge_config.previous_merges = &target_dsg_->merges; - merge_config.update_dynamic_attributes = false; - target_dsg_->graph->mergeGraph(*source_graph_, merge_config); - // Nodes occasionally get added to the backend after they've left the active window, // which means they never get deformed or updated correctly. This forces them to be - // active for at least one update + // active for at least one update. Runs before the graph merge so the nodes are + // also active in the target (merged) graph's views (e.g. for merge finding). std::vector active_nodes_to_restore; for (auto& [layer_id, layer] : source_graph_->layers()) { for (auto& [node_id, node] : layer->nodes()) { @@ -183,6 +181,11 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i } } + GraphMergeConfig merge_config; + merge_config.previous_merges = &target_dsg_->merges; + merge_config.update_dynamic_attributes = false; + target_dsg_->graph->mergeGraph(*source_graph_, merge_config); + const std::set exhaustive_names(config.exhaustive_functors.begin(), config.exhaustive_functors.end()); std::list cleanup_hooks; @@ -213,25 +216,33 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i *source_graph_, *target_dsg_->graph, hooks.merge); } } + } - // TODO(nathan) fix args for multithreading - MLOG(2) << "all merges: " << merge_tracker.print(); - for (const auto& func : cleanup_hooks) { - func(info, *source_graph_, target_dsg_.get()); - } + // TODO(nathan) fix args for multithreading + MLOG(2) << "all merges: " << merge_tracker.print(); + for (const auto& func : cleanup_hooks) { + func(info, *source_graph_, target_dsg_.get()); + } - // We reset active flags for all new nodes that were inactive after one update - for (auto node_id : active_nodes_to_restore) { - auto node = source_graph_->findNode(node_id); - if (node) { - node->attributes().is_active = false; - } + // We reset active flags for all new nodes that were inactive after one update. + // The graph merge cloned the forced-active flag into the target graph, and the + // target copy is never re-cloned once the source node is archived again, so the + // flag has to be restored on both graphs. + for (auto node_id : active_nodes_to_restore) { + auto node = source_graph_->findNode(node_id); + if (node) { + node->attributes().is_active = false; } - // clear new node status - // TODO(nathan) add API for marking new nodes - source_graph_->getNewNodes(true); + auto target_node = target_dsg_->graph->findNode(node_id); + if (target_node) { + target_node->attributes().is_active = false; + } } + + // clear new node status + // TODO(nathan) add API for marking new nodes + source_graph_->getNewNodes(true); } } // namespace hydra diff --git a/src/backend/update_places_functor.cpp b/src/backend/update_places_functor.cpp index cc508298..29b1eba6 100644 --- a/src/backend/update_places_functor.cpp +++ b/src/backend/update_places_functor.cpp @@ -116,9 +116,11 @@ size_t UpdatePlacesFunctor::updateFromValues(const LayerView& view, // TODO(nathan) consider updating distance via parents + deformation graph ++num_changed; - auto& attrs = node.attributes(); - attrs.position = places_values.at(node.id).translation(); - dsg.graph->setNodeAttributes(node.id, attrs.clone()); + // write the optimized position into the merged graph only; the unmerged view + // stays odometric + auto new_attrs = node.attributes().clone(); + new_attrs->position = places_values.at(node.id).translation(); + dsg.graph->setNodeAttributes(node.id, std::move(new_attrs)); } // TODO(nathan) fix this diff --git a/src/backend/update_region_growing_traversability_functor.cpp b/src/backend/update_region_growing_traversability_functor.cpp index f30e7a47..2a975b5f 100644 --- a/src/backend/update_region_growing_traversability_functor.cpp +++ b/src/backend/update_region_growing_traversability_functor.cpp @@ -77,7 +77,14 @@ UpdateFunctor::Hooks UpdateRegionGrowingTraversabilityFunctor::hooks() const { }; my_hooks.merge = [this](const DynamicSceneGraph& dsg, const std::vector& merge_ids) { - return mergeNodes(dsg, merge_ids); + auto attrs = mergeNodes(dsg, merge_ids); + if (attrs) { + // the merged attributes are cloned from the odometric unmerged graph; + // re-apply the surviving node's last deformation so the merge result stays + // in the optimized frame + deformation_interpolator_.applyLastTransform(merge_ids.front(), *attrs); + } + return attrs; }; my_hooks.cleanup = [this](const UpdateInfo::ConstPtr& info, DynamicSceneGraph&, diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 67315da1..cea423c8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -10,6 +10,8 @@ add_executable( test_${PROJECT_NAME} main.cpp src/resources.cpp + backend/test_deformation_interpolator.cpp + backend/test_dsg_updater.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..447d6f71 --- /dev/null +++ b/tests/backend/test_deformation_interpolator.cpp @@ -0,0 +1,194 @@ +#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::ObjectNodeAttributes; +using spark_dsg::SemanticNodeAttributes; + +namespace { + +Eigen::Isometry3d makeTransform() { + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + transform.linear() = + Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + transform.translation() = Eigen::Vector3d(10.0, -2.0, 1.0); + return transform; +} + +} // namespace + +// 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 unmerged graph holds odometric values by invariant, so the cached position +// (used as the pgmo vertex for control-point matching) refreshes on every add, +// including for archived nodes. +TEST(NodeCache, RefreshesPositionEvenWhenArchived) { + NodeAttributes attrs; + attrs.last_update_time_ns = 10u; + attrs.is_active = true; + attrs.position = Eigen::Vector3d(1.0, 1.0, 1.0); + + NodeCache cache; + const NodeId node_id = 8; + cache.add(node_id, attrs); + + attrs.is_active = false; + attrs.position = Eigen::Vector3d(2.0, 3.0, 4.0); + auto* entry = cache.add(node_id, attrs); + ASSERT_NE(entry, nullptr); + EXPECT_TRUE(entry->pos.isApprox(attrs.position.cast())); +} + +// Merge hooks rebuild parent attributes from odometric constituents, so without a +// recorded deformation there is nothing to apply and the attributes stay untouched. +TEST(NodeCache, ApplyLastTransformNoEntryIsNoop) { + NodeCache cache; + + NodeAttributes attrs; + attrs.position = Eigen::Vector3d(1.0, 2.0, 3.0); + EXPECT_FALSE(cache.applyLastTransform(3, attrs)); + EXPECT_TRUE(attrs.position.isApprox(Eigen::Vector3d(1.0, 2.0, 3.0))); + + // an entry that was never deformed also applies nothing + attrs.last_update_time_ns = 10u; + cache.add(3, attrs); + EXPECT_FALSE(cache.applyLastTransform(3, attrs)); + EXPECT_TRUE(attrs.position.isApprox(Eigen::Vector3d(1.0, 2.0, 3.0))); +} + +// Once the deform callback records a node's odometric->optimized transform, merge +// hooks can bring freshly-cloned odometric attributes into the optimized frame. +TEST(NodeCache, ApplyLastTransformAppliesStoredTransform) { + const auto transform = makeTransform(); + + ObjectNodeAttributes attrs; + attrs.last_update_time_ns = 10u; + attrs.is_active = true; + attrs.position = Eigen::Vector3d(1.0, 2.0, 3.0); + attrs.bounding_box = + BoundingBox(Eigen::Vector3f(1.0f, 2.0f, 3.0f), Eigen::Vector3f(1.0f, 2.0f, 3.0f)); + attrs.world_R_object = Eigen::Quaterniond::Identity(); + + NodeCache cache; + const NodeId node_id = 4; + cache.add(node_id, attrs); + cache.nodes.at(node_id).last_transform = transform; + + auto expected_box = attrs.bounding_box; + expected_box.transform(transform); + + auto merged = attrs.clone(); + EXPECT_TRUE(cache.applyLastTransform(node_id, *merged)); + + const auto& result = dynamic_cast(*merged); + EXPECT_TRUE(result.position.isApprox(transform * attrs.position)); + EXPECT_TRUE(result.bounding_box.world_P_center.isApprox(expected_box.world_P_center)); + EXPECT_TRUE( + result.world_R_object.toRotationMatrix().isApprox(transform.linear(), 1.0e-9)); +} + +// The deform callback derives every optimized field from the odometric source, so +// applying the same transform repeatedly to an already-deformed destination must +// not compound (archived merged nodes are re-deformed every post-LC spin). +TEST(ApplyNodeDeformation, DerivesFromSourceIdempotently) { + const auto transform = makeTransform(); + + ObjectNodeAttributes src; + src.position = Eigen::Vector3d(1.0, 2.0, 3.0); + src.bounding_box = + BoundingBox(Eigen::Vector3f(2.0f, 4.0f, 6.0f), Eigen::Vector3f(1.0f, 2.0f, 3.0f)); + src.world_R_object = Eigen::Quaterniond::Identity(); + + auto dst_ptr = src.clone(); + auto& dst = dynamic_cast(*dst_ptr); + // simulate stale optimized state on the destination + dst.position = Eigen::Vector3d(9.0, 9.0, 9.0); + dst.bounding_box = + BoundingBox(Eigen::Vector3f(9.0f, 9.0f, 9.0f), Eigen::Vector3f(9.0f, 9.0f, 9.0f)); + + auto expected_box = src.bounding_box; + expected_box.transform(transform); + + applyNodeDeformation(transform, src, dst); + applyNodeDeformation(transform, src, dst); // second application must not compound + + EXPECT_TRUE(dst.position.isApprox(transform * src.position)); + EXPECT_TRUE(dst.bounding_box.world_P_center.isApprox(expected_box.world_P_center)); + EXPECT_TRUE(dst.bounding_box.dimensions.isApprox(expected_box.dimensions)); + EXPECT_TRUE( + dst.world_R_object.toRotationMatrix().isApprox(transform.linear(), 1.0e-9)); +} + +// Merged-only state (e.g. merge-accumulated mesh connections) must survive +// deformation: only spatial fields are written. +TEST(ApplyNodeDeformation, PreservesNonSpatialDestinationState) { + const auto transform = makeTransform(); + + KhronosObjectAttributes src; + src.position = Eigen::Vector3d(1.0, 2.0, 3.0); + src.mesh_connections = {1, 2}; + + auto dst_ptr = src.clone(); + auto& dst = dynamic_cast(*dst_ptr); + dst.mesh_connections = {1, 2, 3, 4}; // accumulated by merges + + applyNodeDeformation(transform, src, dst); + + EXPECT_TRUE(dst.position.isApprox(transform * src.position)); + EXPECT_EQ(dst.mesh_connections, decltype(dst.mesh_connections)({1, 2, 3, 4})); +} + +// Nodes without a bounding box (plain NodeAttributes, e.g. agent poses) only get +// their position updated; an INVALID source box is never copied over. +TEST(ApplyNodeDeformation, SkipsBoxForPlainAttributes) { + const auto transform = makeTransform(); + + NodeAttributes src; + src.position = Eigen::Vector3d(1.0, 2.0, 3.0); + + NodeAttributes dst; + applyNodeDeformation(transform, src, dst); + EXPECT_TRUE(dst.position.isApprox(transform * src.position)); + + // semantic destination keeps its box when the source box is INVALID + SemanticNodeAttributes semantic_src; + semantic_src.position = Eigen::Vector3d(1.0, 2.0, 3.0); + + SemanticNodeAttributes semantic_dst; + semantic_dst.bounding_box = + BoundingBox(Eigen::Vector3f(1.0f, 1.0f, 1.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f)); + applyNodeDeformation(transform, semantic_src, semantic_dst); + EXPECT_EQ(semantic_dst.bounding_box.type, BoundingBox::Type::AABB); + EXPECT_TRUE( + semantic_dst.bounding_box.dimensions.isApprox(Eigen::Vector3f(1.0f, 1.0f, 1.0f))); +} + +} // namespace hydra diff --git a/tests/backend/test_dsg_updater.cpp b/tests/backend/test_dsg_updater.cpp new file mode 100644 index 00000000..8755389d --- /dev/null +++ b/tests/backend/test_dsg_updater.cpp @@ -0,0 +1,193 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include +#include +#include +#include +#include + +#include "hydra_test/shared_dsg_fixture.h" + +namespace hydra { +namespace { + +struct CallRecord { + std::string functor; + bool node_active; + bool node_new; +}; + +std::vector& callRecords() { + static std::vector records; + return records; +} + +std::map& cleanupCounts() { + static std::map counts; + return counts; +} + +NodeId trackedNode() { return spark_dsg::NodeSymbol('p', 0); } + +// Records, per functor invocation, how the shared per-spin bookkeeping (forced-active +// flags and new-node status on the source graph) looks from inside the functor. +struct RecordingFunctor : UpdateFunctor { + struct Config { + std::string name; + } const config; + + explicit RecordingFunctor(const Config& config) : config(config) {} + + Hooks hooks() const override { + auto my_hooks = UpdateFunctor::hooks(); + my_hooks.cleanup = [name = config.name](const UpdateInfo::ConstPtr&, + DynamicSceneGraph&, + SharedDsgInfo*) { + ++cleanupCounts()[name]; + }; + return my_hooks; + } + + void call(const DynamicSceneGraph& unmerged, + SharedDsgInfo&, + const UpdateInfo::ConstPtr&) const override { + const auto& attrs = unmerged.getNode(trackedNode()).attributes(); + callRecords().push_back({config.name, + attrs.is_active, + unmerged.checkNode(trackedNode()) == NodeStatus::NEW}); + } + + inline static const auto registration_ = + config::RegistrationWithConfig( + "RecordingFunctor"); +}; + +void declare_config(RecordingFunctor::Config& config) { + config::name("RecordingFunctor::Config"); + config::field(config.name, "name"); +} + +} // namespace + +TEST(DsgUpdater, PerSpinBookkeepingSharedAcrossFunctors) { + callRecords().clear(); + cleanupCounts().clear(); + + auto target = test::makeSharedDsg(); + DynamicSceneGraph::Ptr source = target->graph->clone(); + + // a NEW node that archived before the backend ever saw it + auto attrs = std::make_unique(); + attrs->is_active = false; + source->emplaceNode(DsgLayers::PLACES, trackedNode(), std::move(attrs)); + + DsgUpdater::Config config; + config.update_functors.emplace_back( + "a", DsgUpdater::Config::FunctorConfig(RecordingFunctor::Config{"a"})); + config.update_functors.emplace_back( + "b", DsgUpdater::Config::FunctorConfig(RecordingFunctor::Config{"b"})); + + DsgUpdater updater(config, source, target); + UpdateInfo::ConstPtr info(new UpdateInfo{0}); + updater.callUpdateFunctions(0, info); + + // every functor in the spin should observe the forced-active flag and the new-node + // status; both are per-spin state that must only reset after all functors ran + ASSERT_EQ(callRecords().size(), 2u); + EXPECT_EQ(callRecords()[0].functor, "a"); + EXPECT_TRUE(callRecords()[0].node_active); + EXPECT_TRUE(callRecords()[0].node_new); + EXPECT_EQ(callRecords()[1].functor, "b"); + EXPECT_TRUE(callRecords()[1].node_active); + EXPECT_TRUE(callRecords()[1].node_new); + + // each cleanup hook runs exactly once per spin + EXPECT_EQ(cleanupCounts()["a"], 1u); + EXPECT_EQ(cleanupCounts()["b"], 1u); + + // after the spin the forced-active flag is restored + EXPECT_FALSE(source->getNode(trackedNode()).attributes().is_active); +} + +// Duplicates revealed by a loop closure only overlap in the OPTIMIZED frame; the +// unmerged (source) graph stays odometric, so pass-0 merge finding has to run on +// the merged graph like the exhaustive passes already do. +TEST(DsgUpdater, FindsPassZeroMergesOnMergedGraph) { + auto target = test::makeSharedDsg(); + DynamicSceneGraph::Ptr source = target->graph->clone(); + + // two archived objects far apart in the odometric (source) frame + const auto node_a = spark_dsg::NodeSymbol('o', 0); + const auto node_b = spark_dsg::NodeSymbol('o', 1); + for (const auto node_id : {node_a, node_b}) { + auto attrs = std::make_unique(); + attrs->position = + Eigen::Vector3d(10.0 * spark_dsg::NodeSymbol(node_id).categoryId(), 0.0, 0.0); + attrs->is_active = false; + attrs->last_update_time_ns = 10u; + source->emplaceNode(DsgLayers::OBJECTS, node_id, std::move(attrs)); + } + + GenericUpdateFunctor::Config functor_config; + functor_config.layer = DsgLayers::OBJECTS; + functor_config.enable_merging = true; + + DsgUpdater::Config config; + config.update_functors.emplace_back( + "objects", DsgUpdater::Config::FunctorConfig(functor_config)); + + DsgUpdater updater(config, source, target); + + // first spin clones both nodes into the merged graph + UpdateInfo::ConstPtr info(new UpdateInfo{0}); + updater.callUpdateFunctions(0, info); + ASSERT_TRUE(target->graph->hasNode(node_a)); + ASSERT_TRUE(target->graph->hasNode(node_b)); + + // simulate optimization: the merged copies land on top of each other (archived + // nodes are not reset by the graph merge, so this persists across spins) + target->graph->getNode(node_a).attributes().position = Eigen::Vector3d::Zero(); + target->graph->getNode(node_b).attributes().position = + Eigen::Vector3d(0.05, 0.0, 0.0); + + // loop-closure spin: the duplicates only overlap in the merged graph + UpdateInfo::ConstPtr lc_info(new UpdateInfo{1, nullptr, nullptr, true, {}}); + updater.callUpdateFunctions(1, lc_info); + + EXPECT_EQ(target->merges.size(), 1u); + EXPECT_EQ(target->graph->getLayer(DsgLayers::OBJECTS).numNodes(), 1u); +} + +} // namespace hydra diff --git a/tests/backend/test_generic_update_functor.cpp b/tests/backend/test_generic_update_functor.cpp index 936dc65c..42fe8859 100644 --- a/tests/backend/test_generic_update_functor.cpp +++ b/tests/backend/test_generic_update_functor.cpp @@ -109,15 +109,25 @@ TEST(GenericUpdateFunctor, shouldUpdate) { VLOG(1) << "Using config:\n" << config::toString(config); GenericUpdateFunctor functor(config); - callWithUnmerged(functor, *dsg, info, false); + // the unmerged graph persists across spins and stays odometric; the functor only + // ever writes the merged graph + const auto unmerged = graph.clone(); + functor.call(*unmerged, *dsg, info); const auto& result = graph.getNode(0).attributes(); const Eigen::Vector3d expected(1.0, 2.0, 3.0); EXPECT_NEAR(0.0, (expected - result.position).norm(), 1.0e-7); - // second call shouldn't change position + { // the odometric source is never touched + const Eigen::Vector3d odometric(0.0, 3.0, 1.0); + const auto& src = unmerged->getNode(0).attributes(); + EXPECT_NEAR(0.0, (odometric - src.position).norm(), 1.0e-7); + } + + // re-deforming an archived node must not compound graph.getNode(0).attributes().is_active = false; - callWithUnmerged(functor, *dsg, info, false); + unmerged->getNode(0).attributes().is_active = false; + functor.call(*unmerged, *dsg, info); EXPECT_NEAR(0.0, (expected - result.position).norm(), 1.0e-7); } diff --git a/tests/backend/test_update_objects_functor.cpp b/tests/backend/test_update_objects_functor.cpp index 069b76b5..09d99a5f 100644 --- a/tests/backend/test_update_objects_functor.cpp +++ b/tests/backend/test_update_objects_functor.cpp @@ -49,7 +49,9 @@ MergeList callWithUnmerged(const UpdateFunctor& functor, functor.call(*unmerged, dsg, info); const auto hooks = functor.hooks(); if (enable_merging && hooks.find_merges) { - return hooks.find_merges(*unmerged, info); + // mirror DsgUpdater: merges are found on the merged graph, which carries the + // updated (optimized) geometry; the unmerged graph stays odometric + return hooks.find_merges(*dsg.graph, info); } else { return {}; } diff --git a/tests/backend/test_update_places_functor.cpp b/tests/backend/test_update_places_functor.cpp index ed00bd85..6110ebfc 100644 --- a/tests/backend/test_update_places_functor.cpp +++ b/tests/backend/test_update_places_functor.cpp @@ -50,7 +50,9 @@ MergeList callWithUnmerged(const UpdateFunctor& functor, functor.call(*unmerged, dsg, info); const auto hooks = functor.hooks(); if (enable_merging && hooks.find_merges) { - return hooks.find_merges(*unmerged, info); + // mirror DsgUpdater: merges are found on the merged graph, which carries the + // updated (optimized) geometry; the unmerged graph stays odometric + return hooks.find_merges(*dsg.graph, info); } else { return {}; } @@ -107,6 +109,41 @@ TEST(UpdatePlacesFunctor, PlaceUpdate) { } } +TEST(UpdatePlacesFunctor, UpdateFromValuesLeavesUnmergedUntouched) { + auto dsg = test::makeSharedDsg(); + auto& graph = *dsg->graph; + + auto attrs = std::make_unique(0.0, 0.0); + attrs->position = Eigen::Vector3d(1.0, 2.0, 3.0); + attrs->is_active = true; + graph.emplaceNode(DsgLayers::PLACES, NodeSymbol('p', 0), std::move(attrs)); + + const auto unmerged = graph.clone(); + + gtsam::Values values; + values.insert(NodeSymbol('p', 0), + gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(4.0, 5.0, 6.0))); + + UpdateInfo::ConstPtr info(new UpdateInfo{0, &values, nullptr, true, {}}); + UpdatePlacesFunctor::Config config{0.4, 0.3, DeformationInterpolator::Config{}}; + config.use_temp_values = true; + + const UpdatePlacesFunctor functor(config); + functor.call(*unmerged, *dsg, info); + + { // the optimized value lands in the merged graph + Eigen::Vector3d expected(4.0, 5.0, 6.0); + Eigen::Vector3d result = graph.getPosition(NodeSymbol('p', 0)); + EXPECT_NEAR(0.0, (result - expected).norm(), 1.0e-7); + } + + { // the unmerged (odometric) graph keeps its original position + Eigen::Vector3d expected(1.0, 2.0, 3.0); + Eigen::Vector3d result = unmerged->getPosition(NodeSymbol('p', 0)); + EXPECT_NEAR(0.0, (result - expected).norm(), 1.0e-7); + } +} + TEST(UpdatePlacesFunctor, PlaceUpdateNodeFinderBug) { auto dsg = test::makeSharedDsg(); auto& graph = *dsg->graph;