diff --git a/g2o/core/estimate_propagator.cpp b/g2o/core/estimate_propagator.cpp index 7911848fa..ad1ae9a1e 100644 --- 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& 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 index 46acbaeea..0adcd46a0 100644 --- 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& v, - const EstimatePropagator::PropagateCost& cost, + const EstimatePropagatorCostBase& cost, const EstimatePropagator::PropagateAction& action = PropagateAction(), double maxDistance = std::numeric_limits::max(), double maxEdgeCost = std::numeric_limits::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::max(), double maxEdgeCost = std::numeric_limits::max()); diff --git a/g2o/core/sparse_optimizer.cpp b/g2o/core/sparse_optimizer.cpp index 53c639612..bbf0321f2 100644 --- 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 backupVertices; OptimizableGraph::VertexSet fixedVertices; // these are the root nodes where diff --git a/g2o/core/sparse_optimizer.h b/g2o/core/sparse_optimizer.h index 85878b310..e5e527529 100644 --- 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 index f40f560b2..09a6252e6 100644 --- a/python/core/py_estimate_propagator.cpp +++ b/python/core/py_estimate_propagator.cpp @@ -1,11 +1,52 @@ #include "py_estimate_propagator.h" +#include + #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_(m, "EstimatePropagatorCost") + py::class_( + 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_( + m, "EstimatePropagatorCost") .def(py::init(), "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_( + py::class_( m, "EstimatePropagatorCostOdometry") .def(py::init(), "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_ cls(m, "EstimatePropagator"); py::class_(cls, @@ -62,7 +103,7 @@ void delcareEstimatePropagator(py::module& m) { cls.def("propagate", static_cast&, - 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( &EstimatePropagator::propagate), "vset"_a, "cost"_a, "action"_a, diff --git a/python/core/py_sparse_optimizer.cpp b/python/core/py_sparse_optimizer.cpp index c2b611fca..27d7d6062 100644 --- 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(&CLS::computeInitialGuess)) // virtual .def("compute_initial_guess", - static_cast( + static_cast( &CLS::computeInitialGuess), "propagator"_a) // virtual