Pymem simlib

g2o/simulator/CMakeLists.txt

@@ -20,6 +20,7 @@ add_library(simulator_lib ${G2O_LIB_TYPE}
    sensor_pointxyz_disparity.cpp sensor_pointxyz_disparity.h
    sensor_pointxyz_depth.cpp sensor_pointxyz_depth.h
    sensor_se3_prior.cpp sensor_se3_prior.h
+   sensor_se2_prior.cpp sensor_se2_prior.h
 
    g2o_simulator_api.h
 )

g2o/simulator/sensor_se2_prior.cpp

@@ -0,0 +1,60 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "sensor_se2_prior.h"
+
+#include <cassert>
+
+#include "g2o/core/eigen_types.h"
+
+namespace g2o {
+
+// SensorSE2Prior
+SensorSE2Prior::SensorSE2Prior(std::string name)
+    : UnarySensor<Robot2D, EdgeSE2Prior>(std::move(name)) {
+  information_ = Vector3(100., 100., 1000.).asDiagonal();
+  setInformation(information_);
+}
+
+void SensorSE2Prior::addNoise(EdgeType* e) {
+  EdgeType::ErrorVector _n = sampler_.generateSample();
+  EdgeType::Measurement n(_n);
+  e->setMeasurement(e->measurement() * n);
+  e->setInformation(information());
+}
+
+void SensorSE2Prior::sense(BaseRobot& robot, World& world) {
+  robotPoseVertex_ = robotPoseVertex<PoseVertexType>(robot, world);
+  if (!robotPoseVertex_) return;
+  sensorPose_ = robotPoseVertex_->estimate();
+  auto e = mkEdge();
+  if (!e) return;
+  world.graph().addEdge(e);
+  e->setMeasurementFromState();
+  addNoise(e.get());
+}
+
+}  // namespace g2o

g2o/simulator/sensor_se2_prior.h

@@ -0,0 +1,52 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SENSOR_SE2_PRIOR_H_
+#define G2O_SENSOR_SE2_PRIOR_H_
+
+#include "g2o/types/slam2d/edge_se2_prior.h"
+#include "g2o_simulator_api.h"
+#include "pointsensorparameters.h"
+#include "simulator2d_base.h"
+
+namespace g2o {
+
+class G2O_SIMULATOR_API SensorSE2Prior
+    : public PointSensorParameters,
+      public UnarySensor<Robot2D, EdgeSE2Prior> {
+ public:
+  using RobotPoseType = PoseVertexType::EstimateType;
+  explicit SensorSE2Prior(std::string name);
+  void sense(BaseRobot& robot, World& world) override;
+  void addNoise(EdgeType* e) override;
+
+ protected:
+  RobotPoseType sensorPose_;
+};
+
+}  // namespace g2o
+
+#endif

g2o/simulator/simulator.h

