Pymem simulator

.pre-commit-config.yaml

@@ -17,10 +17,10 @@ repos:
 
 # python
 -   repo: https://github.com/pycqa/isort
-    rev: 5.13.2
+    rev: 6.0.0
     hooks:
       - id: isort
 -   repo: https://github.com/psf/black
-    rev: 24.10.0
+    rev: 25.1.0
     hooks:
     -   id: black

g2o/core/estimate_propagator.cpp

@@ -80,7 +80,7 @@
 
 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(
-    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 @@
 
 EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(
     SparseOptimizer* graph)
-    : EstimatePropagatorCost(graph) {}
+    : graph_(graph) {}
 
 double EstimatePropagatorCostOdometry::operator()(
     OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_,

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 @@
  * 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 @@
     }
   };
 
-  using PropagateCost = EstimatePropagatorCost;
-
   class AdjacencyMapEntry;
 
   /**
@@ -173,7 +181,7 @@
    */
   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 @@
    * 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());

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

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,

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

g2o/simulator/simulator.h

@@ -142,6 +142,7 @@
     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);
   }
@@ -208,11 +209,10 @@
     robotPoseVertex_ = robotPoseVertex<PoseVertexType>(robot, world);
 
     auto e = mkEdge();
-    if (e) {
-      e->setMeasurementFromState();
-      addNoise(e.get());
-      world.graph().addEdge(e);
-    }
+    if (!e) return;
+    e->setMeasurementFromState();
+    addNoise(e.get());
+    world.graph().addEdge(e);
   }
 
  protected:

g2o/simulator/simulator2d_base.cpp

@@ -31,6 +31,7 @@
 #include <optional>
 #include <random>
 
+#include "g2o/core/eigen_types.h"
 #include "g2o/simulator/sensor_odometry2d.h"
 #include "g2o/simulator/sensor_pointxy.h"
 #include "g2o/simulator/sensor_pointxy_bearing.h"
@@ -40,6 +41,7 @@
 #include "g2o/simulator/sensor_segment2d_line.h"
 #include "g2o/simulator/sensor_segment2d_pointline.h"
 #include "g2o/stuff/logger.h"
+#include "g2o/stuff/misc.h"
 #include "g2o/stuff/sampler.h"
 #include "g2o/types/slam2d/se2.h"
 
@@ -51,35 +53,33 @@
   G2O_DEBUG("nlandmarks = {}", config.nlandmarks);
   for (int i = 0; i < config.nlandmarks; i++) {
     auto landmark = std::make_unique<WorldObjectPointXY>();
-    double x = sampleUniform(-.5, .5, &generator_) * (config.worldSize + 5);
-    double y = sampleUniform(-.5, .5, &generator_) * (config.worldSize + 5);
+    const double x =
+        sampleUniform(-.5, .5, &generator_) * (config.worldSize + 5);
+    const double y =
+        sampleUniform(-.5, .5, &generator_) * (config.worldSize + 5);
     landmark->vertex()->setEstimate(Vector2(x, y));
     world_.addWorldObject(std::move(landmark));
   }
 
   G2O_DEBUG("nSegments = {}", config.nSegments);
   for (int i = 0; i < config.nSegments; i++) {
     auto segment = std::make_unique<WorldObjectSegment2D>();
-    int ix = sampleUniform(-config.segmentGridSize, config.segmentGridSize,
-                           &generator_);
-    int iy = sampleUniform(-config.segmentGridSize, config.segmentGridSize,
-                           &generator_);
-    int ith = sampleUniform(0, 3, &generator_);
-    double th = (M_PI / 2) * ith;
-    th = atan2(sin(th), cos(th));
-    double xc = ix * (config.worldSize / config.segmentGridSize);
-    double yc = iy * (config.worldSize / config.segmentGridSize);
-
-    double l2 = sampleUniform(config.minSegmentLength, config.maxSegmentLength,
-                              &generator_);
-
-    double x1 = xc + cos(th) * l2;
-    double y1 = yc + sin(th) * l2;
-    double x2 = xc - cos(th) * l2;
-    double y2 = yc - sin(th) * l2;
-
-    segment->vertex()->setEstimateP1(Vector2(x1, y1));
-    segment->vertex()->setEstimateP2(Vector2(x2, y2));
+    const int ix = sampleUniform(-config.segmentGridSize,
+                                 config.segmentGridSize, &generator_);
+    const int iy = sampleUniform(-config.segmentGridSize,
+                                 config.segmentGridSize, &generator_);
+    const int ith = sampleUniform(0, 3, &generator_);
+    const Matrix2 rot_theta = Rotation2D((M_PI / 2) * ith).toRotationMatrix();
+    const Vector2 center_coords(
+        ix * (config.worldSize / config.segmentGridSize),
+        iy * (config.worldSize / config.segmentGridSize));
+    const Vector2 len_offset(
+        sampleUniform(config.minSegmentLength, config.maxSegmentLength,
+                      &generator_),
+        0.);
+
+    segment->vertex()->setEstimateP1(center_coords + rot_theta * len_offset);
+    segment->vertex()->setEstimateP2(center_coords - rot_theta * len_offset);
     world_.addWorldObject(std::move(segment));
   }
 
@@ -96,10 +96,7 @@
   if (config.hasPoseSensor) {
     G2O_DEBUG("Adding pose sensor");
     auto poseSensor = std::make_unique<SensorPose2D>("poseSensor");
-    Matrix3 poseInfo = poseSensor->information();
-    poseInfo.setIdentity();
-    poseInfo *= 500;
-    poseInfo(2, 2) = 5000;
+    const Matrix3 poseInfo = Vector3(500, 500, 5000).asDiagonal();
     poseSensor->setInformation(poseInfo);
     robot->addSensor(std::move(poseSensor), world_);
   }
@@ -113,9 +110,7 @@
   if (config.hasPointSensor) {
     G2O_DEBUG("Adding point sensor");
     auto pointSensor = std::make_unique<SensorPointXY>("pointSensor");
-    Matrix2 pointInfo = pointSensor->information();
-    pointInfo.setIdentity();
-    pointInfo *= 1000;
+    const Matrix2 pointInfo = Vector2(1000., 1000.).asDiagonal();
     pointSensor->setInformation(pointInfo);
     pointSensor->setFov(0.75 * M_PI);
     robot->addSensor(std::move(pointSensor), world_);
@@ -141,19 +136,15 @@
         std::make_unique<SensorSegment2DLine>("segmentSensorSensorLine");
     segmentSensorLine->setMaxRange(3);
     segmentSensorLine->setMinRange(.1);
-    Matrix2 m = segmentSensorLine->information();
-    m = m * 1000;
-    m(0, 0) *= 10;
+    const Matrix2 m = Vector2(10000, 1000).asDiagonal();
     segmentSensorLine->setInformation(m);
     robot->addSensor(std::move(segmentSensorLine), world_);
 
     auto segmentSensorPointLine = std::make_unique<SensorSegment2DPointLine>(
         "segmentSensorSensorPointLine");
     segmentSensorPointLine->setMaxRange(3);
     segmentSensorPointLine->setMinRange(.1);
-    Matrix3 m3 = segmentSensorPointLine->information();
-    m3 = m3 * 1000;
-    m3(2, 2) *= 10;
+    const Matrix3 m3 = Vector3(10000, 1000, 1000).asDiagonal();
     segmentSensorPointLine->setInformation(m3);
     robot->addSensor(std::move(segmentSensorPointLine), world_);
   }