Pymem fileformat

.github/workflows/ci.yml

@@ -55,6 +55,17 @@ jobs:
           sudo apt update
           sudo apt install libeigen3-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev-qt5 libsuitesparse-dev lcov libspdlog-dev
 
+      - name: Install cereal on Ubuntu
+        if: runner.os == 'Linux'
+        run: |
+          cd /tmp
+          wget https://github.com/USCiLab/cereal/archive/refs/tags/v1.3.2.tar.gz
+          tar xaf v1.3.2.tar.gz
+          cd cereal-1.3.2
+          mkdir build; cd build
+          cmake -DJUST_INSTALL_CEREAL:=On ..
+          sudo make install
+
       - name: Install dependencies on macOS
         if: runner.os == 'macOS'
         run: brew install cmake eigen

CMakeLists.txt

@@ -490,10 +490,17 @@ else()
   include_directories(${EIGEN3_INCLUDE_DIR})
 endif ()
 
+find_package(cereal)
+
 # Generate config.h
 set(G2O_OPENGL_FOUND ${OPENGL_FOUND})
 set(G2O_HAVE_CHOLMOD ${CHOLMOD_FOUND})
 set(G2O_HAVE_CSPARSE ${G2O_USE_CSPARSE})
+if (TARGET cereal::cereal)
+  set(G2O_HAVE_CEREAL 1)
+else()
+  set(G2O_HAVE_CEREAL 0)
+endif()
 set(G2O_SHARED_LIBS ${BUILD_SHARED_LIBS})
 set(G2O_LGPL_SHARED_LIBS ${BUILD_LGPL_SHARED_LIBS})
 set(G2O_CXX_COMPILER "${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}")

README.md

@@ -96,6 +96,7 @@ following packages.
 
 -   spdlog <https://github.com/gabime/spdlog>
 -   suitesparse <http://faculty.cse.tamu.edu/davis/suitesparse.html>
+-   cereal <https://uscilab.github.io/cereal/index.html>
 -   Qt5 <http://qt-project.org>
 -   libQGLViewer <http://www.libqglviewer.com>
 
@@ -104,6 +105,7 @@ following packages.
 
 -   libspdlog-dev
 -   libsuitesparse-dev
+-   libcereal-dev
 -   qtdeclarative5-dev
 -   qt5-qmake
 -   libqglviewer-dev-qt5

config.h.in

@@ -7,6 +7,8 @@
 #cmakedefine G2O_SHARED_LIBS 1
 #cmakedefine G2O_LGPL_SHARED_LIBS 1
 
+#cmakedefine G2O_HAVE_CEREAL 1
+
 // available sparse matrix libraries
 #cmakedefine G2O_HAVE_CHOLMOD 1
 #cmakedefine G2O_HAVE_CSPARSE 1

doc/file-format.md

@@ -0,0 +1,73 @@
+# Content to be saved
+- Parameters
+- Vertex / Dynamic
+  - Data
+- Edge / Dynamic Edge
+  - Data
+
+The above list gives an overview about the data to be saved.
+Each kind of element has a set of data which is agnostic to the concrete instance and we have data specific to the instance.
+
+The agnostic data contains, for example, a single ID or a list of IDs. For a vertex and a parameter we will have an ID followed by the specific payload.
+
+# Types
+
+We have the following trivially copyable types which we need to handle.
+
+- String: A string without spaces
+- Int: An integer number
+- Float:  A floating point
+
+## Paramater
+- String, the tag of the parameter
+- Int, the unique ID of the parameter
+- [Float], the value of the parameter
+
+## Data
+- String, the tag of the data
+- [String], the data in serialized form
+
+## Vertex / Dynamic Vertex
+- String, the tag of the vertex
+- Int, the unique ID of the vertex
+- [Float], the estimate of the vertex
+- [Data], the data associated to the vertex
+
+## Edge / Dynamic Edge
+- String, the tag of the vertex
+- [Int], the unique IDs of the edge's vertices
+- [Int], the unique IDs of the edge's parameters
+- [Float], the measurement of the edge
+- [Float], the information matrix of the edge
+- [Data], the data associated to the edge
+
+A Data element belongs to a vertex or an edge in a parent/child relation.
+
+## Fixed vertices
+- [Int], the vertex IDs which are fixed
+
+# A graph
+
+A graph comprises above information. And we will save it in a specific order.
+
+- "Fixed"
+  - [Int]
+- "Parameters"
+  - [Parameter]
+- "Vertices"
+  - [Vertex]
+- "Edges"
+  - [Edge]
+
+# File formats to support
+
+We want to save the original g2o file format but also support potentially new file formats like JSON or a binary format.
+
+## Challenges
+
+To support the original g2o file format, we need to handle the special case of a dynamic edge. However, we can assume that not many files with dynamically sized edges in the g2o format exist.
+To support the original g2o format, we need anyhow to implement a custom reader for the data. Hence, we can handle in there the special format to separate IDs, estimate and information matrix.
+
+As we assume only a very limited number of files have been written with dynamically sized edges, we will break compatibility for reading those. This allows as a smooth path forward.
+
+Currently, g2o can only implicitly differentiate if a edge is dynamic. To this end, an edge with zero vertices upon construction is assumed to be a dynamic edge. The same holds for a vertex.

