Update initial estimator API

RainerKuemmerle · Feb 22, 2025 · 648b134 · 648b134
1 parent 2889c17
commit 648b134
Show file tree

Hide file tree

Showing 6 changed files with 80 additions and 33 deletions.
diff --git a/g2o/core/estimate_propagator.cpp b/g2o/core/estimate_propagator.cpp
@@ -80,7 +80,7 @@ void EstimatePropagator::reset() {
 
 void EstimatePropagator::propagate(
     const std::shared_ptr<OptimizableGraph::Vertex>& v,
-    const EstimatePropagator::PropagateCost& cost,
+    const EstimatePropagatorCostBase& cost,
     const EstimatePropagator::PropagateAction& action, double maxDistance,
     double maxEdgeCost) {
   OptimizableGraph::VertexSet vset;
@@ -89,8 +89,7 @@ void EstimatePropagator::propagate(
 }
 
 void EstimatePropagator::propagate(
-    OptimizableGraph::VertexSet& vset,
-    const EstimatePropagator::PropagateCost& cost,
+    OptimizableGraph::VertexSet& vset, const EstimatePropagatorCostBase& cost,
     const EstimatePropagator::PropagateAction& action, double maxDistance,
     double maxEdgeCost) {
   reset();
@@ -241,7 +240,7 @@ double EstimatePropagatorCost::operator()(
 
 EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(
     SparseOptimizer* graph)
-    : EstimatePropagatorCost(graph) {}
+    : graph_(graph) {}
 
 double EstimatePropagatorCostOdometry::operator()(
     OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_,

diff --git a/g2o/core/estimate_propagator.h b/g2o/core/estimate_propagator.h
@@ -36,25 +36,32 @@
 
 #include "g2o_core_api.h"
 #include "optimizable_graph.h"
-#include "sparse_optimizer.h"
 
 namespace g2o {
 class SparseOptimizer;
 
+class G2O_CORE_API EstimatePropagatorCostBase {
+ public:
+  virtual ~EstimatePropagatorCostBase() = default;
+  virtual double operator()(OptimizableGraph::Edge* edge,
+                            const OptimizableGraph::VertexSet& from,
+                            OptimizableGraph::Vertex* to) const = 0;
+  [[nodiscard]] virtual std::string_view name() const = 0;
+};
+
 /**
- * \brief cost for traversing along active edges in the optimizer
+ * \brief cost for traversing along active edges in the SparseOptimizer
  *
- * You may derive an own one, if necessary. The default is to return
- * initialEstimatePossible(from, to) for the edge.
+ * You may derive an own one, if necessary. Here, we return
+ * initialEstimatePossible(from, to) as cost for the edge.
  */
-class G2O_CORE_API EstimatePropagatorCost {
+class G2O_CORE_API EstimatePropagatorCost : public EstimatePropagatorCostBase {
  public:
-  virtual ~EstimatePropagatorCost() = default;
   explicit EstimatePropagatorCost(SparseOptimizer* graph);
-  virtual double operator()(OptimizableGraph::Edge* edge,
-                            const OptimizableGraph::VertexSet& from,
-                            OptimizableGraph::Vertex* to_) const;
-  [[nodiscard]] virtual std::string_view name() const {
+  double operator()(OptimizableGraph::Edge* edge,
+                    const OptimizableGraph::VertexSet& from,
+                    OptimizableGraph::Vertex* to) const override;
+  [[nodiscard]] std::string_view name() const override {
     return "spanning tree";
   }
 
@@ -69,13 +76,16 @@ class G2O_CORE_API EstimatePropagatorCost {
  * connect vertices whose IDs only differs by one.
  */
 class G2O_CORE_API EstimatePropagatorCostOdometry
-    : public EstimatePropagatorCost {
+    : public EstimatePropagatorCostBase {
  public:
   explicit EstimatePropagatorCostOdometry(SparseOptimizer* graph);
   double operator()(OptimizableGraph::Edge* edge,
                     const OptimizableGraph::VertexSet& from,
                     OptimizableGraph::Vertex* to) const override;
   [[nodiscard]] std::string_view name() const override { return "odometry"; }
+
+ protected:
+  SparseOptimizer* graph_;
 };
 
 /**
@@ -99,8 +109,6 @@ class G2O_CORE_API EstimatePropagator {
     }
   };
 
-  using PropagateCost = EstimatePropagatorCost;
-
   class AdjacencyMapEntry;
 
   /**
@@ -173,7 +181,7 @@ class G2O_CORE_API EstimatePropagator {
    */
   void propagate(
       const std::shared_ptr<OptimizableGraph::Vertex>& v,
-      const EstimatePropagator::PropagateCost& cost,
+      const EstimatePropagatorCostBase& cost,
       const EstimatePropagator::PropagateAction& action = PropagateAction(),
       double maxDistance = std::numeric_limits<double>::max(),
       double maxEdgeCost = std::numeric_limits<double>::max());
@@ -183,8 +191,7 @@ class G2O_CORE_API EstimatePropagator {
    * just a single one.
    */
   void propagate(
-      OptimizableGraph::VertexSet& vset,
-      const EstimatePropagator::PropagateCost& cost,
+      OptimizableGraph::VertexSet& vset, const EstimatePropagatorCostBase& cost,
       const EstimatePropagator::PropagateAction& action = PropagateAction(),
       double maxDistance = std::numeric_limits<double>::max(),
       double maxEdgeCost = std::numeric_limits<double>::max());

diff --git a/g2o/core/sparse_optimizer.cpp b/g2o/core/sparse_optimizer.cpp
@@ -305,11 +305,12 @@ bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset) {
 }
 
 void SparseOptimizer::computeInitialGuess() {
-  EstimatePropagator::PropagateCost costFunction(this);
+  EstimatePropagatorCost costFunction(this);
   computeInitialGuess(costFunction);
 }
 
-void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& propagator) {
+void SparseOptimizer::computeInitialGuess(
+    EstimatePropagatorCostBase& propagator) {
   OptimizableGraph::VertexSet emptySet;
   std::unordered_set<Vertex*> backupVertices;
   OptimizableGraph::VertexSet fixedVertices;  // these are the root nodes where

diff --git a/g2o/core/sparse_optimizer.h b/g2o/core/sparse_optimizer.h
@@ -43,7 +43,7 @@ namespace g2o {
 
 // forward declaration
 class OptimizationAlgorithm;
-class EstimatePropagatorCost;
+class EstimatePropagatorCostBase;
 class HyperGraphAction;
 
 class G2O_CORE_API SparseOptimizer : public OptimizableGraph {
@@ -116,7 +116,7 @@ class G2O_CORE_API SparseOptimizer : public OptimizableGraph {
   /**
    * Same as above but using a specific propagator
    */
-  virtual void computeInitialGuess(EstimatePropagatorCost& propagator);
+  virtual void computeInitialGuess(EstimatePropagatorCostBase& propagator);
 
   /**
    * starts one optimization run given the current configuration of the graph,

diff --git a/python/core/py_estimate_propagator.cpp b/python/core/py_estimate_propagator.cpp
@@ -1,11 +1,52 @@
 #include "py_estimate_propagator.h"
 
+#include <pybind11/pybind11.h>
+
 #include "g2o/core/estimate_propagator.h"
+#include "g2o/core/sparse_optimizer.h"
 
 namespace g2o {
 
+class PyEstimatePropagatorCostBase : public EstimatePropagatorCostBase {
+ public:
+  /* Inherit the constructors */
+  using EstimatePropagatorCostBase::EstimatePropagatorCostBase;
+
+  // Trampoline (need one for each virtual function)
+  double operator()(OptimizableGraph::Edge* edge,
+                    const OptimizableGraph::VertexSet& from,
+                    OptimizableGraph::Vertex* to) const override {
+    PYBIND11_OVERRIDE_PURE_NAME(double,                     /* Return type */
+                                EstimatePropagatorCostBase, /* Parent class */
+                                "__call__", /* Name of function in Python */
+                                operator(), /* Name of function in C++ */
+                                edge,       /* Argument(s) */
+                                from, to);
+  }
+
+  // Trampoline (need one for each virtual function)
+  [[nodiscard]] std::string_view name() const override {
+    PYBIND11_OVERRIDE_PURE(std::string_view,           /* Return type */
+                           EstimatePropagatorCostBase, /* Parent class */
+                           name, /* Name of function in C++ */
+    );
+  }
+};
+
 void delcareEstimatePropagator(py::module& m) {
-  py::class_<EstimatePropagatorCost>(m, "EstimatePropagatorCost")
+  py::class_<EstimatePropagatorCostBase,
+             PyEstimatePropagatorCostBase /* <--- trampoline*/>(
+      m, "EstimatePropagatorCostBase")
+      .def(py::init<>())
+      .def("__call__", &EstimatePropagatorCostBase::operator(), "edge"_a,
+           "from"_a, "to"_a, py::keep_alive<1, 2>(), py::keep_alive<1, 3>(),
+           py::keep_alive<1, 4>())  // (OptimizableGraph::Edge* edge, const
+      // OptimizableGraph::VertexSet& from,
+      // OptimizableGraph::Vertex* to_) -> double
+      .def("name", &EstimatePropagatorCostBase::name);
+
+  py::class_<EstimatePropagatorCost, EstimatePropagatorCostBase>(
+      m, "EstimatePropagatorCost")
       .def(py::init<SparseOptimizer*>(), "graph"_a, py::keep_alive<1, 2>())
       .def("__call__", &EstimatePropagatorCost::operator(), "edge"_a, "from"_a,
            "to"_a, py::keep_alive<1, 2>(), py::keep_alive<1, 3>(),
@@ -14,15 +55,15 @@ void delcareEstimatePropagator(py::module& m) {
                                     // OptimizableGraph::Vertex* to_) -> double
       .def("name", &EstimatePropagatorCost::name);
 
-  py::class_<EstimatePropagatorCostOdometry, EstimatePropagatorCost>(
+  py::class_<EstimatePropagatorCostOdometry, EstimatePropagatorCostBase>(
       m, "EstimatePropagatorCostOdometry")
       .def(py::init<SparseOptimizer*>(), "graph"_a, py::keep_alive<1, 2>())
-      .def("__call__", &EstimatePropagatorCost::operator(), "edge"_a, "from"_a,
-           "to"_a, py::keep_alive<1, 2>(), py::keep_alive<1, 3>(),
+      .def("__call__", &EstimatePropagatorCostOdometry::operator(), "edge"_a,
+           "from"_a, "to"_a, py::keep_alive<1, 2>(), py::keep_alive<1, 3>(),
            py::keep_alive<1, 4>())  // (OptimizableGraph::Edge* edge, const
                                     // OptimizableGraph::VertexSet& from,
                                     // OptimizableGraph::Vertex* to_) -> double
-      .def("name", &EstimatePropagatorCost::name);
+      .def("name", &EstimatePropagatorCostOdometry::name);
 
   py::class_<EstimatePropagator> cls(m, "EstimatePropagator");
   py::class_<EstimatePropagator::PropagateAction>(cls,
@@ -62,7 +103,7 @@ void delcareEstimatePropagator(py::module& m) {
   cls.def("propagate",
           static_cast<void (EstimatePropagator::*)(
               const std::shared_ptr<OptimizableGraph::Vertex>&,
-              const EstimatePropagator::PropagateCost&,
+              const EstimatePropagatorCostBase&,
               const EstimatePropagator::PropagateAction&, double, double)>(
               &EstimatePropagator::propagate),
           "v"_a, "cost"_a, "action"_a,
@@ -73,8 +114,7 @@ void delcareEstimatePropagator(py::module& m) {
 
   cls.def("propagate",
           static_cast<void (EstimatePropagator::*)(
-              OptimizableGraph::VertexSet&,
-              const EstimatePropagator::PropagateCost&,
+              OptimizableGraph::VertexSet&, const EstimatePropagatorCostBase&,
               const EstimatePropagator::PropagateAction&, double, double)>(
               &EstimatePropagator::propagate),
           "vset"_a, "cost"_a, "action"_a,

diff --git a/python/core/py_sparse_optimizer.cpp b/python/core/py_sparse_optimizer.cpp
@@ -40,7 +40,7 @@ void declareSparseOptimizer(py::module& m) {
       .def("compute_initial_guess",
            static_cast<void (CLS::*)()>(&CLS::computeInitialGuess))  // virtual
       .def("compute_initial_guess",
-           static_cast<void (CLS::*)(EstimatePropagatorCost&)>(
+           static_cast<void (CLS::*)(EstimatePropagatorCostBase&)>(
                &CLS::computeInitialGuess),
            "propagator"_a)  // virtual