From 7590ee4e8292b9fff6d2cfe63f43b8dc4f7bbffe Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sat, 17 Oct 2026 15:47:50 +0000 Subject: [PATCH] Build and retain optimization extras in Hydra backends Construct place constraints through read-only deformation graph access and pass caller-owned extras to Kimera-PGMO. Prepare compatible 4DoF inputs, retain extra estimates for DSG updates, and migrate partial optimization to bundled problems and results. Depends on Kimera-PGMO commit 10d4a69. Developed with Codex (GPT-6). --- hydra/include/hydra/backend/backend_module.h | 1 + hydra/include/hydra/backend/mst_factors.h | 12 +-- .../include/hydra/backend/optimization_hook.h | 4 +- hydra/src/backend/backend_module.cpp | 22 ++++- hydra/src/backend/mst_factors.cpp | 90 +++++++++++-------- hydra/tests/CMakeLists.txt | 1 + hydra/tests/backend/test_mst_factors.cpp | 64 +++++++++++++ hydra_multi/src/backend/module.cpp | 53 +++++------ 8 files changed, 167 insertions(+), 80 deletions(-) create mode 100644 hydra/tests/backend/test_mst_factors.cpp diff --git a/hydra/include/hydra/backend/backend_module.h b/hydra/include/hydra/backend/backend_module.h index e51ca291..055c40d4 100644 --- a/hydra/include/hydra/backend/backend_module.h +++ b/hydra/include/hydra/backend/backend_module.h @@ -172,6 +172,7 @@ class BackendModule : public kimera_pgmo::KimeraPgmoInterface, public Module { DsgUpdater::Ptr dsg_updater_; std::vector optimization_hooks_; + gtsam::Values extra_estimates_; std::vector trajectory_; std::vector timestamps_; diff --git a/hydra/include/hydra/backend/mst_factors.h b/hydra/include/hydra/backend/mst_factors.h index 1424fb3c..7b04a4fe 100644 --- a/hydra/include/hydra/backend/mst_factors.h +++ b/hydra/include/hydra/backend/mst_factors.h @@ -34,7 +34,6 @@ * -------------------------------------------------------------------------- */ #pragma once -#include #include "hydra/backend/optimization_hook.h" @@ -55,18 +54,11 @@ class MstPlaceFactors : public OptimizationHook { void updateProblem(uint64_t timestamp_ns, const spark_dsg::SceneGraph& graph, - kimera_pgmo::DeformationGraph& deformation_graph, + const kimera_pgmo::DeformationGraph& deformation_graph, + kimera_pgmo::OptimizationProblem& extras, const NodeRobotMap* robot_lookup) const override; }; void declare_config(MstPlaceFactors::Config& config); -void addPlacesToDeformationGraph( - const spark_dsg::SceneGraph& graph, - size_t timestamp_ns, - kimera_pgmo::DeformationGraph& deformation_graph, - double mst_edge_variance, - double mesh_edge_variance, - const std::function& prefix_lookup); - } // namespace hydra diff --git a/hydra/include/hydra/backend/optimization_hook.h b/hydra/include/hydra/backend/optimization_hook.h index aff33228..c29b4a6a 100644 --- a/hydra/include/hydra/backend/optimization_hook.h +++ b/hydra/include/hydra/backend/optimization_hook.h @@ -42,6 +42,7 @@ namespace kimera_pgmo { class DeformationGraph; +struct OptimizationProblem; } // namespace kimera_pgmo namespace hydra { @@ -55,7 +56,8 @@ class OptimizationHook { virtual void updateProblem(size_t timestamp_ns, const spark_dsg::SceneGraph& graph, - kimera_pgmo::DeformationGraph& deformation_graph, + const kimera_pgmo::DeformationGraph& deformation_graph, + kimera_pgmo::OptimizationProblem& extras, const NodeRobotMap* robot_lookup = nullptr) const = 0; }; diff --git a/hydra/src/backend/backend_module.cpp b/hydra/src/backend/backend_module.cpp index b224314d..6fd684b6 100644 --- a/hydra/src/backend/backend_module.cpp +++ b/hydra/src/backend/backend_module.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -92,6 +93,7 @@ inline bool hasLoopClosure(const PoseGraph& graph) { return true; } } + return false; } @@ -266,7 +268,7 @@ bool BackendModule::spinOnce(bool force_update) { if (have_loopclosures_) { // update graph with cached optimization solution info.reset(new UpdateInfo{timestamp_ns, - deformation_graph_->getTempValues(), + &extra_estimates_, deformation_graph_->getValues(), false, {}, @@ -367,6 +369,7 @@ void BackendModule::updateFactorGraph(const FrontendOutput& input) { LOG(INFO) << "[Hydra Backend] Input pose graph has loop closure @ " << msg.stamp_ns << " [ns]"; } + processIncrementalPoseGraph( msg, inc_mesh_indices, inc_mesh_index_stamps, trajectory_, timestamps_); logIncrementalLoopClosures(msg); @@ -510,26 +513,37 @@ void BackendModule::updateAgentNodeMeasurements(const PoseGraph& meas) { {gtsam::Symbol(GlobalInfo::instance().getRobotPrefix().key, node.key), gtsam::Pose3(node.pose.matrix())}); } + deformation_graph_->processNodeMeasurements(agent_measurements); } void BackendModule::optimize(size_t timestamp_ns, bool force_find_merge) { + kimera_pgmo::OptimizationProblem extras; for (const auto& hook : optimization_hooks_) { if (!hook) { continue; } - hook->updateProblem(timestamp_ns, *unmerged_graph_, *deformation_graph_); + hook->updateProblem(timestamp_ns, *unmerged_graph_, *deformation_graph_, extras); + } + + if (KimeraPgmoInterface::config_.use_4dof_optim) { + auto [factors, initial] = + kimera_pgmo::convertGraphToPose4DoF(extras.factors, extras.initial); + extras.factors = std::move(factors); + extras.initial = std::move(initial); } ScopedTimer timer("backend/optimization", timestamp_ns, true, 0, false); - KimeraPgmoInterface::optimize(); + kimera_pgmo::OptimizationResult result; + KimeraPgmoInterface::optimize(extras, &result); + extra_estimates_ = kimera_pgmo::convertPose4DoFToPose3(result.extra_estimates); timer.stop(); updateDsgMesh(timestamp_ns); UpdateInfo::ConstPtr info(new UpdateInfo{timestamp_ns, - deformation_graph_->getTempValues(), + &extra_estimates_, deformation_graph_->getValues(), have_new_loopclosures_ || force_find_merge, {}, diff --git a/hydra/src/backend/mst_factors.cpp b/hydra/src/backend/mst_factors.cpp index f909d3f2..4151be6a 100644 --- a/hydra/src/backend/mst_factors.cpp +++ b/hydra/src/backend/mst_factors.cpp @@ -38,8 +38,10 @@ #include #include #include +#include +#include #include -#include +#include #include #include @@ -72,19 +74,20 @@ void declare_config(MstPlaceFactors::Config& config) { MstPlaceFactors::MstPlaceFactors(const Config& config) : config(config::checkValid(config)) {} -void MstPlaceFactors::updateProblem(uint64_t timestamp_ns, - const SceneGraph& graph, - kimera_pgmo::DeformationGraph& deformation_graph, - const NodeRobotMap* robot_lookup) const { +void MstPlaceFactors::updateProblem( + uint64_t timestamp_ns, + const SceneGraph& graph, + const kimera_pgmo::DeformationGraph& deformation_graph, + kimera_pgmo::OptimizationProblem& extras, + const NodeRobotMap* robot_lookup) const { const auto& places = graph.getLayer(DsgLayers::PLACES); if (places.numNodes() == 0) { - LOG(WARNING) << "Attempting to add places to deformation graph without places"; + LOG(WARNING) << "Attempting to build place constraints without places"; return; } const auto this_vertex_key = GlobalInfo::instance().getRobotPrefix().vertex_key; ScopedTimer timer("backend/mst_place_factors", timestamp_ns); - deformation_graph.clearTemporaryStructures(); MinimumSpanningTreeInfo mst_info; { // start timing scope @@ -95,55 +98,64 @@ void MstPlaceFactors::updateProblem(uint64_t timestamp_ns, { // start timing scope ScopedTimer add_timer("backend/add_places_nodes", timestamp_ns); - kimera_pgmo::NodeValenceInfoList factors; + const auto noise = gtsam::noiseModel::Isotropic::Variance(3, config.mesh_variance); for (const auto& node : places.nodes()) { const auto& attrs = node.attributes(); if (!node.hasSiblings()) { continue; } - auto& factor = factors.emplace_back(); + const auto pose = gtsam::Pose3(gtsam::Rot3(), attrs.position); + extras.initial.insert(node.id, pose); + auto vertex_prefix = this_vertex_key; if (robot_lookup) { - } else { - factor.valence_prefix = this_vertex_key; + vertex_prefix = kimera_pgmo::GetVertexPrefix(robot_lookup->at(node.id)); } - factor.key = node.id; - factor.pose = gtsam::Pose3(gtsam::Rot3(), attrs.position); - - if (mst_info.leaves.count(node.id)) { - for (const auto& idx : attrs.deformation_connections) { - if (idx == std::numeric_limits::max()) { - continue; - } - factor.valence.push_back(idx); + + if (!mst_info.leaves.count(node.id)) { + continue; + } + + for (const auto idx : attrs.deformation_connections) { + if (idx == std::numeric_limits::max()) { + continue; + } + + const auto vertex = gtsam::Symbol(vertex_prefix, idx); + if (!deformation_graph.getValues()->exists(vertex)) { + continue; } + + const auto position = + deformation_graph.getInitialPositionVertex(vertex_prefix, idx); + extras.factors.emplace_shared( + node.id, vertex, pose, position, noise); + extras.factors.emplace_shared( + vertex, + node.id, + gtsam::Pose3(gtsam::Rot3(), position), + attrs.position, + noise); } } - - deformation_graph.processNewTempNodesValences(factors, false, config.mesh_variance); } // end timing scope { // start timing scope ScopedTimer between_timer("backend/add_places_between", timestamp_ns); - const auto robot_id = GlobalInfo::instance().getRobotPrefix().id; - pose_graph_tools::PoseGraph mst_edges; + gtsam::Vector6 variances; + variances.head<3>().setConstant(1.0e-2 * config.edge_variance); + variances.tail<3>().setConstant(config.edge_variance); + const auto noise = gtsam::noiseModel::Diagonal::Variances(variances); for (const auto& [s_id, t_id, dist] : mst_info.edges) { - gtsam::Pose3 source(gtsam::Rot3(), places.getNode(s_id).attributes().position); - gtsam::Pose3 target(gtsam::Rot3(), places.getNode(t_id).attributes().position); - pose_graph_tools::PoseGraphEdge mst_e; - mst_e.key_from = s_id; - mst_e.key_to = t_id; - mst_e.robot_from = robot_id; - mst_e.robot_to = robot_id; - // TODO(nathan) this should technically be something else - mst_e.type = pose_graph_tools::PoseGraphEdge::Type::MESH; - mst_e.stamp_ns = 0; - mst_e.pose = source.between(target).matrix(); - mst_e.covariance.setIdentity(); - mst_edges.edges.push_back(mst_e); - } + if (!extras.initial.exists(s_id) || !extras.initial.exists(t_id)) { + continue; + } - deformation_graph.processNewTempEdges(mst_edges, config.edge_variance); + const auto source = extras.initial.at(s_id); + const auto target = extras.initial.at(t_id); + extras.factors.emplace_shared>( + s_id, t_id, source.between(target), noise); + } } // end timing scope } diff --git a/hydra/tests/CMakeLists.txt b/hydra/tests/CMakeLists.txt index 89165f3b..1a126abe 100644 --- a/hydra/tests/CMakeLists.txt +++ b/hydra/tests/CMakeLists.txt @@ -14,6 +14,7 @@ add_executable( backend/test_deformation_interpolator.cpp backend/test_external_loop_closure.cpp backend/test_merge_tracker.cpp + backend/test_mst_factors.cpp backend/updates/test_generic_update_functor.cpp backend/updates/test_ib_objects_update_functor.cpp backend/updates/test_update_agents_functor.cpp diff --git a/hydra/tests/backend/test_mst_factors.cpp b/hydra/tests/backend/test_mst_factors.cpp new file mode 100644 index 00000000..fd8253a9 --- /dev/null +++ b/hydra/tests/backend/test_mst_factors.cpp @@ -0,0 +1,64 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hydra_test/shared_dsg_fixture.h" + +namespace hydra { +namespace { + +TEST(MstPlaceFactors, BuildsIndependentExtrasWithMeshConnections) { + auto dsg = test::makeSharedDsg(); + const auto prefix = GlobalInfo::instance().getRobotPrefix().vertex_key; + kimera_pgmo::DeformationGraph deformation_graph; + gtsam::Values mesh_nodes; + const auto vertex = gtsam::Symbol(prefix, 0); + mesh_nodes.insert(vertex, gtsam::Pose3()); + std::vector indices; + std::vector stamps; + deformation_graph.processNewMeshEdgesAndNodes( + {}, mesh_nodes, {{vertex, 0}}, &indices, &stamps); + + for (size_t i = 0; i < 2; ++i) { + auto attrs = std::make_unique(); + attrs->position = Eigen::Vector3d(i + 1, 0, 0); + attrs->deformation_connections = {0, 9, std::numeric_limits::max()}; + dsg->graph->emplaceNode( + spark_dsg::DsgLayers::PLACES, spark_dsg::NodeSymbol('p', i), std::move(attrs)); + } + + dsg->graph->insertEdge(spark_dsg::NodeSymbol('p', 0), spark_dsg::NodeSymbol('p', 1)); + const auto base = deformation_graph.getOptimizationProblem(); + MstPlaceFactors hook(MstPlaceFactors::Config{}); + kimera_pgmo::OptimizationProblem extras; + hook.updateProblem(0, *dsg->graph, deformation_graph, extras, nullptr); + ASSERT_EQ(extras.initial.size(), 2u); + ASSERT_EQ(extras.factors.size(), 5u); + EXPECT_TRUE(extras.inliers.empty()); + auto combined = base.initial; + combined.insert(extras.initial); + EXPECT_NEAR(extras.factors.error(combined), 0.0, 1e-10); + EXPECT_EQ(deformation_graph.getValues()->size(), 1u); + EXPECT_TRUE(deformation_graph.getFactors()->empty()); + + // Callers can prepare the same constraints for a 4DoF problem. + auto [factors_4dof, initial_4dof] = + kimera_pgmo::convertGraphToPose4DoF(extras.factors, extras.initial); + ASSERT_EQ(factors_4dof.size(), 5u); + initial_4dof.insert(base.initial); + EXPECT_NEAR(factors_4dof.error(initial_4dof), 0.0, 1e-10); + + auto empty_dsg = test::makeSharedDsg(); + kimera_pgmo::OptimizationProblem next_extras; + hook.updateProblem(1, *empty_dsg->graph, deformation_graph, next_extras, nullptr); + EXPECT_TRUE(next_extras.initial.empty()); + EXPECT_TRUE(next_extras.factors.empty()); +} + +} // namespace +} // namespace hydra diff --git a/hydra_multi/src/backend/module.cpp b/hydra_multi/src/backend/module.cpp index 6819b12d..4d9a802b 100644 --- a/hydra_multi/src/backend/module.cpp +++ b/hydra_multi/src/backend/module.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -375,7 +376,10 @@ void MultiBackendModule::partialOptimization(const Timestamp& timestamp_ns) { } ScopedTimer timer("backend/partial_optimization", timestamp_ns); - auto initial = deformation_graph_->getValuesCopy(); + auto problem = deformation_graph_->getOptimizationProblem( + KimeraPgmoInterface::config_.use_4dof_optim); + auto& initial = problem.initial; + auto& factors = problem.factors; bool has_all_keys = true; for (const auto& key : initial.keys()) { @@ -390,34 +394,32 @@ void MultiBackendModule::partialOptimization(const Timestamp& timestamp_ns) { return; } - auto factors = deformation_graph_->getFactorsCopy(); - auto temp_factors = deformation_graph_->getTempFactorsCopy(); - auto temp_initial = deformation_graph_->getTempValuesCopy(); - // TODO(Yun): remove hard coded value? static const gtsam::SharedNoiseModel& prior_noise = gtsam::noiseModel::Isotropic::Variance(6, 1.0e-3); + gtsam::NonlinearFactorGraph priors; for (const auto& key : optimized.keys()) { - auto value = optimized.at(key); - factors.add(gtsam::PriorFactor(key, value, prior_noise)); - initial.update(key, value); + const auto value = optimized.at(key); + priors.emplace_shared>(key, value, prior_noise); + } + + if (KimeraPgmoInterface::config_.use_4dof_optim) { + auto converted = kimera_pgmo::convertGraphToPose4DoF(priors, optimized); + priors = std::move(converted.first); + optimized = std::move(converted.second); } + factors.push_back(priors); + initial.update(optimized); + // Set all factors as inliers auto known_indices = std::views::iota(size_t(0), factors.size()); - std::set known_inliers(known_indices.begin(), known_indices.end()); - auto temp_known_indices = std::views::iota(size_t(0), temp_factors.size()); - std::set temp_known_inliers(temp_known_indices.begin(), - temp_known_indices.end()); + problem.inliers = std::set(known_indices.begin(), known_indices.end()); LOG(INFO) << "Starting partial optimization"; + kimera_pgmo::OptimizationResult result; try { - partial_pgo_->update(factors, - initial, - known_inliers, - temp_factors, - temp_initial, - temp_known_inliers); + result = partial_pgo_->update(problem); LOG(INFO) << "Finished partial optimization"; } catch (const gtsam::IndeterminantLinearSystemException& e) { LOG(ERROR) << "Failed optimization: '" << gtsam::Symbol(e.nearbyVariable()); @@ -443,16 +445,17 @@ void MultiBackendModule::partialOptimization(const Timestamp& timestamp_ns) { ss << ", "; } } + ss << "]"; LOG(ERROR) << "factor " << i << ": " << ss.str(); } + + return; } - auto estimates = partial_pgo_->getEstimates(); - auto temp_estimates = partial_pgo_->getTempEstimates(); - // Only update values - deformation_graph_->updateValues(estimates); - deformation_graph_->updateTempValues(temp_estimates); + // Partial optimization only updates base values. + deformation_graph_->updateValues( + kimera_pgmo::convertPose4DoFToPose3(result.estimates)); } bool MultiBackendModule::spinOnce(Timestamp timestamp_ns) { @@ -504,6 +507,7 @@ void MultiBackendModule::addLoopClosures() { if (lc.added_to_dgraph) { continue; } + deformation_graph_->processNewBetween( lc.src, lc.dest, @@ -666,9 +670,6 @@ bool MultiBackendModule::updateFromStates(Timestamp timestamp_ns) { staged_mesh_data_[id]->transform(transform); } - // Add objects or places to deformation graph - deformation_graph_->clearTemporaryStructures(); - // Add loop closures so they appear in the published pose graph addLoopClosures();