@@ -289,6 +289,18 @@ class BinarySensor : public BaseSensor {
  */
 class Simulator {
  public:
+  struct Config {
+    double worldSize = 25.;
+    int nlandmarks = 0;
+    int simSteps = 100;
+    bool hasOdom = false;
+    // Poses and landmarks
+    bool hasPoseSensor = false;
+    bool hasPointSensor = false;
+    bool hasCompass = false;
+    bool hasGPS = false;
+  };
+
   virtual ~Simulator() = default;
   virtual void setup() = 0;
   virtual void simulate() = 0;

g2o/simulator/simulator2d_base.cpp

@@ -35,6 +35,7 @@
 #include "g2o/simulator/sensor_pointxy.h"
 #include "g2o/simulator/sensor_pointxy_bearing.h"
 #include "g2o/simulator/sensor_pose2d.h"
+#include "g2o/simulator/sensor_se2_prior.h"
 #include "g2o/simulator/sensor_segment2d.h"
 #include "g2o/simulator/sensor_segment2d_line.h"
 #include "g2o/simulator/sensor_segment2d_pointline.h"
@@ -85,13 +86,15 @@ void Simulator2D::setup() {
   auto robot = std::make_unique<Robot2D>("myRobot");
 
   if (config.hasOdom) {
+    G2O_DEBUG("Adding odometry sensor");
     auto odometrySensor = std::make_unique<SensorOdometry2D>("odometry");
     const Vector3 diagonal(500, 500, 5000);
     odometrySensor->setInformation(diagonal.asDiagonal());
     robot->addSensor(std::move(odometrySensor), world_);
   }
 
   if (config.hasPoseSensor) {
+    G2O_DEBUG("Adding pose sensor");
     auto poseSensor = std::make_unique<SensorPose2D>("poseSensor");
     Matrix3 poseInfo = poseSensor->information();
     poseInfo.setIdentity();
@@ -101,7 +104,14 @@ void Simulator2D::setup() {
     robot->addSensor(std::move(poseSensor), world_);
   }
 
+  if (config.hasGPS) {
+    G2O_DEBUG("Adding GPS sensor");
+    auto gpsSensor = std::make_unique<SensorSE2Prior>("gpsSensor");
+    robot->addSensor(std::move(gpsSensor), world_);
+  }
+
   if (config.hasPointSensor) {
+    G2O_DEBUG("Adding point sensor");
     auto pointSensor = std::make_unique<SensorPointXY>("pointSensor");
     Matrix2 pointInfo = pointSensor->information();
     pointInfo.setIdentity();
@@ -112,16 +122,16 @@ void Simulator2D::setup() {
   }
 
   if (config.hasPointBearingSensor) {
+    G2O_DEBUG("Adding point bearing sensor");
     auto bearingSensor =
         std::make_unique<SensorPointXYBearing>("bearingSensor");
     bearingSensor->setInformation(bearingSensor->information() * 1000);
     robot->addSensor(std::move(bearingSensor), world_);
   }
 
   if (config.hasSegmentSensor) {
-    G2O_TRACE("creating Segment Sensor");
+    G2O_DEBUG("Adding segment sensor");
     auto segmentSensor = std::make_unique<SensorSegment2D>("segmentSensor");
-    G2O_TRACE("segmentSensorCreated");
     segmentSensor->setMaxRange(3);
     segmentSensor->setMinRange(.1);
     segmentSensor->setInformation(segmentSensor->information() * 1000);
@@ -155,6 +165,9 @@ void Simulator2D::simulate() {
   const Config& sim_conf = config;
   std::mt19937& generator = generator_;
 
+  G2O_DEBUG("Simulate {} steps in world size {}", sim_conf.simSteps,
+            sim_conf.worldSize);
+
   for (const auto& robot : world_.robots()) {
     auto* rob2d = dynamic_cast<Robot2D*>(robot.get());
     if (!rob2d) continue;

g2o/simulator/simulator2d_base.h

@@ -51,16 +51,7 @@ class G2O_SIMULATOR_API Simulator2D : public Simulator {
   /**
    * @brief Configuration of the 2D simulator
    */
-  struct Config {
-    double worldSize = 25.;
-    int nlandmarks = 0;
-    int simSteps = 100;
-    bool hasOdom = false;
-    // Poses and landmarks
-    bool hasPoseSensor = false;
-    bool hasPointSensor = false;
-    bool hasCompass = false;
-    bool hasGPS = false;
+  struct Config : public Simulator::Config {
     bool hasPointBearingSensor = false;
 
     // Segment

g2o/simulator/simulator3d_base.h

@@ -50,17 +50,9 @@ class G2O_SIMULATOR_API Simulator3D : public Simulator {
   /**
    * @brief Configuration of the 3D simulator
    */
-  struct Config {
-    double worldSize = 25.;
-    int nlandmarks = 0;
-    int simSteps = 100;
-    bool hasOdom = false;
-    bool hasPoseSensor = false;
-    bool hasPointSensor = false;
+  struct Config : public Simulator::Config {
     bool hasPointDepthSensor = false;
     bool hasPointDisparitySensor = false;
-    bool hasCompass = false;
-    bool hasGPS = false;
   };
 
   Simulator3D() = default;

g2o/types/slam2d/edge_se2_prior.cpp

@@ -45,4 +45,9 @@ void EdgeSE2Prior::setMeasurement(const SE2& m) {
   inverseMeasurement_ = m.inverse();
 }
 
+bool EdgeSE2Prior::setMeasurementFromState() {
+  setMeasurement(vertexXnRaw<0>()->estimate());
+  return true;
+}
+
 }  // namespace g2o

g2o/types/slam2d/edge_se2_prior.h

@@ -58,6 +58,8 @@ class G2O_TYPES_SLAM2D_API EdgeSE2Prior
 
   void setMeasurement(const SE2& m) override;
 
+  bool setMeasurementFromState() override;
+
   double initialEstimatePossible(const OptimizableGraph::VertexSet&,
                                  OptimizableGraph::Vertex*) override {
     return 1.;

g2o/types/slam3d/edge_se3_prior.cpp

@@ -59,13 +59,10 @@ void EdgeSE3Prior::computeError() {
 
 void EdgeSE3Prior::linearizeOplus() {
   VertexSE3* from = vertexXnRaw<0>();
+  const Isometry3& X = from->estimate();
+  const Isometry3& P = cache_->offsetParam()->param();
+  const Isometry3& Z = measurement_;
   Isometry3 E;
-  Isometry3 Z;
-  Isometry3 X;
-  Isometry3 P;
-  X = from->estimate();
-  P = cache_->offsetParam()->param();
-  Z = measurement_;
   internal::computeEdgeSE3PriorGradient(E, jacobianOplusXi_, Z, X, P);
 }
 

python/simulator/py_simulator.cpp

@@ -24,17 +24,20 @@ void declareSimulator(py::module& m) {
            static_cast<g2o::World& (Simulator::*)()>(&Simulator::world),
            py::return_value_policy::reference);
 
+  py::class_<Simulator::Config>(m, "SimulatorConfig")
+      .def(py::init<>())
+      .def_readwrite("world_size", &Simulator::Config::worldSize)
+      .def_readwrite("nlandmarks", &Simulator::Config::nlandmarks)
+      .def_readwrite("sim_steps", &Simulator::Config::simSteps)
+      .def_readwrite("has_odom", &Simulator::Config::hasOdom)
+      .def_readwrite("has_pose_sensor", &Simulator::Config::hasPoseSensor)
+      .def_readwrite("has_point_sensor", &Simulator::Config::hasPointSensor)
+      .def_readwrite("has_compass", &Simulator::Config::hasCompass)
+      .def_readwrite("has_gps", &Simulator::Config::hasGPS);
+
   // 2D Simulator
-  py::class_<Simulator2D::Config>(m, "Simulator2DConfig")
+  py::class_<Simulator2D::Config, Simulator::Config>(m, "Simulator2DConfig")
       .def(py::init<>())
-      .def_readwrite("world_size", &Simulator2D::Config::worldSize)
-      .def_readwrite("nlandmarks", &Simulator2D::Config::nlandmarks)
-      .def_readwrite("sim_steps", &Simulator2D::Config::simSteps)
-      .def_readwrite("has_odom", &Simulator2D::Config::hasOdom)
-      .def_readwrite("has_pose_sensor", &Simulator2D::Config::hasPoseSensor)
-      .def_readwrite("has_point_sensor", &Simulator2D::Config::hasPointSensor)
-      .def_readwrite("has_compass", &Simulator2D::Config::hasCompass)
-      .def_readwrite("has_gps", &Simulator2D::Config::hasGPS)
       .def_readwrite("has_point_bearing_sensor",
                      &Simulator2D::Config::hasPointBearingSensor)
       .def_readwrite("has_segment_sensor",
@@ -56,20 +59,12 @@ void declareSimulator(py::module& m) {
       .def("simulate", &Simulator2D::simulate);
 
   // 3D Simulator
-  py::class_<Simulator3D::Config>(m, "Simulator3DConfig")
+  py::class_<Simulator3D::Config, Simulator::Config>(m, "Simulator3DConfig")
       .def(py::init<>())
-      .def_readwrite("world_size", &Simulator3D::Config::worldSize)
-      .def_readwrite("nlandmarks", &Simulator3D::Config::nlandmarks)
-      .def_readwrite("sim_steps", &Simulator3D::Config::simSteps)
-      .def_readwrite("has_odom", &Simulator3D::Config::hasOdom)
-      .def_readwrite("has_pose_sensor", &Simulator3D::Config::hasPoseSensor)
-      .def_readwrite("has_point_sensor", &Simulator3D::Config::hasPointSensor)
       .def_readwrite("has_point_depth_sensor",
                      &Simulator3D::Config::hasPointDepthSensor)
       .def_readwrite("has_point_disparity_sensor",
-                     &Simulator3D::Config::hasPointDisparitySensor)
-      .def_readwrite("has_compass", &Simulator3D::Config::hasCompass)
-      .def_readwrite("has_gps", &Simulator3D::Config::hasGPS);
+                     &Simulator3D::Config::hasPointDisparitySensor);
 
   py::class_<Simulator3D, Simulator>(m, "Simulator3D")
       .def(py::init<>())