From 7fd25d367f9afc94d75d8a5888a8706db792648a Mon Sep 17 00:00:00 2001
From: Rainer Kuemmerle <rainer.kuemmerle@gmail.com>
Date: Sat, 22 Feb 2025 13:34:56 +0100
Subject: [PATCH] Compute initial estimate in simulator

---
 g2o/simulator/simulator.cpp | 34 +++++++++++++++++++++++++++++-----
 g2o/simulator/simulator.h   |  1 +
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/g2o/simulator/simulator.cpp b/g2o/simulator/simulator.cpp
index a3988f863..b1baa742d 100644
--- a/g2o/simulator/simulator.cpp
+++ b/g2o/simulator/simulator.cpp
@@ -26,12 +26,29 @@
 
 #include "simulator.h"
 
+#include <memory>
 #include <unordered_set>
 
+#include "g2o/core/estimate_propagator.h"
 #include "g2o/core/optimizable_graph.h"
 
 namespace g2o {
 
+namespace {
+class G2O_CORE_API 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 +113,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
index 55bc22377..a9ea7078d 100644
--- 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);
   }