Build and retain optimization extras in Hydra backends

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<OptimizationHook::Ptr> optimization_hooks_;
+  gtsam::Values extra_estimates_;
 
   std::vector<gtsam::Pose3> trajectory_;
   std::vector<size_t> timestamps_;

hydra/include/hydra/backend/mst_factors.h

@@ -34,7 +34,6 @@
  * -------------------------------------------------------------------------- */
 
 #pragma once
-#include <functional>
 
 #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<char(spark_dsg::NodeId)>& prefix_lookup);
-
 }  // namespace hydra

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;
 };
 

hydra/src/backend/backend_module.cpp

@@ -39,6 +39,7 @@
 #include <config_utilities/validation.h>
 #include <glog/logging.h>
 #include <glog/stl_logging.h>
+#include <kimera_pgmo/deformation_graph_4dof.h>
 #include <kimera_pgmo/mesh_delta.h>
 #include <kimera_pgmo/pcl_mesh_traits.h>
 #include <kimera_pgmo/utils/mesh_io.h>
@@ -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,
                                            {},

hydra/src/backend/mst_factors.cpp

@@ -38,8 +38,10 @@
 #include <config_utilities/factory.h>
 #include <config_utilities/validation.h>
 #include <glog/logging.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <kimera_pgmo/deformation_edge_factor.h>
 #include <kimera_pgmo/deformation_graph.h>
-#include <pose_graph_tools/pose_graph.h>
+#include <kimera_pgmo/utils/common_functions.h>
 #include <spark_dsg/node_attributes.h>
 #include <spark_dsg/scene_graph.h>
 
@@ -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<PlaceNodeAttributes>();
       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<size_t>::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<size_t>::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<kimera_pgmo::DeformationEdgeFactor>(
+            node.id, vertex, pose, position, noise);
+        extras.factors.emplace_shared<kimera_pgmo::DeformationEdgeFactor>(
+            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<gtsam::Pose3>(s_id);
+      const auto target = extras.initial.at<gtsam::Pose3>(t_id);
+      extras.factors.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(
+          s_id, t_id, source.between(target), noise);
+    }
   }  // end timing scope
 }
 

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

hydra/tests/backend/test_mst_factors.cpp

@@ -0,0 +1,64 @@
+#include <gtest/gtest.h>
+#include <hydra/backend/mst_factors.h>
+#include <hydra/common/global_info.h>
+#include <kimera_pgmo/deformation_graph.h>
+#include <kimera_pgmo/deformation_graph_4dof.h>
+#include <spark_dsg/node_attributes.h>
+#include <spark_dsg/node_symbol.h>
+#include <spark_dsg/scene_graph.h>
+
+#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<size_t> indices;
+  std::vector<kimera_pgmo::Timestamp> stamps;
+  deformation_graph.processNewMeshEdgesAndNodes(
+      {}, mesh_nodes, {{vertex, 0}}, &indices, &stamps);
+
+  for (size_t i = 0; i < 2; ++i) {
+    auto attrs = std::make_unique<spark_dsg::PlaceNodeAttributes>();
+    attrs->position = Eigen::Vector3d(i + 1, 0, 0);
+    attrs->deformation_connections = {0, 9, std::numeric_limits<size_t>::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