g2o/apps/g2o_simulator/CMakeLists.txt

@@ -1,7 +1,7 @@
 add_library(g2o_simulator_library ${G2O_LIB_TYPE}
    simulator.cpp simulator.h
    pointsensorparameters.cpp  pointsensorparameters.h
-   simulator2d_base.h simulator2d.h
+   simulator2d_base.h
    sensor_odometry2d.cpp sensor_odometry2d.h
    sensor_pose2d.cpp sensor_pose2d.h
    sensor_pointxy.cpp sensor_pointxy.h
@@ -12,7 +12,7 @@ add_library(g2o_simulator_library ${G2O_LIB_TYPE}
    sensor_segment2d_line.cpp  sensor_segment2d_line.h
    sensor_segment2d_pointline.cpp  sensor_segment2d_pointline.h
 
-   simulator3d_base.h    simulator3d.h
+   simulator3d_base.h
    sensor_odometry3d.cpp sensor_odometry3d.h
    sensor_pose3d.cpp sensor_pose3d.h
    sensor_pose3d_offset.cpp sensor_pose3d_offset.h

g2o/apps/g2o_simulator/pointsensorparameters.h

@@ -36,13 +36,15 @@ namespace g2o {
 class G2O_SIMULATOR_API PointSensorParameters {
  public:
   PointSensorParameters() = default;
-  double maxRange() const { return sqrt(maxRange2_); }
+  [[nodiscard]] double maxRange() const { return sqrt(maxRange2_); }
   void setMaxRange(double maxRange);
-  double minRange() const { return sqrt(minRange2_); }
+  [[nodiscard]] double minRange() const { return sqrt(minRange2_); }
   void setMinRange(double minRange);
-  double fov() const { return fov_; }
+  [[nodiscard]] double fov() const { return fov_; }
   void setFov(double fov);
-  double maxAngularDifference() const { return maxAngularDifference_; }
+  [[nodiscard]] double maxAngularDifference() const {
+    return maxAngularDifference_;
+  }
   void setMaxAngularDifference(double angularDifference);
 
  protected:

g2o/apps/g2o_simulator/sensor_odometry2d.h

@@ -27,6 +27,7 @@
 #ifndef G2O_SENSOR_ODOMETRY2D_H_
 #define G2O_SENSOR_ODOMETRY2D_H_
 
+#include "g2o/types/slam2d/edge_se2.h"
 #include "g2o_simulator_api.h"
 #include "simulator2d_base.h"
 

g2o/apps/g2o_simulator/sensor_odometry3d.h

@@ -27,6 +27,7 @@
 #ifndef G2O_SENSOR_ODOMETRY3D_H_
 #define G2O_SENSOR_ODOMETRY3D_H_
 
+#include "g2o/types/slam3d/edge_se3.h"
 #include "g2o_simulator_api.h"
 #include "simulator3d_base.h"
 

g2o/apps/g2o_simulator/sensor_pointxy.h

@@ -27,6 +27,7 @@
 #ifndef G2O_SENSOR_POINTXY_H_
 #define G2O_SENSOR_POINTXY_H_
 
+#include "g2o/types/slam2d/edge_se2_pointxy.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator2d_base.h"

g2o/apps/g2o_simulator/sensor_pointxy_bearing.h

@@ -27,6 +27,7 @@
 #ifndef G2O_SENSOR_POINTXY_BEARING_H_
 #define G2O_SENSOR_POINTXY_BEARING_H_
 
+#include "g2o/types/slam2d/edge_se2_pointxy_bearing.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator2d_base.h"

g2o/apps/g2o_simulator/sensor_pointxy_offset.cpp

@@ -28,8 +28,6 @@
 
 #include <cassert>
 
-#include "g2o/core/factory.h"
-
 namespace g2o {
 
 // SensorPointXYOffset
@@ -82,7 +80,7 @@ void SensorPointXYOffset::sense() {
     count++;
   }
   if (!robotPoseObject_) return;
-  sensorPose_ = robotPoseObject_->vertex()->estimate() * offsetParam_->offset();
+  sensorPose_ = robotPoseObject_->vertex()->estimate() * offsetParam_->param();
   for (auto* it : world()->objects()) {
     auto* o = dynamic_cast<WorldObjectType*>(it);
     if (o && isVisible(o)) {

g2o/apps/g2o_simulator/sensor_pointxy_offset.h

@@ -27,6 +27,7 @@
 #ifndef G2O_SENSOR_POINTXY_OFFSET_H_
 #define G2O_SENSOR_POINTXY_OFFSET_H_
 
+#include "g2o/types/slam2d/edge_se2_pointxy_offset.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator2d_base.h"

g2o/apps/g2o_simulator/sensor_pointxyz.cpp

@@ -81,7 +81,7 @@ void SensorPointXYZ::sense() {
     count++;
   }
   if (!robotPoseObject_) return;
-  sensorPose_ = robotPoseObject_->vertex()->estimate() * offsetParam_->offset();
+  sensorPose_ = robotPoseObject_->vertex()->estimate() * offsetParam_->param();
   for (auto* it : world()->objects()) {
     auto* o = dynamic_cast<WorldObjectType*>(it);
     if (o && isVisible(o)) {

g2o/apps/g2o_simulator/sensor_pointxyz.h

@@ -26,6 +26,7 @@
 
 #ifndef G2O_SENSOR_POINTXYZ_H_
 #define G2O_SENSOR_POINTXYZ_H_
+#include "g2o/types/slam3d/edge_se3_pointxyz.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator3d_base.h"

g2o/apps/g2o_simulator/sensor_pointxyz_depth.cpp

@@ -78,7 +78,8 @@ void SensorPointXYZDepth::sense() {
     count++;
   }
   if (!robotPoseObject_) return;
-  sensorPose_ = robotPoseObject_->vertex()->estimate() * offsetParam_->offset();
+  sensorPose_ =
+      robotPoseObject_->vertex()->estimate() * offsetParam_->param().offset();
   for (auto* it : world()->objects()) {
     auto* o = dynamic_cast<WorldObjectType*>(it);
     if (o && isVisible(o)) {

g2o/apps/g2o_simulator/sensor_pointxyz_depth.h

@@ -26,6 +26,8 @@
 
 #ifndef G2O_SENSOR_POINTXYZ_DEPTH_H_
 #define G2O_SENSOR_POINTXYZ_DEPTH_H_
+#include "g2o/types/slam3d/edge_se3_pointxyz_depth.h"
+#include "g2o/types/slam3d/parameter_camera.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator3d_base.h"
@@ -40,13 +42,13 @@ class G2O_SIMULATOR_API SensorPointXYZDepth
   explicit SensorPointXYZDepth(const std::string& name);
   void sense() override;
   void addParameters() override;
-  std::shared_ptr<ParameterSE3Offset> offsetParam() { return offsetParam_; };
+  std::shared_ptr<ParameterCamera> offsetParam() { return offsetParam_; };
   void addNoise(EdgeType* e) override;
 
  protected:
   bool isVisible(WorldObjectType* to);
   RobotPoseType sensorPose_;
-  std::shared_ptr<ParameterSE3Offset> offsetParam_;
+  std::shared_ptr<ParameterCamera> offsetParam_;
 };
 }  // namespace g2o
 

g2o/apps/g2o_simulator/sensor_pointxyz_disparity.cpp

@@ -82,7 +82,8 @@ void SensorPointXYZDisparity::sense() {
     count++;
   }
   if (!robotPoseObject_) return;
-  sensorPose_ = robotPoseObject_->vertex()->estimate() * offsetParam_->offset();
+  sensorPose_ =
+      robotPoseObject_->vertex()->estimate() * offsetParam_->param().offset();
   for (auto* it : world()->objects()) {
     auto* o = dynamic_cast<WorldObjectType*>(it);
     if (o && isVisible(o)) {

g2o/apps/g2o_simulator/sensor_pointxyz_disparity.h

@@ -26,6 +26,8 @@
 
 #ifndef G2O_SENSOR_POINTXYZ_DISPARITY_H_
 #define G2O_SENSOR_POINTXYZ_DISPARITY_H_
+#include "g2o/types/slam3d/edge_se3_pointxyz_disparity.h"
+#include "g2o/types/slam3d/parameter_camera.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator3d_base.h"
@@ -41,13 +43,13 @@ class G2O_SIMULATOR_API SensorPointXYZDisparity
   explicit SensorPointXYZDisparity(const std::string& name);
   void sense() override;
   void addParameters() override;
-  std::shared_ptr<ParameterSE3Offset> offsetParam() { return offsetParam_; };
+  std::shared_ptr<ParameterCamera> offsetParam() { return offsetParam_; };
   void addNoise(EdgeType* e) override;
 
  protected:
   bool isVisible(WorldObjectType* to);
   RobotPoseType sensorPose_;
-  std::shared_ptr<ParameterSE3Offset> offsetParam_;
+  std::shared_ptr<ParameterCamera> offsetParam_;
 };
 
 }  // namespace g2o

g2o/apps/g2o_simulator/sensor_pose2d.h

@@ -27,6 +27,7 @@
 #ifndef G2O_SENSOR_POSE2D_H_
 #define G2O_SENSOR_POSE2D_H_
 
+#include "g2o/types/slam2d/edge_se2.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator2d_base.h"

g2o/apps/g2o_simulator/sensor_pose3d.h

@@ -26,6 +26,7 @@
 
 #ifndef G2O_SENSOR_POSE3D_H_
 #define G2O_SENSOR_POSE3D_H_
+#include "g2o/types/slam3d/edge_se3.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator3d_base.h"

g2o/apps/g2o_simulator/sensor_pose3d_offset.h

@@ -27,6 +27,8 @@
 #ifndef G2O_SENSOR_POSE3D_OFFSET_H_
 #define G2O_SENSOR_POSE3D_OFFSET_H_
 
+#include "g2o/types/slam3d/edge_se3_offset.h"
+#include "g2o/types/slam3d/parameter_se3_offset.h"
 #include "pointsensorparameters.h"
 #include "simulator3d_base.h"
 
@@ -38,7 +40,7 @@ class SensorPose3DOffset
  public:
   explicit SensorPose3DOffset(const std::string& name);
   void sense() override;
-  int stepsToIgnore() const { return stepsToIgnore_; }
+  [[nodiscard]] int stepsToIgnore() const { return stepsToIgnore_; }
   void setStepsToIgnore(int stepsToIgnore) { stepsToIgnore_ = stepsToIgnore; }
   void addNoise(EdgeType* e) override;
   void addParameters() override;

g2o/apps/g2o_simulator/sensor_se3_prior.cpp

@@ -67,7 +67,7 @@ void SensorSE3Prior::sense() {
     count++;
   }
   if (!robotPoseObject_) return;
-  sensorPose_ = robotPoseObject_->vertex()->estimate() * offsetParam_->offset();
+  sensorPose_ = robotPoseObject_->vertex()->estimate() * offsetParam_->param();
   auto e = mkEdge();
   if (e && graph()) {
     e->setParameterId(0, offsetParam_->id());

g2o/apps/g2o_simulator/sensor_se3_prior.h

@@ -26,6 +26,7 @@
 
 #ifndef G2O_SENSOR_SE3_PRIOR_H_
 #define G2O_SENSOR_SE3_PRIOR_H_
+#include "g2o/types/slam3d/edge_se3_prior.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator3d_base.h"