Compute initial estimate in simulator

RainerKuemmerle · Feb 22, 2025 · 7bd198d · 7bd198d
1 parent 648b134
commit 7bd198d
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 5 deletions.
diff --git a/g2o/simulator/simulator.cpp b/g2o/simulator/simulator.cpp
@@ -26,12 +26,28 @@
 
 #include "simulator.h"
 
+#include <memory>
 #include <unordered_set>
 
+#include "g2o/core/estimate_propagator.h"
 #include "g2o/core/optimizable_graph.h"
 
 namespace g2o {
 
+namespace {
+class SimulationEstimatePropagatorCost : public EstimatePropagatorCostBase {
+ public:
+  double operator()(OptimizableGraph::Edge* edge,
+                    const OptimizableGraph::VertexSet& from,
+                    OptimizableGraph::Vertex* to) const override {
+    return edge->initialEstimatePossible(from, to);
+  }
+  [[nodiscard]] std::string_view name() const override {
+    return "simulation spanning tree";
+  }
+};
+}  // namespace
+
 void BaseWorldObject::setVertex(
     const std::shared_ptr<OptimizableGraph::Vertex>& vertex) {
   vertex_ = vertex;
@@ -96,11 +112,18 @@ void Simulator::finalize() {
     }
     iter = world_.graph().parameters().erase(iter);
   }
-  // TODO(Rainer): Initial estimate
-  /* Fails since only implemented on SparseOptimizer
-  EstimatePropagatorCostOdometry costFunction(&world_.graph());
-  world_.graph().computeInitialGuess(costFunction);
-  */
+
+  // Initial estimate
+  OptimizableGraph::VertexSet fixedVertices;
+  for (const auto& id_v : world_.graph().vertices()) {
+    auto v = std::dynamic_pointer_cast<OptimizableGraph::Vertex>(id_v.second);
+    if (!v || !v->fixed()) continue;
+    fixedVertices.emplace(std::move(v));
+  }
+
+  SimulationEstimatePropagatorCost propagator;
+  EstimatePropagator estimatePropagator(&world_.graph());
+  estimatePropagator.propagate(fixedVertices, propagator);
 }
 
 }  // namespace g2o
diff --git a/g2o/simulator/simulator.h b/g2o/simulator/simulator.h
@@ -142,6 +142,7 @@ class Robot : public BaseRobot {
     pose_ = pose;
     auto po = std::make_unique<PoseObject>();
     po->vertex()->setEstimate(pose_);
+    po->vertex()->setFixed(trajectory_.empty());
     const int pose_id = world.addWorldObject(std::move(po));
     trajectory_.emplace_back(pose_id);
   }

diff --git a/unit_test/simulator/simulator2D_tests.cpp b/unit_test/simulator/simulator2D_tests.cpp
@@ -66,6 +66,8 @@ TEST(Simulator2D, Odom) {
   const OptimizableGraph& graph = simulator.world().graph();
 
   EXPECT_THAT(graph.vertices(), SizeIs(simulator.config.simSteps + 1));
+  int count_fixed = g2o::internal::countFixed(graph);
+  EXPECT_THAT(count_fixed, Eq(1));
   const int poses = g2o::internal::countVerticesMatchingType<VertexSE2>(graph);
   EXPECT_THAT(poses, Eq(simulator.config.simSteps + 1));
   const int odom_cnt = g2o::internal::countEdgesMatchingType<EdgeSE2>(graph);

diff --git a/unit_test/test_helper/graph_functions.h b/unit_test/test_helper/graph_functions.h
@@ -47,4 +47,14 @@ int countEdgesMatchingType(const OptimizableGraph& graph) {
                          return ptr != nullptr;
                        });
 }
+
+inline int countFixed(const OptimizableGraph& graph) {
+  return std::count_if(
+      graph.vertices().begin(), graph.vertices().end(),
+      [](const OptimizableGraph::VertexIDMap::value_type& elem) {
+        auto* ptr = dynamic_cast<OptimizableGraph::Vertex*>(elem.second.get());
+        return ptr != nullptr && ptr->fixed();
+      });
+}
+
 }  // namespace g2o::internal