diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 78f1add0f..47fbd06a7 100644 --- a/.github/workflows/ci.yml +++ b/.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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 52dc8588f..390066d00 100644 --- a/CMakeLists.txt +++ b/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}") diff --git a/Makefile b/Makefile deleted file mode 100644 index 2083de93c..000000000 --- a/Makefile +++ /dev/null @@ -1,22 +0,0 @@ -SHELL = /bin/bash - -ifeq ($(VERBOSE), 1) -QUIET= -else -QUIET=-s --no-print-directory -endif - -all: build/Makefile - @ $(MAKE) $(QUIET) -C build - -debug: build/Makefile - @ $(MAKE) $(QUIET) -C build - -clean: build/Makefile - @ $(MAKE) $(QUIET) -C build clean - -build/Makefile: - @ echo "Running cmake to generate Makefile"; \ - cd build; \ - cmake ../; \ - cd - diff --git a/README.md b/README.md index d819daf4d..01908d69b 100644 --- a/README.md +++ b/README.md @@ -96,6 +96,7 @@ following packages. - spdlog - suitesparse +- cereal - Qt5 - libQGLViewer @@ -104,6 +105,7 @@ following packages. - libspdlog-dev - libsuitesparse-dev +- libcereal-dev - qtdeclarative5-dev - qt5-qmake - libqglviewer-dev-qt5 diff --git a/config.h.in b/config.h.in index dc91effbf..63610523d 100644 --- a/config.h.in +++ b/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 diff --git a/doc/file-format.md b/doc/file-format.md new file mode 100644 index 000000000..2574e2ce2 --- /dev/null +++ b/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. diff --git a/g2o/apps/g2o_simulator/CMakeLists.txt b/g2o/apps/g2o_simulator/CMakeLists.txt index 3e67fab03..820ec4b80 100644 --- a/g2o/apps/g2o_simulator/CMakeLists.txt +++ b/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 diff --git a/g2o/apps/g2o_simulator/pointsensorparameters.h b/g2o/apps/g2o_simulator/pointsensorparameters.h index 1ddeb8bf3..aa28e0bed 100644 --- a/g2o/apps/g2o_simulator/pointsensorparameters.h +++ b/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: diff --git a/g2o/apps/g2o_simulator/sensor_odometry2d.h b/g2o/apps/g2o_simulator/sensor_odometry2d.h index 376d5a56d..5f2358e90 100644 --- a/g2o/apps/g2o_simulator/sensor_odometry2d.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_odometry3d.h b/g2o/apps/g2o_simulator/sensor_odometry3d.h index ca02e1958..5c2780b81 100644 --- a/g2o/apps/g2o_simulator/sensor_odometry3d.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_pointxy.h b/g2o/apps/g2o_simulator/sensor_pointxy.h index 2cfccc2ff..eb584bde8 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxy.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_pointxy_bearing.h b/g2o/apps/g2o_simulator/sensor_pointxy_bearing.h index 799aa80af..23b3bbb96 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxy_bearing.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_pointxy_offset.cpp b/g2o/apps/g2o_simulator/sensor_pointxy_offset.cpp index da23f5f39..556a2f5c2 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxy_offset.cpp +++ b/g2o/apps/g2o_simulator/sensor_pointxy_offset.cpp @@ -28,8 +28,6 @@ #include -#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(it); if (o && isVisible(o)) { diff --git a/g2o/apps/g2o_simulator/sensor_pointxy_offset.h b/g2o/apps/g2o_simulator/sensor_pointxy_offset.h index 2382d736e..dfc9a9d69 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxy_offset.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_pointxyz.cpp b/g2o/apps/g2o_simulator/sensor_pointxyz.cpp index 659d7231c..a5c9f1714 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxyz.cpp +++ b/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(it); if (o && isVisible(o)) { diff --git a/g2o/apps/g2o_simulator/sensor_pointxyz.h b/g2o/apps/g2o_simulator/sensor_pointxyz.h index f9daf79eb..3d7c78d6e 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxyz.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_pointxyz_depth.cpp b/g2o/apps/g2o_simulator/sensor_pointxyz_depth.cpp index 4ea1df95a..64bffb493 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxyz_depth.cpp +++ b/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(it); if (o && isVisible(o)) { diff --git a/g2o/apps/g2o_simulator/sensor_pointxyz_depth.h b/g2o/apps/g2o_simulator/sensor_pointxyz_depth.h index fdf19b6c0..f4cacbda8 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxyz_depth.h +++ b/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 offsetParam() { return offsetParam_; }; + std::shared_ptr offsetParam() { return offsetParam_; }; void addNoise(EdgeType* e) override; protected: bool isVisible(WorldObjectType* to); RobotPoseType sensorPose_; - std::shared_ptr offsetParam_; + std::shared_ptr offsetParam_; }; } // namespace g2o diff --git a/g2o/apps/g2o_simulator/sensor_pointxyz_disparity.cpp b/g2o/apps/g2o_simulator/sensor_pointxyz_disparity.cpp index 6d52ccec0..5568b903b 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxyz_disparity.cpp +++ b/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(it); if (o && isVisible(o)) { diff --git a/g2o/apps/g2o_simulator/sensor_pointxyz_disparity.h b/g2o/apps/g2o_simulator/sensor_pointxyz_disparity.h index 87fd1910b..e79871dda 100644 --- a/g2o/apps/g2o_simulator/sensor_pointxyz_disparity.h +++ b/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 offsetParam() { return offsetParam_; }; + std::shared_ptr offsetParam() { return offsetParam_; }; void addNoise(EdgeType* e) override; protected: bool isVisible(WorldObjectType* to); RobotPoseType sensorPose_; - std::shared_ptr offsetParam_; + std::shared_ptr offsetParam_; }; } // namespace g2o diff --git a/g2o/apps/g2o_simulator/sensor_pose2d.h b/g2o/apps/g2o_simulator/sensor_pose2d.h index 296cea5a3..df736cf54 100644 --- a/g2o/apps/g2o_simulator/sensor_pose2d.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_pose3d.h b/g2o/apps/g2o_simulator/sensor_pose3d.h index 366fbb9fc..20b201cbe 100644 --- a/g2o/apps/g2o_simulator/sensor_pose3d.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_pose3d_offset.h b/g2o/apps/g2o_simulator/sensor_pose3d_offset.h index 6f7cad716..9f8cac5b1 100644 --- a/g2o/apps/g2o_simulator/sensor_pose3d_offset.h +++ b/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; diff --git a/g2o/apps/g2o_simulator/sensor_se3_prior.cpp b/g2o/apps/g2o_simulator/sensor_se3_prior.cpp index f77aec10d..8e552e828 100644 --- a/g2o/apps/g2o_simulator/sensor_se3_prior.cpp +++ b/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()); diff --git a/g2o/apps/g2o_simulator/sensor_se3_prior.h b/g2o/apps/g2o_simulator/sensor_se3_prior.h index 5fcc47767..8e32ae5b0 100644 --- a/g2o/apps/g2o_simulator/sensor_se3_prior.h +++ b/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" diff --git a/g2o/apps/g2o_simulator/sensor_segment2d.h b/g2o/apps/g2o_simulator/sensor_segment2d.h index c89a88b20..a0bec633a 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d.h +++ b/g2o/apps/g2o_simulator/sensor_segment2d.h @@ -28,8 +28,8 @@ #define G2O_SENSOR_SEGMENT2D_H_ #include "g2o/apps/g2o_simulator/pointsensorparameters.h" -#include "g2o/types/slam2d_addons/types_slam2d_addons.h" -#include "simulator2d.h" +#include "g2o/apps/g2o_simulator/simulator2d_base.h" +#include "g2o/types/slam2d_addons/edge_se2_segment2d.h" namespace g2o { diff --git a/g2o/apps/g2o_simulator/sensor_segment2d_line.h b/g2o/apps/g2o_simulator/sensor_segment2d_line.h index 954da0a92..d61729a41 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d_line.h +++ b/g2o/apps/g2o_simulator/sensor_segment2d_line.h @@ -28,8 +28,8 @@ #define G2O_SENSOR_SEGMENT2D_LINE_H_ #include "g2o/apps/g2o_simulator/pointsensorparameters.h" -#include "g2o/types/slam2d_addons/types_slam2d_addons.h" -#include "simulator2d.h" +#include "g2o/apps/g2o_simulator/simulator2d_base.h" +#include "g2o/types/slam2d_addons/edge_se2_segment2d_line.h" namespace g2o { diff --git a/g2o/apps/g2o_simulator/sensor_segment2d_pointline.h b/g2o/apps/g2o_simulator/sensor_segment2d_pointline.h index 592883f15..0e0bb9847 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d_pointline.h +++ b/g2o/apps/g2o_simulator/sensor_segment2d_pointline.h @@ -28,8 +28,8 @@ #define G2O_SENSOR_SEGMENT2D_POINTLINE_H_ #include "g2o/apps/g2o_simulator/pointsensorparameters.h" -#include "g2o/types/slam2d_addons/types_slam2d_addons.h" -#include "simulator2d.h" +#include "g2o/apps/g2o_simulator/simulator2d_base.h" +#include "g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h" namespace g2o { diff --git a/g2o/apps/g2o_simulator/simulator.h b/g2o/apps/g2o_simulator/simulator.h index 2c7cc4e61..d3ad1ef2a 100644 --- a/g2o/apps/g2o_simulator/simulator.h +++ b/g2o/apps/g2o_simulator/simulator.h @@ -30,11 +30,9 @@ #include #include #include -#include -#include "g2o/config.h" +#include "g2o/core/optimizable_graph.h" #include "g2o/stuff/sampler.h" -#include "g2o/types/slam3d/types_slam3d.h" #include "g2o_simulator_api.h" namespace g2o { diff --git a/g2o/apps/g2o_simulator/simulator2d.h b/g2o/apps/g2o_simulator/simulator2d.h deleted file mode 100644 index 06f7c21c0..000000000 --- a/g2o/apps/g2o_simulator/simulator2d.h +++ /dev/null @@ -1,40 +0,0 @@ -// 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_SIMULATOR2D_ -#define G2O_SIMULATOR2D_ - -#include "sensor_odometry2d.h" -#include "sensor_pointxy.h" -#include "sensor_pointxy_bearing.h" -#include "sensor_pointxy_offset.h" -#include "sensor_pose2d.h" -#include "sensor_segment2d.h" -#include "sensor_segment2d_line.h" -#include "sensor_segment2d_pointline.h" -#include "simulator2d_base.h" - -#endif diff --git a/g2o/apps/g2o_simulator/simulator2d_base.h b/g2o/apps/g2o_simulator/simulator2d_base.h index 519fbee94..fb05bc5f5 100644 --- a/g2o/apps/g2o_simulator/simulator2d_base.h +++ b/g2o/apps/g2o_simulator/simulator2d_base.h @@ -27,8 +27,9 @@ #ifndef G2O_SIMULATOR2D_BASE_H_ #define G2O_SIMULATOR2D_BASE_H_ -#include "g2o/types/slam2d/types_slam2d.h" -#include "g2o/types/slam2d_addons/types_slam2d_addons.h" +#include "g2o/types/slam2d/vertex_point_xy.h" +#include "g2o/types/slam2d/vertex_se2.h" +#include "g2o/types/slam2d_addons/vertex_segment2d.h" #include "simulator.h" namespace g2o { diff --git a/g2o/apps/g2o_simulator/simulator3d.h b/g2o/apps/g2o_simulator/simulator3d.h deleted file mode 100644 index ab8287773..000000000 --- a/g2o/apps/g2o_simulator/simulator3d.h +++ /dev/null @@ -1,38 +0,0 @@ -// 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_SIMULATOR3D_ -#define G2O_SIMULATOR3D_ - -#include "sensor_odometry3d.h" -#include "sensor_pointxyz.h" -#include "sensor_pointxyz_depth.h" -#include "sensor_pointxyz_disparity.h" -#include "sensor_pose3d.h" -#include "sensor_pose3d_offset.h" -#include "sensor_se3_prior.h" - -#endif diff --git a/g2o/apps/g2o_simulator/simulator3d_base.h b/g2o/apps/g2o_simulator/simulator3d_base.h index 88c2b1625..c5931ad5b 100644 --- a/g2o/apps/g2o_simulator/simulator3d_base.h +++ b/g2o/apps/g2o_simulator/simulator3d_base.h @@ -27,8 +27,9 @@ #ifndef G2O_SIMULATOR3D_BASE_H_ #define G2O_SIMULATOR3D_BASE_H_ -#include "g2o/types/slam3d/types_slam3d.h" -#include "g2o/types/slam3d_addons/types_slam3d_addons.h" +#include "g2o/types/slam3d/vertex_pointxyz.h" +#include "g2o/types/slam3d/vertex_se3.h" +#include "g2o/types/slam3d_addons/vertex_line3d.h" #include "simulator.h" namespace g2o { diff --git a/g2o/apps/g2o_simulator/test_simulator2d.cpp b/g2o/apps/g2o_simulator/test_simulator2d.cpp index 96f067297..757395269 100644 --- a/g2o/apps/g2o_simulator/test_simulator2d.cpp +++ b/g2o/apps/g2o_simulator/test_simulator2d.cpp @@ -24,18 +24,24 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include #include #include #include +#include "g2o/apps/g2o_simulator/sensor_odometry2d.h" +#include "g2o/apps/g2o_simulator/sensor_pointxy.h" +#include "g2o/apps/g2o_simulator/sensor_pointxy_bearing.h" +#include "g2o/apps/g2o_simulator/sensor_pose2d.h" +#include "g2o/apps/g2o_simulator/sensor_segment2d.h" +#include "g2o/apps/g2o_simulator/sensor_segment2d_line.h" +#include "g2o/apps/g2o_simulator/sensor_segment2d_pointline.h" +#include "g2o/apps/g2o_simulator/simulator.h" +#include "g2o/apps/g2o_simulator/simulator2d_base.h" #include "g2o/core/optimizable_graph.h" #include "g2o/stuff/command_args.h" #include "g2o/stuff/sampler.h" -#include "simulator2d.h" using std::cerr; -using std::endl; int main(int argc, char** argv) { g2o::CommandArgs arg; @@ -94,7 +100,7 @@ int main(int argc, char** argv) { world.addWorldObject(landmark); } - cerr << "nSegments = " << nSegments << endl; + cerr << "nSegments = " << nSegments << '\n'; for (int i = 0; i < nSegments; i++) { auto* segment = new g2o::WorldObjectSegment2D; @@ -168,9 +174,9 @@ int main(int argc, char** argv) { } if (hasSegmentSensor) { - cerr << "creating Segment Sensor" << endl; + cerr << "creating Segment Sensor\n"; auto* segmentSensor = new g2o::SensorSegment2D("segmentSensor"); - cerr << "segmentSensorCreated" << endl; + cerr << "segmentSensorCreated\n"; segmentSensor->setMaxRange(3); segmentSensor->setMinRange(.1); robot.addSensor(segmentSensor); diff --git a/g2o/apps/g2o_simulator/test_simulator3d.cpp b/g2o/apps/g2o_simulator/test_simulator3d.cpp index 45a8e2700..6dcf43f20 100644 --- a/g2o/apps/g2o_simulator/test_simulator3d.cpp +++ b/g2o/apps/g2o_simulator/test_simulator3d.cpp @@ -24,14 +24,20 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include #include #include +#include "g2o/apps/g2o_simulator/sensor_odometry3d.h" +#include "g2o/apps/g2o_simulator/sensor_pointxyz.h" +#include "g2o/apps/g2o_simulator/sensor_pointxyz_depth.h" +#include "g2o/apps/g2o_simulator/sensor_pointxyz_disparity.h" +#include "g2o/apps/g2o_simulator/sensor_pose3d.h" +#include "g2o/apps/g2o_simulator/simulator.h" +#include "g2o/apps/g2o_simulator/simulator3d_base.h" #include "g2o/core/optimizable_graph.h" #include "g2o/stuff/command_args.h" #include "g2o/stuff/sampler.h" -#include "simulator3d.h" +#include "g2o/types/slam3d/parameter_camera.h" // #define _POSE_SENSOR_OFFSET // #define _POSE_PRIOR_SENSOR @@ -106,7 +112,7 @@ int main(int argc, char** argv) { pointSensor->setMaxRange(2.); cameraPose = R; cameraPose.translation() = g2o::Vector3(0., 0., 0.3); - pointSensor->offsetParam()->setOffset(cameraPose); + pointSensor->offsetParam()->setParam(cameraPose); ss << "-pointXYZ"; } @@ -116,12 +122,10 @@ int main(int argc, char** argv) { disparitySensor->setMinRange(0.5); disparitySensor->setMaxRange(2.); robot.addSensor(disparitySensor); - Eigen::Isometry3d cameraPose; - Eigen::Matrix3d R; - R << 0, 0, 1, -1, 0, 0, 0, -1, 0; - cameraPose = R; - cameraPose.translation() = g2o::Vector3(0., 0., 0.3); - disparitySensor->offsetParam()->setOffset(cameraPose); + g2o::CameraWithOffset cameraPose; + cameraPose.offset().linear() << 0, 0, 1, -1, 0, 0, 0, -1, 0; + cameraPose.offset().translation() = g2o::Vector3(0., 0., 0.3); + disparitySensor->offsetParam()->setParam(cameraPose); ss << "-disparity"; } @@ -131,12 +135,10 @@ int main(int argc, char** argv) { depthSensor->setMinRange(0.5); depthSensor->setMaxRange(2.); robot.addSensor(depthSensor); - Eigen::Isometry3d cameraPose; - Eigen::Matrix3d R; - R << 0, 0, 1, -1, 0, 0, 0, -1, 0; - cameraPose = R; - cameraPose.translation() = g2o::Vector3(0., 0., 0.3); - depthSensor->offsetParam()->setOffset(cameraPose); + g2o::CameraWithOffset cameraPose; + cameraPose.offset().linear() << 0, 0, 1, -1, 0, 0, 0, -1, 0; + cameraPose.offset().translation() = g2o::Vector3(0., 0., 0.3); + depthSensor->offsetParam()->setParam(cameraPose); ss << "-depth"; } diff --git a/g2o/apps/g2o_viewer/base_main_window.ui b/g2o/apps/g2o_viewer/base_main_window.ui index cf3f2c3f6..120f93701 100644 --- a/g2o/apps/g2o_viewer/base_main_window.ui +++ b/g2o/apps/g2o_viewer/base_main_window.ui @@ -180,13 +180,6 @@ - - - - SetZero - - - diff --git a/g2o/apps/g2o_viewer/main_window.cpp b/g2o/apps/g2o_viewer/main_window.cpp index 7dc7e32f7..a8f9c9b24 100644 --- a/g2o/apps/g2o_viewer/main_window.cpp +++ b/g2o/apps/g2o_viewer/main_window.cpp @@ -128,15 +128,6 @@ void MainWindow::on_btnInitialGuess_clicked() { viewer->update(); } -void MainWindow::on_btnSetZero_clicked() { - if (viewer->graph->activeEdges().empty()) - viewer->graph->initializeOptimization(); - - viewer->graph->setToOrigin(); - viewer->setUpdateDisplay(true); - viewer->update(); -} - void MainWindow::on_btnReload_clicked() { if (filename_.length() > 0) { std::cerr << "reloading " << filename_ << '\n'; diff --git a/g2o/apps/g2o_viewer/main_window.h b/g2o/apps/g2o_viewer/main_window.h index bd18d424e..e16561ae6 100644 --- a/g2o/apps/g2o_viewer/main_window.h +++ b/g2o/apps/g2o_viewer/main_window.h @@ -71,7 +71,6 @@ class G2O_VIEWER_API MainWindow : public QMainWindow, void on_btnOptimize_clicked(); void on_btnInitialGuess_clicked(); - void on_btnSetZero_clicked(); void on_btnForceStop_clicked(); void on_btnOptimizerParameters_clicked(); void on_btnReload_clicked(); diff --git a/g2o/core/CMakeLists.txt b/g2o/core/CMakeLists.txt index 8568bec4c..09b748a3f 100644 --- a/g2o/core/CMakeLists.txt +++ b/g2o/core/CMakeLists.txt @@ -37,8 +37,13 @@ jacobian_workspace.cpp jacobian_workspace.h robust_kernel.cpp robust_kernel.h robust_kernel_impl.cpp robust_kernel_impl.h robust_kernel_factory.cpp robust_kernel_factory.h -io_helper.h g2o_core_api.h +# IO +abstract_graph.cpp abstract_graph.h +io/io_binary.cpp io/io_binary.h +io/io_json.cpp io/io_json.h +io/io_g2o.cpp io/io_g2o.h +io/io_xml.cpp io/io_xml.h ) target_include_directories(core PUBLIC @@ -52,6 +57,9 @@ set_target_properties(core PROPERTIES SOVERSION ${G2O_LIB_SOVERSION}) target_link_libraries(core PUBLIC stuff ${G2O_EIGEN3_EIGEN_TARGET}) target_link_libraries(core PUBLIC g2o_ceres_ad) +if (TARGET cereal::cereal) +target_link_libraries(core PUBLIC cereal::cereal) +endif() target_compile_features(core PUBLIC cxx_std_17) if (APPLE) @@ -67,5 +75,7 @@ install(TARGETS core ) file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") - install(FILES ${headers} DESTINATION ${INCLUDES_INSTALL_DIR}/core) + +file(GLOB io_headers "${CMAKE_CURRENT_SOURCE_DIR}/io/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/io/*.hpp") +install(FILES ${io_headers} DESTINATION ${INCLUDES_INSTALL_DIR}/core/io) diff --git a/g2o/core/abstract_graph.cpp b/g2o/core/abstract_graph.cpp new file mode 100644 index 000000000..e78a95d2c --- /dev/null +++ b/g2o/core/abstract_graph.cpp @@ -0,0 +1,126 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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 "abstract_graph.h" + +#include +#include + +#include "g2o/core/io/io_binary.h" +#include "g2o/core/io/io_format.h" +#include "g2o/core/io/io_json.h" +#include "g2o/core/io/io_xml.h" +#include "g2o/stuff/logger.h" +#include "io/io_g2o.h" + +namespace { +#ifdef G2O_HAVE_LOGGING +std::string_view to_string(g2o::io::Format format) { + switch (format) { + case g2o::io::Format::kG2O: + return "G2O"; + case g2o::io::Format::kBinary: + return "Binary"; + case g2o::io::Format::kJson: + return "JSON"; + case g2o::io::Format::kXML: + return "XML"; + } + return ""; +} +#endif + +/** + * @brief Allocate an g2o::IoInterface to load/save data of the graph. + * + * @param format see g2o::io::Format + * @return std::unique_ptr or nullptr for unsupported format. + */ +std::unique_ptr allocate(g2o::io::Format format) { + switch (format) { + case g2o::io::Format::kG2O: + return std::make_unique(); + case g2o::io::Format::kBinary: + return std::make_unique(); + case g2o::io::Format::kJson: + return std::make_unique(); + case g2o::io::Format::kXML: + return std::make_unique(); + } + G2O_CRITICAL("Failed to create graph IO interface for format {}", + to_string(format)); + return nullptr; +} + +} // namespace + +namespace g2o { + +bool AbstractGraph::load(std::istream& input, io::Format format) { + std::unique_ptr loader_interface = allocate(format); + if (!loader_interface) return false; + std::optional load_result = loader_interface->load(input); + if (!load_result.has_value()) { + G2O_ERROR("Failure while loading graph, result will be empty"); + return false; + } + *this = std::move(load_result.value()); + return true; +} + +bool AbstractGraph::save(std::ostream& output, io::Format format) const { + std::unique_ptr loader_interface = allocate(format); + if (!loader_interface) return false; + return loader_interface->save(output, *this); +} + +void AbstractGraph::clear() { + fixed_.clear(); + parameters_.clear(); + vertices_.clear(); + edges_.clear(); +} + +void AbstractGraph::renameTags( + const std::unordered_map& tag_mapping) { + auto map_tag = [&tag_mapping](const std::string& tag) { + auto it = tag_mapping.find(tag); + if (it == tag_mapping.end()) return tag; + return it->second; + }; + + for (auto& entry : vertices_) { + entry.tag = map_tag(entry.tag); + for (auto& c : entry.data) c.tag = map_tag(c.tag); + } + for (auto& entry : edges_) { + entry.tag = map_tag(entry.tag); + for (auto& c : entry.data) c.tag = map_tag(c.tag); + } + for (auto& entry : parameters_) entry.tag = map_tag(entry.tag); +} + +} // namespace g2o diff --git a/g2o/core/abstract_graph.h b/g2o/core/abstract_graph.h new file mode 100644 index 000000000..557472096 --- /dev/null +++ b/g2o/core/abstract_graph.h @@ -0,0 +1,147 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_CORE_ABSTRACT_GRAPH_H +#define G2O_CORE_ABSTRACT_GRAPH_H + +#include +#include +#include +#include +#include + +#include "g2o_core_api.h" +#include "io/io_format.h" + +namespace g2o { + +class OptimizableGraph; + +/** + * @brief An abstract graph holding the data of vertices and edges + * + * An abstract graph which holds all the data of a optimization problem + * represented as a factor graph. It consists of nodes and edges. Additionally, + * each node/edge can hold a list of data. Furthermore, parameters can be + * stored. Whereas a parameter represents an agnostic part of the optimization + * problem such as parameters of a camera. + */ +class G2O_CORE_API AbstractGraph { + public: + struct G2O_CORE_API AbstractData { + std::string tag; ///< the tag of this data + std::string data; ///< the serialized data as a string + AbstractData() = default; + AbstractData(std::string tag, std::string data) + : tag(std::move(tag)), data(std::move(data)) {} + }; + + struct G2O_CORE_API AbstractParameter { + std::string tag; ///< the tag of this parameter + int id; ///< its ID + std::vector value; ///< its value as a vector + AbstractParameter() = default; + AbstractParameter(std::string tag, int id, std::vector value) + : tag(std::move(tag)), id(id), value(std::move(value)) {} + }; + + struct G2O_CORE_API AbstractGraphElement { + std::string tag; + std::vector data; ///< the associated data + AbstractGraphElement() = default; + AbstractGraphElement(std::string tag, std::vector data) + : tag(std::move(tag)), data(std::move(data)) {} + }; + + struct G2O_CORE_API AbstractVertex : public AbstractGraphElement { + int id; ///< its ID + std::vector estimate; ///< the estimate as a vector + AbstractVertex() = default; + AbstractVertex(std::string tag, int id, std::vector estimate, + std::vector data = {}) + : AbstractGraphElement(std::move(tag), std::move(data)), + id(id), + estimate(std::move(estimate)) {} + }; + + struct G2O_CORE_API AbstractEdge : public AbstractGraphElement { + std::vector ids; ///< the ids of the vertices connected by this edge + std::vector param_ids; ///< the ids of the parameters of this edge + std::vector measurement; ///< the measurement as a vector + std::vector + information; ///< upper triangular part of the information matrix + AbstractEdge() = default; + AbstractEdge(std::string tag, std::vector ids, + std::vector measurement, + std::vector information, + std::vector param_ids = {}, + std::vector data = {}) + : AbstractGraphElement(std::move(tag), std::move(data)), + ids(std::move(ids)), + param_ids(std::move(param_ids)), + measurement(std::move(measurement)), + information(std::move(information)) {} + }; + + AbstractGraph() = default; + ~AbstractGraph() = default; + + bool load(std::istream& input, io::Format format); + + [[nodiscard]] bool save(std::ostream& output, io::Format format) const; + + std::vector& fixed() { return fixed_; }; + [[nodiscard]] const std::vector& fixed() const { return fixed_; }; + + std::vector& parameters() { return parameters_; }; + [[nodiscard]] const std::vector& parameters() const { + return parameters_; + }; + + std::vector& vertices() { return vertices_; }; + [[nodiscard]] const std::vector& vertices() const { + return vertices_; + }; + + std::vector& edges() { return edges_; }; + [[nodiscard]] const std::vector& edges() const { + return edges_; + }; + + void clear(); + + void renameTags( + const std::unordered_map& tag_mapping); + + protected: + std::vector fixed_; + std::vector parameters_; + std::vector vertices_; + std::vector edges_; +}; +} // namespace g2o + +#endif diff --git a/g2o/core/base_dynamic_vertex.h b/g2o/core/base_dynamic_vertex.h index 3b0508c80..8826ca3b1 100644 --- a/g2o/core/base_dynamic_vertex.h +++ b/g2o/core/base_dynamic_vertex.h @@ -35,7 +35,7 @@ namespace g2o { template class BaseDynamicVertex : public BaseVertex<-1, T> { public: - inline virtual bool setDimension(int newDimension); + bool setDimension(int newDimension) override; protected: // This method is responsible for actually changing the dimension of the state diff --git a/g2o/core/base_edge.h b/g2o/core/base_edge.h index 991356976..32fb3fb39 100644 --- a/g2o/core/base_edge.h +++ b/g2o/core/base_edge.h @@ -28,7 +28,7 @@ #define G2O_BASE_EDGE_H #include -#include +#include #include #include "g2o/core/type_traits.h" @@ -107,7 +107,8 @@ class BaseEdge : public OptimizableGraph::Edge { return information_; } EIGEN_STRONG_INLINE InformationType& information() { return information_; } - void setInformation(const InformationType& information) { + template + void setInformation(const Eigen::EigenBase& information) { information_ = information; } @@ -143,10 +144,13 @@ class BaseEdge : public OptimizableGraph::Edge { error_.resize(dim, 1); } + [[nodiscard]] int dimensionAtCompileTime() const final { return kDimension; } + // methods based on the traits interface bool setMeasurementData(const double* d) final { + if (d == nullptr) return false; static_assert(TypeTraits::kVectorDimension != INT_MIN, - "Forgot to implement TypeTrait for your Measurement"); + "Forgot to implement TypeTraits for your Measurement"); typename TypeTraits::VectorType::ConstMapType aux( d, DimensionTraits::dimension(measurement_)); setMeasurement(TypeTraits::fromVector(aux)); @@ -155,7 +159,7 @@ class BaseEdge : public OptimizableGraph::Edge { bool getMeasurementData(double* d) const final { static_assert(TypeTraits::kVectorDimension != INT_MIN, - "Forgot to implement TypeTrait for your Measurement"); + "Forgot to implement TypeTraits for your Measurement"); typename TypeTraits::VectorType::MapType aux( d, DimensionTraits::dimension(measurement_)); TypeTraits::toData(measurement_, d); @@ -163,9 +167,17 @@ class BaseEdge : public OptimizableGraph::Edge { } [[nodiscard]] int measurementDimension() const final { + return DimensionTraits::dimension(measurement_); + } + + [[nodiscard]] int measurementDimensionAtCompileTime() const override { return TypeTraits::kVectorDimension; } + [[nodiscard]] int minimalMeasurementDimension() const final { + return DimensionTraits::minimalDimension(measurement_); + } + //! Return the identity information matrix of this edge type InformationType informationIdentity() const { if constexpr (D != Eigen::Dynamic) { @@ -195,40 +207,6 @@ class BaseEdge : public OptimizableGraph::Edge { // weightedError.transpose()); return result; } - - //! write the upper trinagular part of the information matrix into the stream - bool writeInformationMatrix(std::ostream& os) const { - for (int i = 0; i < information().rows(); ++i) - for (int j = i; j < information().cols(); ++j) - os << information()(i, j) << " "; - return os.good(); - } - //! reads the upper triangular part of the matrix and recovers the missing - //! symmetrical elements - bool readInformationMatrix(std::istream& is) { - for (int i = 0; i < information().rows() && is.good(); ++i) - for (int j = i; j < information().cols() && is.good(); ++j) { - is >> information()(i, j); - if (i != j) information()(j, i) = information()(i, j); - } - return is.good() || is.eof(); - } - //! write the param IDs that are potentially used by the edge - bool writeParamIds(std::ostream& os) const { - for (auto id : parameterIds_) os << id << " "; - return os.good(); - } - //! reads the param IDs from the stream - bool readParamIds(std::istream& is) { - for (size_t i = 0; i < numParameters(); ++i) { - int paramId; - is >> paramId; - setParameterId(i, paramId); - } - return is.good() || is.eof(); - } - - public: }; } // end namespace g2o diff --git a/g2o/core/base_fixed_sized_edge.h b/g2o/core/base_fixed_sized_edge.h index f65201fa9..ca40c952c 100644 --- a/g2o/core/base_fixed_sized_edge.h +++ b/g2o/core/base_fixed_sized_edge.h @@ -111,14 +111,14 @@ std::tuple createHessianMaps(const std::tuple&) { } template -typename std::enable_if::type -createNthVertexType(int /*i*/, const EdgeType& /*t*/, CtorArgs... /*args*/) { +std::enable_if_t createNthVertexType( + int /*i*/, const EdgeType& /*t*/, CtorArgs... /*args*/) { return nullptr; } template -typename std::enable_if::type -createNthVertexType(int i, const EdgeType& t, CtorArgs... args) { +std::enable_if_t createNthVertexType( + int i, const EdgeType& t, CtorArgs... args) { if (i == I) { using VertexType = typename EdgeType::template VertexXnType; return new VertexType(args...); @@ -147,11 +147,11 @@ class BaseFixedSizedEdge : public BaseEdge { */ // clang-format off template - constexpr typename std::enable_if::kDimension != -1, int>::type vertexDimension() const { + constexpr std::enable_if_t::kDimension != -1, int> vertexDimension() const { return VertexXnType::kDimension; }; template - typename std::enable_if::kDimension == -1, int>::type vertexDimension() const { + std::enable_if_t::kDimension == -1, int> vertexDimension() const { return vertexXn()->dimension(); }; // clang-format on @@ -204,8 +204,8 @@ class BaseFixedSizedEdge : public BaseEdge { using type = std::tuple...>; using typeTransposed = std::tuple...>; }; - static const std::size_t kNrOfVertices = sizeof...(VertexTypes); - static const std::size_t kNrOfVertexPairs = + static constexpr std::size_t kNrOfVertices = sizeof...(VertexTypes); + static constexpr std::size_t kNrOfVertexPairs = internal::pair_to_index(0, kNrOfVertices); using HessianTuple = typename HessianTupleType< std::make_index_sequence>::type; @@ -228,9 +228,8 @@ class BaseFixedSizedEdge : public BaseEdge { OptimizableGraph::Vertex* createVertex(int i, CtorArgs... args) { if (i < 0) return nullptr; return internal::createNthVertexType< - sizeof...(VertexTypes) - 1, - typename std::remove_reference::type, CtorArgs...>( - i, *this, args...); + sizeof...(VertexTypes) - 1, std::remove_reference_t, + CtorArgs...>(i, *this, args...); }; void resize(size_t size) override; @@ -299,6 +298,10 @@ class BaseFixedSizedEdge : public BaseEdge { using BaseEdge::resize; using BaseEdge::computeError; + [[nodiscard]] int numVerticesAtCompileTime() const final { + return kNrOfVertices; + } + protected: using BaseEdge::measurement_; using BaseEdge::information_; diff --git a/g2o/core/base_vertex.h b/g2o/core/base_vertex.h index 3d80f3d15..0ef53c007 100644 --- a/g2o/core/base_vertex.h +++ b/g2o/core/base_vertex.h @@ -34,20 +34,14 @@ #include #include #include -#include -#include #include #include #include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" #include "g2o/core/type_traits.h" #include "optimizable_graph.h" namespace g2o { -template -class BaseVertex; - #define G2O_VERTEX_DIM ((D == Eigen::Dynamic) ? dimension_ : D) /** @@ -64,7 +58,7 @@ template class BaseVertex : public OptimizableGraph::Vertex { public: using EstimateType = T; - using BackupStackType = std::stack >; + using BackupStackType = std::stack>; static const int kDimension = D; ///< dimension of the estimate (minimal) in the manifold space @@ -97,9 +91,7 @@ class BaseVertex : public OptimizableGraph::Vertex { void clearQuadraticForm() final { b_.setZero(); } - //! updates the current vertex with the direct solution x += H_ii\b_ii - //! @returns the determinant of the inverted hessian - double solveDirect(double lambda = 0) override; + bool solveDirect(double lambda = 0) override; //! return right hand side b of the constructed linear system BVector& b() { return b_; } @@ -129,49 +121,38 @@ class BaseVertex : public OptimizableGraph::Vertex { updateCache(); } - //! generic read based on Trait reading a vector - bool read(std::istream& is) override { - typename TypeTraits::VectorType estimate_data; - bool state = internal::readVector(is, estimate_data); - setEstimate(TypeTraits::fromVector(estimate_data)); - return state; - } - - //! generic write based on Trait reading a vector - bool write(std::ostream& os) const override { - return internal::writeVector( - os, TypeTraits::toVector(estimate())); - } - - // methods based on the traits interface - void setToOriginImpl() final { - setEstimate(TypeTraits::Identity()); - } - bool setEstimateData(const double* est) final { + if (est == nullptr) return false; static_assert(TypeTraits::kVectorDimension != INT_MIN, "Forgot to implement TypeTrait for your Estimate"); typename TypeTraits::VectorType::ConstMapType aux( est, DimensionTraits::dimension(estimate_)); - estimate_ = TypeTraits::fromVector(aux); - updateCache(); + setEstimate(TypeTraits::fromVector(aux)); return true; } + using OptimizableGraph::Vertex::setEstimateData; bool getEstimateData(double* est) const final { + if (est == nullptr) return false; static_assert(TypeTraits::kVectorDimension != INT_MIN, "Forgot to implement TypeTrait for your Estimate"); TypeTraits::toData(estimate(), est); return true; } + using OptimizableGraph::Vertex::getEstimateData; [[nodiscard]] int estimateDimension() const final { static_assert(TypeTraits::kVectorDimension != INT_MIN, "Forgot to implement TypeTrait for your Estimate"); + return DimensionTraits::dimension(estimate_); + } + + [[nodiscard]] int estimateDimensionAtCompileTime() const override { return TypeTraits::kVectorDimension; } bool setMinimalEstimateData(const double* est) final { + if (est == nullptr) return false; static_assert(TypeTraits::kMinimalVectorDimension != INT_MIN, "Forgot to implement TypeTrait for your Estimate"); typename TypeTraits::MinimalVectorType::ConstMapType aux( @@ -179,18 +160,21 @@ class BaseVertex : public OptimizableGraph::Vertex { setEstimate(TypeTraits::fromMinimalVector(aux)); return true; } + using OptimizableGraph::Vertex::setMinimalEstimateData; bool getMinimalEstimateData(double* est) const final { + if (est == nullptr) return false; static_assert(TypeTraits::kMinimalVectorDimension != INT_MIN, "Forgot to implement TypeTrait for your Estimate"); TypeTraits::toMinimalData(estimate(), est); return true; } + using OptimizableGraph::Vertex::getMinimalEstimateData; [[nodiscard]] int minimalEstimateDimension() const final { static_assert(TypeTraits::kMinimalVectorDimension != INT_MIN, "Forgot to implement TypeTrait for your Estimate"); - return TypeTraits::kMinimalVectorDimension; + return DimensionTraits::minimalDimension(estimate_); } protected: @@ -198,8 +182,6 @@ class BaseVertex : public OptimizableGraph::Vertex { BVector b_; EstimateType estimate_; BackupStackType backup_; - - public: }; template @@ -209,17 +191,19 @@ BaseVertex::BaseVertex() } template -double BaseVertex::solveDirect(double lambda) { - Eigen::Matrix tempA = - hessian_ + Eigen::Matrix::Identity( - G2O_VERTEX_DIM, G2O_VERTEX_DIM) * - lambda; - double det = tempA.determinant(); - if (std::isnan(det) || det < std::numeric_limits::epsilon()) - return det; - BVector dx = tempA.llt().solve(b_); +bool BaseVertex::solveDirect(double lambda) { + const MatrixN tempA = + (abs(lambda) < 1e-10) + ? hessian_ + : (hessian_ + + MatrixN( + VectorN::Constant(G2O_VERTEX_DIM, lambda).asDiagonal())) + .eval(); + Eigen::LLT> cholesky(tempA); + if (cholesky.info() != Eigen::ComputationInfo::Success) return false; + BVector dx = cholesky.solve(b_); oplus(VectorX::MapType(dx.data(), dx.size())); - return det; + return true; } template diff --git a/g2o/core/block_solver.h b/g2o/core/block_solver.h index 93199ab41..fba66b466 100644 --- a/g2o/core/block_solver.h +++ b/g2o/core/block_solver.h @@ -601,7 +601,7 @@ bool BlockSolver::solve() { double* cp = coefficients_.get(); double* xl = x_ + sizePoses_; - double* cl = coefficients_.get() + sizePoses_; + double* cl = cp + sizePoses_; double* bl = b_ + sizePoses_; // cp = -xp diff --git a/g2o/core/factory.cpp b/g2o/core/factory.cpp index e56722d3d..214a01e7e 100644 --- a/g2o/core/factory.cpp +++ b/g2o/core/factory.cpp @@ -32,6 +32,8 @@ #include #include "creators.h" +#include "g2o/core/optimizable_graph.h" +#include "g2o/core/parameter.h" #include "g2o/stuff/logger.h" #include "hyper_graph.h" @@ -74,10 +76,10 @@ void Factory::registerType( G2O_DEBUG("Factory {} registering {}", static_cast(this), tag); G2O_DEBUG("{}", static_cast(c)); switch (element->elementType()) { - case HyperGraph::HGET_VERTEX: + case HyperGraph::kHgetVertex: G2O_DEBUG(" -> Vertex"); break; - case HyperGraph::HGET_EDGE: + case HyperGraph::kHgetEdge: G2O_DEBUG(" -> Edge"); break; case HyperGraph::HGET_PARAMETER: @@ -97,8 +99,42 @@ void Factory::registerType( std::unique_ptr ci = std::make_unique(); - ci->elementTypeBit = element->elementType(); + ci->type_info.elementTypeBit = element->elementType(); ci->creator = std::move(c); + // TODO(rainer): simplify storing dimension information + switch (element->elementType()) { + case HyperGraph::kHgetVertex: { + auto* v = static_cast(element.get()); + ci->type_info.dimension = v->estimateDimension(); + ci->type_info.dimension_at_compile_time = + v->estimateDimensionAtCompileTime(); + ci->type_info.minimal_dimension = v->minimalEstimateDimension(); + ci->type_info.minimal_dimension = v->minimalEstimateDimension(); + } break; + case HyperGraph::kHgetEdge: { + auto* e = static_cast(element.get()); + ci->type_info.dimension = e->measurementDimension(); + ci->type_info.dimension_at_compile_time = + e->measurementDimensionAtCompileTime(); + ci->type_info.minimal_dimension = e->minimalMeasurementDimension(); + ci->type_info.number_vertices = e->vertices().size(); + ci->type_info.number_vertices_at_compile_time = + e->numVerticesAtCompileTime(); + ci->type_info.number_parameters = e->numParameters(); + ci->type_info.error_dimension = e->dimension(); + ci->type_info.error_dimension_at_compile_time = + e->dimensionAtCompileTime(); + + } break; + case HyperGraph::kHgetParameter: { + auto* p = static_cast(element.get()); + ci->type_info.dimension = p->parameterDimension(); + ci->type_info.minimal_dimension = p->minimalParameterDimension(); + } break; + default: // not handled on purpose + break; + } + tagLookup_[ci->creator->name()] = tag; creator_[tag] = std::move(ci); } @@ -146,10 +182,16 @@ bool Factory::knowsTag(const std::string& tag, int* elementType) const { if (elementType) *elementType = -1; return false; } - if (elementType) *elementType = foundIt->second->elementTypeBit; + if (elementType) *elementType = foundIt->second->type_info.elementTypeBit; return true; } +Factory::TypeInfo Factory::typeInfo(const std::string& tag) const { + auto foundIt = creator_.find(tag); + if (foundIt == creator_.end()) return TypeInfo(); + return foundIt->second->type_info; +} + void Factory::destroy() { std::unique_ptr aux; factoryInstance_.swap(aux); @@ -171,8 +213,9 @@ std::unique_ptr Factory::construct( return construct(tag); } auto foundIt = creator_.find(tag); - if (foundIt != creator_.end() && foundIt->second->elementTypeBit >= 0 && - elemsToConstruct.test(foundIt->second->elementTypeBit)) { + if (foundIt != creator_.end() && + foundIt->second->type_info.elementTypeBit >= 0 && + elemsToConstruct.test(foundIt->second->type_info.elementTypeBit)) { return foundIt->second->creator->construct(); } return nullptr; diff --git a/g2o/core/factory.h b/g2o/core/factory.h index 3a8e95edc..b6cf09a01 100644 --- a/g2o/core/factory.h +++ b/g2o/core/factory.h @@ -49,6 +49,17 @@ namespace g2o { */ class G2O_CORE_API Factory { public: + struct TypeInfo { + int elementTypeBit = -1; + int dimension = -1; + int dimension_at_compile_time = -1; + int minimal_dimension = -1; + int number_vertices = -1; + int number_vertices_at_compile_time = -1; + int number_parameters = -1; + int error_dimension = -1; + int error_dimension_at_compile_time = -1; + }; //! return the instance static Factory* instance(); @@ -89,6 +100,8 @@ class G2O_CORE_API Factory { */ bool knowsTag(const std::string& tag, int* elementType = nullptr) const; + [[nodiscard]] TypeInfo typeInfo(const std::string& tag) const; + //! return the TAG given a vertex const std::string& tag(const HyperGraph::HyperGraphElement* e) const; @@ -106,7 +119,7 @@ class G2O_CORE_API Factory { class CreatorInformation { public: std::unique_ptr creator; - int elementTypeBit = -1; + TypeInfo type_info; }; using CreatorMap = std::map>; diff --git a/g2o/core/hyper_graph.h b/g2o/core/hyper_graph.h index f93cb1879..303b1353c 100644 --- a/g2o/core/hyper_graph.h +++ b/g2o/core/hyper_graph.h @@ -103,8 +103,7 @@ class G2O_CORE_API HyperGraph { virtual bool read(std::istream& is) = 0; //! write the data to a stream virtual bool write(std::ostream& os) const = 0; - [[nodiscard]] HyperGraph::HyperGraphElementType elementType() - const override { + [[nodiscard]] HyperGraph::HyperGraphElementType elementType() const final { return HyperGraph::kHgetData; } [[nodiscard]] std::shared_ptr next() const { return next_; } @@ -165,7 +164,7 @@ class G2O_CORE_API HyperGraph { [[nodiscard]] const EdgeSetWeak& edges() const { return edges_; } //! returns the set of hyper-edges that are leaving/entering in this vertex EdgeSetWeak& edges() { return edges_; } - [[nodiscard]] HyperGraphElementType elementType() const override { + [[nodiscard]] HyperGraphElementType elementType() const final { return kHgetVertex; } @@ -223,7 +222,7 @@ class G2O_CORE_API HyperGraph { [[nodiscard]] int id() const { return id_; } void setId(int id); - [[nodiscard]] HyperGraphElementType elementType() const override { + [[nodiscard]] HyperGraphElementType elementType() const final { return kHgetEdge; } diff --git a/g2o/types/slam3d_addons/vertex_se3_euler.h b/g2o/core/io/io_binary.cpp similarity index 62% rename from g2o/types/slam3d_addons/vertex_se3_euler.h rename to g2o/core/io/io_binary.cpp index 2db2e2c0b..166e12480 100644 --- a/g2o/types/slam3d_addons/vertex_se3_euler.h +++ b/g2o/core/io/io_binary.cpp @@ -24,27 +24,46 @@ // 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_VERTEX_SE3_EULER_ -#define G2O_VERTEX_SE3_EULER_ +#include "io_binary.h" -#include +#include -#include "g2o/types/slam3d/vertex_se3.h" -#include "g2o_types_slam3d_addons_api.h" +#include "g2o/config.h" +#include "g2o/core/abstract_graph.h" + +#ifdef G2O_HAVE_CEREAL +#include +#include + +#include "io_wrapper_cereal.h" // IWYU pragma: keep +#else +#include "g2o/stuff/logger.h" +#endif // HAVE CEREAL namespace g2o { -/** - * \brief 3D pose Vertex, (x,y,z,roll,pitch,yaw) - * the internal parameterization is the same as vertex_se3_quat. - * Only the read/write operations are rewritten to input/output euler angles. - */ -class G2O_TYPES_SLAM3D_ADDONS_API VertexSE3Euler : public VertexSE3 { - public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; -}; +#ifdef G2O_HAVE_CEREAL -} // namespace g2o +std::optional IoBinary::load(std::istream& input) { + return io::load(input, "BINARY"); +} + +bool IoBinary::save(std::ostream& output, const AbstractGraph& graph) { + return io::save(output, graph, "BINARY"); +} + +#else + +std::optional IoBinary::load(std::istream&) { + G2O_WARN("Loading BINARY is not supported"); + return std::nullopt; +} + +bool IoBinary::save(std::ostream&, const AbstractGraph&) { + G2O_WARN("Saving BINARY is not supported"); + return false; +} #endif + +} // namespace g2o diff --git a/g2o/types/slam3d_addons/vertex_se3_euler.cpp b/g2o/core/io/io_binary.h similarity index 76% rename from g2o/types/slam3d_addons/vertex_se3_euler.cpp rename to g2o/core/io/io_binary.h index 228f98d46..dda1abd44 100644 --- a/g2o/types/slam3d_addons/vertex_se3_euler.cpp +++ b/g2o/core/io/io_binary.h @@ -24,23 +24,20 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include "vertex_se3_euler.h" +#ifndef G2O_CORE_IO_BINARY_FORMAT_H +#define G2O_CORE_IO_BINARY_FORMAT_H -#include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" -#include "g2o/types/slam3d/isometry3d_mappings.h" +#include "g2o/core/g2o_core_api.h" +#include "io_interface.h" namespace g2o { -bool VertexSE3Euler::read(std::istream& is) { - Vector6 est; - bool state = internal::readVector(is, est); - setEstimate(internal::fromVectorET(est)); - return state; -} - -bool VertexSE3Euler::write(std::ostream& os) const { - return internal::writeVector(os, internal::toVectorET(estimate())); -} +class G2O_CORE_API IoBinary : public IoInterface { + public: + std::optional load(std::istream& input) override; + bool save(std::ostream& output, const AbstractGraph& graph) override; +}; } // namespace g2o + +#endif diff --git a/g2o/examples/tutorial_slam2d/types_tutorial_slam2d.h b/g2o/core/io/io_format.h similarity index 86% rename from g2o/examples/tutorial_slam2d/types_tutorial_slam2d.h rename to g2o/core/io/io_format.h index 3bdaaedb0..88487e8cb 100644 --- a/g2o/examples/tutorial_slam2d/types_tutorial_slam2d.h +++ b/g2o/core/io/io_format.h @@ -24,12 +24,14 @@ // 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_TYPES_TUTORIAL_SLAM2D_ -#define G2O_TYPES_TUTORIAL_SLAM2D_ +#ifndef G2O_CORE_IO_FORMAT_H +#define G2O_CORE_IO_FORMAT_H -#include "edge_se2.h" -#include "edge_se2_pointxy.h" -#include "parameter_se2_offset.h" -#include "vertex_se2.h" +#include "g2o/core/g2o_core_api.h" +namespace g2o::io { + +enum class G2O_CORE_API Format { kG2O = 0, kBinary = 1, kJson = 2, kXML = 3 }; + +} // namespace g2o::io #endif diff --git a/g2o/core/io/io_g2o.cpp b/g2o/core/io/io_g2o.cpp new file mode 100644 index 000000000..9747a7557 --- /dev/null +++ b/g2o/core/io/io_g2o.cpp @@ -0,0 +1,246 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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 "io_g2o.h" + +#include +#include +#include +#include +#include + +#include "g2o/core/abstract_graph.h" +#include "g2o/core/factory.h" +#include "g2o/stuff/logger.h" +#include "g2o/stuff/logger_format.h" // IWYU pragma: keep +#include "g2o/stuff/string_tools.h" + +namespace { +std::ostream& operator<<(std::ostream& os, const std::vector& v) { + for (size_t i = 0; i < v.size(); ++i) { + if (i > 0) os << " "; + os << v[i]; + } + return os; +} + +std::ostream& operator<<(std::ostream& os, const std::vector& v) { + for (size_t i = 0; i < v.size(); ++i) { + if (i > 0) os << " "; + os << v[i]; + } + return os; +} +} // namespace + +namespace g2o { + +std::optional IoG2O::load(std::istream& input) { + Factory* factory = Factory::instance(); + std::unordered_set warned_unknown_types; + + AbstractGraph result; + AbstractGraph::AbstractGraphElement* last_data_container = nullptr; + + std::stringstream current_line; + std::string token; + + int line_number = 0; + while (true) { + const int bytesRead = readLine(input, current_line); + line_number++; + if (bytesRead == -1) break; + current_line >> token; + if (bytesRead == 0 || token.empty() || token[0] == '#') continue; + + // handle FIX command encoded in the input + if (token == "FIX") { + int id; + while (current_line >> id) { + result.fixed().emplace_back(id); + G2O_TRACE("Fixing vertex {}", id); + } + continue; + } + + token = mapType(token); + G2O_TRACE("Reading token {}", token); + Factory::TypeInfo type_info = factory->typeInfo(token); + if (type_info.elementTypeBit < 0) { + if (warned_unknown_types.count(token) != 1) { + warned_unknown_types.insert(token); + G2O_ERROR("Unknown type {} in line {}", token, line_number); + } + continue; + } + + // first handle the parameters + if (type_info.elementTypeBit == HyperGraph::kHgetParameter) { + AbstractGraph::AbstractParameter parameter; + parameter.tag = token; + current_line >> parameter.id; + double value; + while (current_line >> value) parameter.value.emplace_back(value); + result.parameters().emplace_back(parameter); + continue; + } + + // it's a vertex type + if (type_info.elementTypeBit == HyperGraph::kHgetVertex) { + AbstractGraph::AbstractVertex vertex; + vertex.tag = token; + current_line >> vertex.id; + // read estimate vector + if (type_info.dimension_at_compile_time < 0) { + int estimate_size = 0; + current_line >> estimate_size; + vertex.estimate.resize(estimate_size); + } else + vertex.estimate.resize(type_info.dimension); + for (auto& v : vertex.estimate) current_line >> v; + if (!current_line) { + G2O_ERROR("Error reading vertex {} at line {}", token, line_number); + continue; + } + result.vertices().emplace_back(vertex); + G2O_TRACE("Read vertex {} with estimate [{}]", vertex.tag, + fmt::join(vertex.estimate, ",")); + last_data_container = &result.vertices().back(); + } else if (type_info.elementTypeBit == HyperGraph::kHgetEdge) { + AbstractGraph::AbstractEdge edge; + edge.tag = token; + if (type_info.number_vertices_at_compile_time < 0) { + // reading the IDs of a dynamically sized edge + int number_of_vertices = 0; + current_line >> number_of_vertices; + edge.ids.resize(number_of_vertices); + } else { + edge.ids.resize(type_info.number_vertices_at_compile_time); + } + for (auto& id : edge.ids) current_line >> id; + // read the parameter ids + if (type_info.number_parameters > 0) { + edge.param_ids.resize(type_info.number_parameters); + for (auto& param_id : edge.param_ids) current_line >> param_id; + } + // read measurement vector + if (type_info.dimension_at_compile_time < 0) { + int measurement_size = 0; + current_line >> measurement_size; + edge.measurement.resize(measurement_size); + } else + edge.measurement.resize(type_info.dimension); + for (auto& v : edge.measurement) current_line >> v; + // read upper triangle of the information matrix + if (type_info.error_dimension_at_compile_time < 0) { + int information_size = 0; + current_line >> information_size; + edge.information.resize(information_size); + } else { + const int& min_dim = type_info.error_dimension; + edge.information.resize((min_dim * (min_dim + 1)) / 2); + } + for (auto& v : edge.information) current_line >> v; + + if (!current_line) { + G2O_ERROR("Error reading edge {} at line {}", token, line_number); + continue; + } + result.edges().emplace_back(edge); + G2O_TRACE( + "Read edge {} connecting [{}] with measurement [{}] and information " + "[{}]", + edge.tag, fmt::join(edge.ids, ","), fmt::join(edge.measurement, ","), + fmt::join(edge.information, ",")); + last_data_container = &result.edges().back(); + } else if (type_info.elementTypeBit == HyperGraph::kHgetData) { + if (!last_data_container) { + G2O_ERROR( + "Error reading data {} at line {}: got data element, but no data " + "container available", + token, line_number); + } + AbstractGraph::AbstractData data; + data.tag = token; + std::stringstream remaining_line; + remaining_line << current_line.rdbuf(); + data.data = trim(remaining_line.str()); + G2O_TRACE("Read data {} with content {}", data.tag, data.data); + last_data_container->data.emplace_back(data); + } + } + + return result; +} + +bool IoG2O::save(std::ostream& output, const AbstractGraph& graph) { + Factory* factory = Factory::instance(); + + for (const auto& param : graph.parameters()) { + output << param.tag << " " << param.id << " " << param.value << '\n'; + } + + auto printData = [](std::ostream& output, + const std::vector& data) { + for (const auto& d : data) { + output << d.tag << " " << d.data << '\n'; + } + }; + + for (const auto& vertex : graph.vertices()) { + const Factory::TypeInfo type_info = factory->typeInfo(vertex.tag); + output << vertex.tag << " " << vertex.id << " "; + if (type_info.dimension_at_compile_time < 0) + output << vertex.estimate.size() << " "; + output << vertex.estimate << '\n'; + printData(output, vertex.data); + } + + for (const auto& edge : graph.edges()) { + const Factory::TypeInfo type_info = factory->typeInfo(edge.tag); + output << edge.tag << " "; + if (type_info.number_vertices_at_compile_time < 0) + output << edge.ids.size() << " "; + output << edge.ids << " "; + if (!edge.param_ids.empty()) output << edge.param_ids << " "; + if (type_info.dimension_at_compile_time < 0) + output << edge.measurement.size() << " "; + output << edge.measurement << " "; + if (type_info.error_dimension_at_compile_time < 0) + output << edge.information.size() << " "; + output << edge.information << "\n"; + printData(output, edge.data); + } + + // After the vertices to be backward compatible + if (!graph.fixed().empty()) { + output << "FIX " << graph.fixed() << '\n'; + } + + return output.good(); +} + +} // namespace g2o diff --git a/g2o/core/io/io_g2o.h b/g2o/core/io/io_g2o.h new file mode 100644 index 000000000..1dcdc4e1b --- /dev/null +++ b/g2o/core/io/io_g2o.h @@ -0,0 +1,45 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_CORE_IO_G2O_FORMAT_H +#define G2O_CORE_IO_G2O_FORMAT_H + +#include "g2o/core/g2o_core_api.h" +#include "io_interface.h" + +namespace g2o { + +class AbstractGraph; + +class G2O_CORE_API IoG2O : public IoInterface { + public: + std::optional load(std::istream& input) override; + bool save(std::ostream& output, const AbstractGraph& graph) override; +}; + +} // namespace g2o + +#endif diff --git a/g2o/types/slam3d_addons/edge_se3_euler.h b/g2o/core/io/io_interface.h similarity index 53% rename from g2o/types/slam3d_addons/edge_se3_euler.h rename to g2o/core/io/io_interface.h index a94893356..fb8c1eb0d 100644 --- a/g2o/types/slam3d_addons/edge_se3_euler.h +++ b/g2o/core/io/io_interface.h @@ -24,25 +24,60 @@ // 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_EDGE_SE3_EULER_ -#define G2O_EDGE_SE3_EULER_ +#ifndef G2O_CORE_IO_INTERFACE_H +#define G2O_CORE_IO_INTERFACE_H #include +#include +#include +#include -#include "g2o/types/slam3d/edge_se3.h" -#include "g2o/types/slam3d/vertex_se3.h" -#include "g2o_types_slam3d_addons_api.h" +#include "g2o/core/g2o_core_api.h" namespace g2o { +class AbstractGraph; + /** - * \brief 3D edge between two VertexSE3, uses the euler angle parameterization - * for the read/write functions *only*. + * @brief Base interface class for IO of a g2o graph */ -class G2O_TYPES_SLAM3D_ADDONS_API EdgeSE3Euler : public EdgeSE3 { +class G2O_CORE_API IoInterface { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + virtual ~IoInterface() = default; + + /** + * @brief Set the Renamed Types lookup table + * + * @param mapping the map containing the name mapping for types + */ + void setRenamedTypes( + const std::unordered_map& mapping) { + renamed_types_ = mapping; + } + + /** + * @brief Return the mapping for renaming types + * + * @return const std::unordered_map& + */ + const std::unordered_map& renamedTypes() const { + return renamed_types_; + } + + //! abstract method loading a graph + virtual std::optional load(std::istream& input) = 0; + //! abstract method for saving a graph + virtual bool save(std::ostream& output, const AbstractGraph& graph) = 0; + + protected: + std::unordered_map renamed_types_; + + std::string mapType(const std::string& token) { + if (renamed_types_.empty()) return token; + auto foundIt = renamed_types_.find(token); + if (foundIt == renamed_types_.end()) return token; + return foundIt->second; + } }; } // namespace g2o diff --git a/g2o/types/slam3d/types_slam3d.h b/g2o/core/io/io_json.cpp similarity index 64% rename from g2o/types/slam3d/types_slam3d.h rename to g2o/core/io/io_json.cpp index e1f2b80e6..89e84c3bc 100644 --- a/g2o/types/slam3d/types_slam3d.h +++ b/g2o/core/io/io_json.cpp @@ -24,29 +24,46 @@ // 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_TYPES_SLAM3D_ -#define G2O_TYPES_SLAM3D_ +#include "io_json.h" + +#include #include "g2o/config.h" -#include "g2o/core/base_binary_edge.h" -#include "g2o/core/base_vertex.h" -#include "g2o/core/hyper_graph_action.h" - -#define THREE_D_TYPES_ANALYTIC_JACOBIAN - -#include "edge_pointxyz.h" -#include "edge_se3.h" -#include "edge_se3_lotsofxyz.h" -#include "edge_se3_offset.h" -#include "edge_se3_pointxyz.h" -#include "edge_se3_pointxyz_depth.h" -#include "edge_se3_pointxyz_disparity.h" -#include "edge_se3_prior.h" -#include "edge_se3_xyzprior.h" -#include "edge_xyz_prior.h" -#include "parameter_camera.h" -#include "parameter_se3_offset.h" -#include "parameter_stereo_camera.h" -#include "vertex_pointxyz.h" -#include "vertex_se3.h" +#include "g2o/core/abstract_graph.h" + +#ifdef G2O_HAVE_CEREAL +#include +#include + +#include "io_wrapper_cereal.h" // IWYU pragma: keep +#else +#include "g2o/stuff/logger.h" +#endif // HAVE CEREAL + +namespace g2o { + +#ifdef G2O_HAVE_CEREAL + +std::optional IoJson::load(std::istream& input) { + return io::load(input, "JSON"); +} + +bool IoJson::save(std::ostream& output, const AbstractGraph& graph) { + return io::save(output, graph, "JSON"); +} + +#else + +std::optional IoJson::load(std::istream&) { + G2O_WARN("Loading JSON is not supported"); + return std::nullopt; +} + +bool IoJson::save(std::ostream&, const AbstractGraph&) { + G2O_WARN("Saving JSON is not supported"); + return false; +} + #endif + +} // namespace g2o diff --git a/g2o/types/slam2d_addons/types_slam2d_addons.h b/g2o/core/io/io_json.h similarity index 79% rename from g2o/types/slam2d_addons/types_slam2d_addons.h rename to g2o/core/io/io_json.h index 759ce0766..cc1517ed1 100644 --- a/g2o/types/slam2d_addons/types_slam2d_addons.h +++ b/g2o/core/io/io_json.h @@ -24,18 +24,20 @@ // 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_TYPES_SLAM2D_SEGMENT_ -#define G2O_TYPES_SLAM2D_SEGMENT_ +#ifndef G2O_CORE_IO_JSON_FORMAT_H +#define G2O_CORE_IO_JSON_FORMAT_H -#include "g2o/config.h" -#include "g2o/types/slam2d/types_slam2d.h" -// line slam ver 1 -#include "edge_se2_segment2d.h" -#include "edge_se2_segment2d_line.h" -#include "edge_se2_segment2d_pointLine.h" -// line slam ver 2 -#include "edge_line2d.h" -#include "edge_line2d_pointxy.h" -#include "edge_se2_line2d.h" +#include "g2o/core/g2o_core_api.h" +#include "io_interface.h" + +namespace g2o { + +class G2O_CORE_API IoJson : public IoInterface { + public: + std::optional load(std::istream& input) override; + bool save(std::ostream& output, const AbstractGraph& graph) override; +}; + +} // namespace g2o #endif diff --git a/g2o/core/io/io_wrapper_cereal.h b/g2o/core/io/io_wrapper_cereal.h new file mode 100644 index 000000000..f2515a99f --- /dev/null +++ b/g2o/core/io/io_wrapper_cereal.h @@ -0,0 +1,165 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_CORE_IO_WARPPER_CEREAL_H +#define G2O_CORE_IO_WARPPER_CEREAL_H + +#include "g2o/config.h" + +#ifdef G2O_HAVE_CEREAL + +#include +#include +#include +#include +#include +#include +#include + +#include "g2o/core/abstract_graph.h" +#include "g2o/stuff/logger.h" + +namespace g2o { + +/** + * @brief Versions of the IO API for the graph using cereal. + */ +enum class IoVersions { + kGraph = 0, + kData = 0, + kParameter = 0, + kGraphElement = 0, + kVertex = 0, + kEdge = 0, +}; + +template +void serialize(Archive& archive, AbstractGraph& graph, + const std::uint32_t version) { + (void)version; + archive(cereal::make_nvp("fixed", graph.fixed()), + cereal::make_nvp("params", graph.parameters()), + cereal::make_nvp("vertices", graph.vertices()), + cereal::make_nvp("edges", graph.edges())); +} + +template +void serialize(Archive& archive, AbstractGraph::AbstractData& data, + const std::uint32_t version) { + (void)version; + archive(cereal::make_nvp("tag", data.tag), + cereal::make_nvp("data", data.data)); +} + +template +void serialize(Archive& archive, AbstractGraph::AbstractParameter& param, + const std::uint32_t version) { + (void)version; + archive(cereal::make_nvp("tag", param.tag), cereal::make_nvp("id", param.id), + cereal::make_nvp("value", param.value)); +} + +template +void serialize(Archive& archive, AbstractGraph::AbstractGraphElement& elem, + const std::uint32_t version) { + (void)version; + archive(cereal::make_nvp("tag", elem.tag), + cereal::make_nvp("data", elem.data)); +} + +template +void serialize(Archive& archive, AbstractGraph::AbstractVertex& vertex, + const std::uint32_t version) { + (void)version; + archive(cereal::make_nvp( + "elem", + cereal::base_class(&vertex)), + cereal::make_nvp("id", vertex.id), + cereal::make_nvp("estimate", vertex.estimate)); +} + +template +void serialize(Archive& archive, AbstractGraph::AbstractEdge& edge, + const std::uint32_t version) { + (void)version; + archive(cereal::make_nvp( + "elem", + cereal::base_class(&edge)), + cereal::make_nvp("ids", edge.ids), + cereal::make_nvp("param_ids", edge.param_ids), + cereal::make_nvp("measurement", edge.measurement), + cereal::make_nvp("information", edge.information)); +} + +namespace io { +template +std::optional load(std::istream& input, std::string_view name) { + try { + ArchiveType archive(input); + AbstractGraph result; + archive(cereal::make_nvp("graph", result)); + return result; + } catch (const std::exception& e) { + G2O_ERROR("Exception while loading {}: {}", name, e.what()); + } + return std::nullopt; +} + +template +bool save(std::ostream& output, const AbstractGraph& graph, + std::string_view name) { + try { + ArchiveType archive(output); + archive(cereal::make_nvp("graph", graph)); + return true; + } catch (const std::exception& e) { + G2O_ERROR("Exception while saving {}: {}", name, e.what()); + } + return false; +} + +} // namespace io + +} // namespace g2o + +// Register Version numbers +CEREAL_CLASS_VERSION(g2o::AbstractGraph, + static_cast(g2o::IoVersions::kGraph)); +CEREAL_CLASS_VERSION(g2o::AbstractGraph::AbstractData, + static_cast(g2o::IoVersions::kData)); +CEREAL_CLASS_VERSION(g2o::AbstractGraph::AbstractParameter, + static_cast(g2o::IoVersions::kParameter)); +CEREAL_CLASS_VERSION( + g2o::AbstractGraph::AbstractGraphElement, + static_cast(g2o::IoVersions::kGraphElement)); +CEREAL_CLASS_VERSION(g2o::AbstractGraph::AbstractVertex, + static_cast(g2o::IoVersions::kVertex)); +CEREAL_CLASS_VERSION(g2o::AbstractGraph::AbstractEdge, + static_cast(g2o::IoVersions::kEdge)); + +#endif // HAVE_CEREAL + +#endif diff --git a/g2o/types/slam3d/parameter_stereo_camera.h b/g2o/core/io/io_xml.cpp similarity index 63% rename from g2o/types/slam3d/parameter_stereo_camera.h rename to g2o/core/io/io_xml.cpp index f248965fe..e8099db05 100644 --- a/g2o/types/slam3d/parameter_stereo_camera.h +++ b/g2o/core/io/io_xml.cpp @@ -24,32 +24,46 @@ // 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_STEREO_CAMERA_PARAMETERS_H_ -#define G2O_STEREO_CAMERA_PARAMETERS_H_ +#include "io_xml.h" -#include +#include -#include "g2o/core/hyper_graph_action.h" -#include "g2o_types_slam3d_api.h" -#include "parameter_camera.h" +#include "g2o/config.h" +#include "g2o/core/abstract_graph.h" + +#ifdef G2O_HAVE_CEREAL +#include +#include + +#include "io_wrapper_cereal.h" // IWYU pragma: keep +#else +#include "g2o/stuff/logger.h" +#endif // HAVE CEREAL namespace g2o { -/** - * \brief parameters for a camera - */ -class G2O_TYPES_SLAM3D_API ParameterStereoCamera : public ParameterCamera { - public: - ParameterStereoCamera(); - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - - void setBaseline(double baseline) { baseline_ = baseline; } - double baseline() const { return baseline_; } - - protected: - double baseline_; -}; -} // namespace g2o + +#ifdef G2O_HAVE_CEREAL + +std::optional IoXml::load(std::istream& input) { + return io::load(input, "XML"); +} + +bool IoXml::save(std::ostream& output, const AbstractGraph& graph) { + return io::save(output, graph, "XML"); +} + +#else + +std::optional IoXml::load(std::istream&) { + G2O_WARN("Loading XML is not supported"); + return std::nullopt; +} + +bool IoXml::save(std::ostream&, const AbstractGraph&) { + G2O_WARN("Saving XML is not supported"); + return false; +} #endif + +} // namespace g2o diff --git a/g2o/core/io/io_xml.h b/g2o/core/io/io_xml.h new file mode 100644 index 000000000..e77149c0e --- /dev/null +++ b/g2o/core/io/io_xml.h @@ -0,0 +1,43 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_CORE_IO_XML_FORMAT_H +#define G2O_CORE_IO_XML_FORMAT_H + +#include "g2o/core/g2o_core_api.h" +#include "io_interface.h" + +namespace g2o { + +class G2O_CORE_API IoXml : public IoInterface { + public: + std::optional load(std::istream& input) override; + bool save(std::ostream& output, const AbstractGraph& graph) override; +}; + +} // namespace g2o + +#endif diff --git a/g2o/core/jacobian_workspace.h b/g2o/core/jacobian_workspace.h index 5c32f705c..1f457cd8c 100644 --- a/g2o/core/jacobian_workspace.h +++ b/g2o/core/jacobian_workspace.h @@ -38,7 +38,7 @@ namespace g2o { -struct OptimizableGraph; +class OptimizableGraph; /** * \brief provide memory workspace for computing the Jacobians diff --git a/g2o/core/optimizable_graph.cpp b/g2o/core/optimizable_graph.cpp index 7aa91d6f4..87b233b9a 100644 --- a/g2o/core/optimizable_graph.cpp +++ b/g2o/core/optimizable_graph.cpp @@ -33,11 +33,16 @@ #include #include #include +#include #include +#include #include "cache.h" #include "factory.h" +#include "g2o/core/abstract_graph.h" +#include "g2o/core/eigen_types.h" #include "g2o/core/hyper_graph.h" +#include "g2o/core/io/io_format.h" #include "g2o/core/jacobian_workspace.h" #include "g2o/core/parameter.h" #include "g2o/core/parameter_container.h" @@ -51,8 +56,80 @@ namespace g2o { namespace { std::shared_ptr kNonExistantVertex(nullptr); + +void saveUserData(AbstractGraph::AbstractGraphElement& graph_element, + HyperGraph::Data* d) { + Factory* factory = Factory::instance(); + while (d) { // write the data packet for the vertex + const std::string tag = factory->tag(d); + if (!tag.empty()) { + std::stringstream buffer; + d->write(buffer); + graph_element.data.emplace_back(tag, buffer.str()); + } + d = d->next().get(); + } +} + +bool saveParameter(AbstractGraph& abstract_graph, Parameter* p) { + Factory* factory = Factory::instance(); + const std::string tag = factory->tag(p); + if (tag.empty()) return false; + std::vector data; + p->getParameterData(data); + abstract_graph.parameters().emplace_back(tag, p->id(), data); + return true; +} + +bool saveParameters(g2o::AbstractGraph& abstract_graph, + const g2o::ParameterContainer& params) { + bool status = true; + for (const auto& param : params) { + status = saveParameter(abstract_graph, param.second.get()) && status; + } + return status; } +// helper to add data to the graph +void addDataToGraphElement( + std::shared_ptr previousDataContainer, + const std::vector& data_vector) { + Factory* factory = Factory::instance(); + + HyperGraph::GraphElemBitset elemDataBitset; + elemDataBitset[HyperGraph::kHgetData] = true; + OptimizableGraph::Data* previousData = nullptr; + for (const auto& abstract_data : data_vector) { + const std::shared_ptr element = + factory->construct(abstract_data.tag, elemDataBitset); + if (!element) { + G2O_WARN("{} could not be constructed as data", abstract_data.tag); + continue; + } + assert(element->elementType() == HyperGraph::kHgetData && "Should be data"); + auto d = std::static_pointer_cast(element); + std::stringstream buffer(abstract_data.data); + const bool r = d->read(buffer); + if (!r) { + G2O_ERROR("Error reading data {}", abstract_data.tag); + previousData = nullptr; + } else if (previousData) { + previousData->setNext(d); + d->setDataContainer(previousData->dataContainer()); + previousData = d.get(); + } else if (previousDataContainer) { + previousDataContainer->setUserData(d); + d->setDataContainer(previousDataContainer); + previousData = d.get(); + previousDataContainer = nullptr; + } else { + G2O_ERROR("got data element, but no data container available"); + previousData = nullptr; + } + } +}; +} // namespace + using std::string; using std::vector; @@ -142,12 +219,6 @@ void OptimizableGraph::Edge::setRobustKernel( bool OptimizableGraph::Edge::resolveCaches() { return true; } -bool OptimizableGraph::Edge::setMeasurementData(const double*) { return false; } - -bool OptimizableGraph::Edge::getMeasurementData(double*) const { return false; } - -int OptimizableGraph::Edge::measurementDimension() const { return -1; } - bool OptimizableGraph::Edge::setMeasurementFromState() { return false; } OptimizableGraph::OptimizableGraph() { @@ -337,198 +408,166 @@ void OptimizableGraph::forEachVertex( } } -bool OptimizableGraph::load(std::istream& is) { - std::set warnedUnknownTypes; - std::stringstream currentLine; - string token; - +bool OptimizableGraph::load(std::istream& is, io::Format format) { Factory* factory = Factory::instance(); - HyperGraph::GraphElemBitset elemBitset; - elemBitset[HyperGraph::kHgetParameter] = true; - elemBitset.flip(); + g2o::AbstractGraph abstract_graph; + bool load_status = abstract_graph.load(is, format); + if (!load_status) { + G2O_ERROR("Failed to load graph"); + return false; + } + + if (!renamedTypesLookup_.empty()) { + abstract_graph.renameTags(renamedTypesLookup_); + } + + // Create the parameters of the graph HyperGraph::GraphElemBitset elemParamBitset; elemParamBitset[HyperGraph::kHgetParameter] = true; - - std::shared_ptr previousDataContainer; - Data* previousData = nullptr; - - int lineNumber = 0; - while (true) { - const int bytesRead = readLine(is, currentLine); - lineNumber++; - if (bytesRead == -1) break; - currentLine >> token; - if (bytesRead == 0 || token.empty() || token[0] == '#') continue; - - // handle commands encoded in the file - if (token == "FIX") { - int id; - while (currentLine >> id) { - auto v = vertex(id); - if (v) { -#ifndef NDEBUG - G2O_DEBUG("Fixing vertex {}", v->id()); -#endif - v->setFixed(true); - } else { - G2O_WARN("Unable to fix vertex with id {}. Not found in the graph.", - id); - } - } + for (const auto& abstract_param : abstract_graph.parameters()) { + const std::shared_ptr pelement = + factory->construct(abstract_param.tag, elemParamBitset); + if (!pelement) { + G2O_WARN("{} could not be constructed as parameter", abstract_param.tag); continue; } - - // do the mapping to an internal type if it matches - if (!renamedTypesLookup_.empty()) { - auto foundIt = renamedTypesLookup_.find(token); - if (foundIt != renamedTypesLookup_.end()) { - token = foundIt->second; - } + assert(pelement->elementType() == HyperGraph::kHgetParameter && + "Should be a param"); + auto p = std::static_pointer_cast(pelement); + p->setId(abstract_param.id); + if (!p->setParameterData(abstract_param.value)) { + G2O_WARN("{} could not set parameter data", abstract_param.tag); + continue; + } + if (!parameters_.addParameter(p)) { + G2O_ERROR("Parameter of type: {} id: {} already defined", + abstract_param.tag, abstract_param.id); } + } - if (!factory->knowsTag(token)) { - if (warnedUnknownTypes.count(token) != 1) { - warnedUnknownTypes.insert(token); - G2O_ERROR("Unknown type {}", token); - } + // Create the vertices of the graph + HyperGraph::GraphElemBitset elemVertexBitset; + elemVertexBitset[HyperGraph::kHgetVertex] = true; + for (const auto& abstract_vertex : abstract_graph.vertices()) { + const std::shared_ptr graph_element = + factory->construct(abstract_vertex.tag, elemVertexBitset); + if (!graph_element) { + G2O_WARN("{} could not be constructed as vertex", abstract_vertex.tag); continue; } - - // first handle the parameters - const std::shared_ptr pelement = - factory->construct(token, elemParamBitset); - if (pelement) { // not a parameter or otherwise unknown tag - assert(pelement->elementType() == HyperGraph::kHgetParameter && - "Should be a param"); - auto p = std::static_pointer_cast(pelement); - int pid; - currentLine >> pid; - p->setId(pid); - const bool r = p->read(currentLine); - if (!r) { - G2O_ERROR("reading data {} for parameter {} at line ", pid, lineNumber); - } else { - if (!parameters_.addParameter(p)) { - G2O_ERROR("Parameter of type: {} id: {} already defined at line {}", - token, pid, lineNumber); - } - } + assert(graph_element->elementType() == HyperGraph::kHgetVertex && + "Should be a vertex"); + auto vertex = std::static_pointer_cast(graph_element); + vertex->setId(abstract_vertex.id); + vertex->setDimension(abstract_vertex.estimate.size()); + if (!vertex->setEstimateData(abstract_vertex.estimate)) { + G2O_WARN("{} could not set estimate", abstract_vertex.tag); continue; } + if (!addVertex(vertex)) { + G2O_ERROR("Failure adding Vertex {} {}", abstract_vertex.tag, + abstract_vertex.id); + } + if (!abstract_vertex.data.empty()) + addDataToGraphElement(vertex, abstract_vertex.data); + } - const std::shared_ptr element = - factory->construct(token, elemBitset); - if (dynamic_cast(element.get())) { // it's a vertex type - previousData = nullptr; - auto v = std::static_pointer_cast(element); - int id; - currentLine >> id; - const bool r = v->read(currentLine); - if (!r) - G2O_ERROR("Error reading vertex {} {} at line {}", token, id, - lineNumber); - v->setId(id); - if (!addVertex(v)) { - G2O_ERROR("Failure adding Vertex {} {} at line {}", token, id, - lineNumber); - } else { - previousDataContainer = v; - } - } else if (dynamic_cast(element.get())) { - previousData = nullptr; - auto e = std::static_pointer_cast(element); - const int numV = e->vertices().size(); - - vector ids; - if (!e->vertices().empty()) { - ids.resize(e->vertices().size()); - for (int l = 0; l < numV; ++l) currentLine >> ids[l]; - } else { - string buff; // reading the IDs of a dynamically sized edge - while (currentLine >> buff) { - // TODO(rainer): reading/writing multi dynamically sized edges is a - // bad design. Get rid of writing || in the edges - if (buff == "||") break; - ids.push_back(stoi(buff)); - currentLine >> buff; - } - e->resize(numV); - } - bool vertsOkay = true; - for (vector::size_type l = 0; l < ids.size(); ++l) { - const int vertexId = ids[l]; - if (vertexId != HyperGraph::kUnassignedId) { - auto v = vertex(vertexId); - if (!v) { - vertsOkay = false; - break; - } - e->setVertex(l, v); + // Create the edges of the graph + HyperGraph::GraphElemBitset elemEdgeBitset; + elemEdgeBitset[HyperGraph::kHgetEdge] = true; + for (const auto& abstract_edge : abstract_graph.edges()) { + const std::shared_ptr graph_element = + factory->construct(abstract_edge.tag, elemEdgeBitset); + if (!graph_element) { + G2O_WARN("{} could not be constructed as edge", abstract_edge.tag); + continue; + } + assert(graph_element->elementType() == HyperGraph::kHgetEdge && + "Should be an edge"); + auto edge = std::static_pointer_cast(graph_element); + edge->resize(abstract_edge.ids.size()); + bool vertsOkay = true; + for (vector::size_type l = 0; l < abstract_edge.ids.size(); ++l) { + const int vertexId = abstract_edge.ids[l]; + if (vertexId != HyperGraph::kUnassignedId) { + auto v = vertex(vertexId); + if (!v) { + vertsOkay = false; + break; } + edge->setVertex(l, v); } - if (!vertsOkay) { - G2O_ERROR("Unable to find vertices for edge {} at line {} IDs: {}", - token, lineNumber, fmt::join(ids, " ")); - e = nullptr; - } else { - const bool r = e->read(currentLine); - if (!r || !addEdge(e)) { - G2O_ERROR("Unable to add edge {} at line {} IDs: {}", token, - lineNumber, fmt::join(ids, " ")); - e = nullptr; - } + } + if (!vertsOkay) { + G2O_ERROR("Unable to find vertices for edge {} IDs: {}", + abstract_edge.tag, fmt::join(abstract_edge.ids, " ")); + continue; + } + for (size_t i = 0; i < abstract_edge.param_ids.size(); ++i) { + edge->setParameterId(i, abstract_edge.param_ids[i]); + } + if (!edge->setMeasurementData(abstract_edge.measurement.data())) { + G2O_WARN("{} could not set measurement", abstract_edge.tag); + continue; + } + MatrixX::MapType information(edge->informationData(), edge->dimension(), + edge->dimension()); + for (int r = 0, idx = 0; r < information.rows(); ++r) + for (int c = r; c < information.cols(); ++c) { + information(r, c) = abstract_edge.information[idx++]; + if (r != c) information(c, r) = information(r, c); } + if (!addEdge(edge)) { + G2O_ERROR("Failure adding Edge {} IDs {}", abstract_edge.tag, + fmt::join(abstract_edge.ids, " ")); + } + if (!abstract_edge.data.empty()) + addDataToGraphElement(edge, abstract_edge.data); + } - previousDataContainer = e; - } else if (dynamic_cast(element.get())) { // reading in the data - // packet for the vertex - auto d = std::static_pointer_cast(element); - const bool r = d->read(currentLine); - if (!r) { - G2O_ERROR("Error reading data {} at line {}", token, lineNumber); - previousData = nullptr; - } else if (previousData) { - previousData->setNext(d); - d->setDataContainer(previousData->dataContainer()); - previousData = d.get(); - } else if (previousDataContainer) { - previousDataContainer->setUserData(d); - d->setDataContainer(previousDataContainer); - previousData = d.get(); - previousDataContainer = nullptr; - } else { - G2O_ERROR("got data element, but no data container available"); - previousData = nullptr; - } + for (const auto fixed_vertex_id : abstract_graph.fixed()) { + auto v = vertex(fixed_vertex_id); + if (!v) { + G2O_WARN("Cannot fix vertex {}", fixed_vertex_id); + continue; } - } // while read line + v->setFixed(true); + } -#ifndef NDEBUG - G2O_DEBUG("Loaded {} parameters", parameters_.size()); -#endif + G2O_TRACE("Loaded {} parameters", parameters_.size()); + G2O_TRACE("Loaded {} vertices", vertices_.size()); + G2O_TRACE("Loaded {} edges", edges_.size()); return true; } -bool OptimizableGraph::load(const char* filename) { - std::ifstream ifs(filename); +bool OptimizableGraph::load(const char* filename, io::Format format) { + std::ifstream ifs(filename, format == io::Format::kBinary + ? std::ios_base::in | std::ios::binary + : std::ios_base::in); if (!ifs) { G2O_ERROR("Unable to open file {}", filename); return false; } - return load(ifs); + return load(ifs, format); } -bool OptimizableGraph::save(const char* filename, int level) const { - std::ofstream ofs(filename); +bool OptimizableGraph::save(const char* filename, io::Format format, + int level) const { + std::ofstream ofs(filename, format == io::Format::kBinary + ? std::ios_base::out | std::ios::binary + : std::ios_base::out); if (!ofs) return false; - return save(ofs, level); + return save(ofs, format, level); } -bool OptimizableGraph::save(std::ostream& os, int level) const { +bool OptimizableGraph::save(std::ostream& os, io::Format format, + int level) const { + g2o::AbstractGraph abstract_graph; + bool status = saveParameters(abstract_graph, parameters_); + // write the parameters to the top of the file - if (!parameters_.write(os)) return false; std::set verticesToSave; // set sorted by ID for (const auto& it : edges()) { auto* e = static_cast(it.get()); @@ -541,7 +580,7 @@ bool OptimizableGraph::save(std::ostream& os, int level) const { } } - for (auto* v : verticesToSave) saveVertex(os, v); + for (auto* v : verticesToSave) status &= saveVertex(abstract_graph, v); std::vector> edgesToSave; std::copy_if(edges().begin(), edges().end(), std::back_inserter(edgesToSave), @@ -550,16 +589,19 @@ bool OptimizableGraph::save(std::ostream& os, int level) const { return (e->level() == level); }); sort(edgesToSave.begin(), edgesToSave.end(), EdgeIDCompare()); - for (const auto& e : edgesToSave) saveEdge(os, static_cast(e.get())); + for (const auto& e : edgesToSave) + status &= saveEdge(abstract_graph, static_cast(e.get())); - return os.good(); + return abstract_graph.save(os, format) && status; } bool OptimizableGraph::saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, - int level) { - if (!parameters_.write(os)) return false; + io::Format format, int level) { + g2o::AbstractGraph abstract_graph; + bool status = saveParameters(abstract_graph, parameters_); - for (const auto& v : vset) saveVertex(os, static_cast(v.get())); + for (const auto& v : vset) + saveVertex(abstract_graph, static_cast(v.get())); for (const auto& it : edges()) { auto* e = dynamic_cast(it.get()); @@ -572,22 +614,25 @@ bool OptimizableGraph::saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, } } if (!verticesInEdge) continue; - saveEdge(os, e); + status &= saveEdge(abstract_graph, e); } - - return os.good(); + return abstract_graph.save(os, format) && status; } -bool OptimizableGraph::saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset) { - if (!parameters_.write(os)) return false; +bool OptimizableGraph::saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset, + io::Format format) { + g2o::AbstractGraph abstract_graph; + bool status = saveParameters(abstract_graph, parameters_); HyperGraph::VertexSet vset; for (const auto& e : eset) for (const auto& v : e->vertices()) if (v) vset.insert(v); - for (const auto& v : vset) saveVertex(os, static_cast(v.get())); - for (const auto& e : eset) saveEdge(os, static_cast(e.get())); - return os.good(); + for (const auto& v : vset) + status &= saveVertex(abstract_graph, static_cast(v.get())); + for (const auto& e : eset) + status &= saveEdge(abstract_graph, static_cast(e.get())); + return abstract_graph.save(os, format) && status; } int OptimizableGraph::maxDimension() const { @@ -702,63 +747,50 @@ bool OptimizableGraph::removePostIterationAction( return graphActions_[kAtPostiteration].erase(action) > 0; } -bool OptimizableGraph::saveUserData(std::ostream& os, HyperGraph::Data* d) { - Factory* factory = Factory::instance(); - while (d) { // write the data packet for the vertex - const string tag = factory->tag(d); - if (!tag.empty()) { - os << tag << " "; - d->write(os); - os << '\n'; - } - d = d->next().get(); - } - return os.good(); -} - -bool OptimizableGraph::saveVertex(std::ostream& os, +bool OptimizableGraph::saveVertex(AbstractGraph& abstract_graph, OptimizableGraph::Vertex* v) { Factory* factory = Factory::instance(); const string tag = factory->tag(v); - if (!tag.empty()) { - os << tag << " " << v->id() << " "; - v->write(os); - os << '\n'; - saveUserData(os, v->userData().get()); - if (v->fixed()) { - os << "FIX " << v->id() << '\n'; - } - return os.good(); + if (tag.empty()) { + G2O_WARN("Got empty tag for vertex {} while saving", v->id()); + return false; } - return false; -} - -bool OptimizableGraph::saveParameter(std::ostream& os, Parameter* p) { - Factory* factory = Factory::instance(); - const string tag = factory->tag(p); - if (!tag.empty()) { - os << tag << " " << p->id() << " "; - p->write(os); - os << '\n'; + std::vector vertex_estimate; + v->getEstimateData(vertex_estimate); + abstract_graph.vertices().emplace_back(tag, v->id(), vertex_estimate); + saveUserData(abstract_graph.vertices().back(), v->userData().get()); + if (v->fixed()) { + abstract_graph.fixed().push_back(v->id()); } - return os.good(); + return true; } -bool OptimizableGraph::saveEdge(std::ostream& os, OptimizableGraph::Edge* e) { +bool OptimizableGraph::saveEdge(AbstractGraph& abstract_graph, + OptimizableGraph::Edge* e) { Factory* factory = Factory::instance(); const string tag = factory->tag(e); - if (!tag.empty()) { - os << tag << " "; - for (auto& it : e->vertices()) { - const int vertexId = it ? it->id() : HyperGraph::kUnassignedId; - os << vertexId << " "; - } - e->write(os); - os << '\n'; - saveUserData(os, e->userData().get()); - return os.good(); + if (tag.empty()) { + G2O_WARN("Got empty tag for edge while saving"); + return false; + } + std::vector ids; + ids.reserve(e->vertices().size()); + for (const auto& vertex : e->vertices()) { + ids.push_back(vertex ? vertex->id() : HyperGraph::kUnassignedId); } - return false; + std::vector data(e->measurementDimension()); + e->getMeasurementData(data.data()); + MatrixX::MapType information(e->informationData(), e->dimension(), + e->dimension()); + std::vector upper_triangle; + upper_triangle.reserve((e->dimension() * (e->dimension() + 1)) / 2); + for (int r = 0; r < e->dimension(); ++r) + for (int c = r; c < e->dimension(); ++c) + upper_triangle.push_back(information(r, c)); + abstract_graph.edges().emplace_back(tag, ids, data, upper_triangle, + e->parameterIds()); + saveUserData(abstract_graph.edges().back(), e->userData().get()); + return true; } void OptimizableGraph::clear() { diff --git a/g2o/core/optimizable_graph.h b/g2o/core/optimizable_graph.h index e816c7a27..164f0541a 100644 --- a/g2o/core/optimizable_graph.h +++ b/g2o/core/optimizable_graph.h @@ -32,14 +32,15 @@ #include #include #include -#include #include #include #include #include +#include #include #include "g2o/core/eigen_types.h" +#include "g2o/core/io/io_format.h" #include "g2o_core_api.h" #include "hyper_graph.h" #include "jacobian_workspace.h" @@ -53,6 +54,7 @@ class HyperGraphAction; struct OptimizationAlgorithmProperty; class CacheContainer; class RobustKernel; +class AbstractGraph; /** @addtogroup g2o @@ -65,7 +67,8 @@ class RobustKernel; also provides basic functionalities to handle the backup/restore of portions of the vertices. */ -struct G2O_CORE_API OptimizableGraph : public HyperGraph { +class G2O_CORE_API OptimizableGraph : public HyperGraph { + public: enum ActionType { kAtPreiteration, kAtPostiteration, @@ -131,18 +134,12 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { class G2O_CORE_API Vertex : public HyperGraph::Vertex, public HyperGraph::DataContainer { private: - friend struct OptimizableGraph; + friend class OptimizableGraph; public: Vertex(); ~Vertex() override; - //! sets the node to the origin (used in the multilevel stuff) - void setToOrigin() { - setToOriginImpl(); - updateCache(); - } - //! get the mapped memory of the hessian matrix [[nodiscard]] virtual double* hessianData() const = 0; @@ -171,9 +168,9 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { /** * updates the current vertex with the direct solution x += H_ii\b_ii - * @return the determinant of the inverted hessian + * @return True, iff solution was possible */ - virtual double solveDirect(double lambda = 0) = 0; + virtual bool solveDirect(double lambda = 0) = 0; /** * sets the initial estimate from an array of double @@ -247,6 +244,10 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { * get/setEstimate(double*) -1 if it is not supported */ [[nodiscard]] virtual int estimateDimension() const = 0; + /** + * Returns the dimension of the estimate at compile time. + */ + [[nodiscard]] virtual int estimateDimensionAtCompileTime() const = 0; /** * sets the initial estimate from an array of double. @@ -368,6 +369,8 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { //! dimension of the estimated state belonging to this node [[nodiscard]] int dimension() const { return dimension_; } + virtual bool setDimension(int /*dimension*/) { return false; } + //! sets the id of the node in the graph be sure that the graph keeps //! consistent after changing the id void setId(int id) override { id_ = id; } @@ -391,11 +394,6 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { */ void unlockQuadraticForm() { quadraticFormMutex_.unlock(); } - //! read the vertex from a stream, i.e., the internal state of the vertex - virtual bool read(std::istream& is) = 0; - //! write the vertex to a stream - virtual bool write(std::ostream& os) const = 0; - virtual void updateCache(); CacheContainer& cacheContainer(); @@ -417,9 +415,6 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { */ virtual void oplusImpl(const VectorX::MapType& v) = 0; - //! sets the node to the origin (used in the multilevel stuff) - virtual void setToOriginImpl() = 0; - /** * sets the initial estimate from an array of double * @return true on success @@ -430,7 +425,7 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { class G2O_CORE_API Edge : public HyperGraph::Edge, public HyperGraph::DataContainer { private: - friend struct OptimizableGraph; + friend class OptimizableGraph; public: Edge(); @@ -444,15 +439,26 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { //! sets the measurement from an array of double //! @returns true on success - virtual bool setMeasurementData(const double* m); + virtual bool setMeasurementData(const double* m) = 0; //! writes the measurement to an array of double //! @returns true on success - virtual bool getMeasurementData(double* m) const; + virtual bool getMeasurementData(double* m) const = 0; //! returns the dimension of the measurement in the extended representation //! which is used by get/setMeasurement; - [[nodiscard]] virtual int measurementDimension() const; + [[nodiscard]] virtual int measurementDimension() const = 0; + + /** + * @brief Returns the measurement's dimension at compile time. + */ + [[nodiscard]] virtual int measurementDimensionAtCompileTime() const = 0; + + /** + * returns the minimal dimension of the measurement which corresponds to the + * dimension of the information matrix. + */ + [[nodiscard]] virtual int minimalMeasurementDimension() const = 0; /** * sets the estimate to have a zero error, based on the current value of the @@ -533,14 +539,10 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { //! returns the dimensions of the error function [[nodiscard]] int dimension() const { return dimension_; } + [[nodiscard]] virtual int dimensionAtCompileTime() const = 0; virtual Vertex* createVertex(int) { return nullptr; } - //! read the vertex from a stream, i.e., the internal state of the vertex - virtual bool read(std::istream& is) = 0; - //! write the vertex to a stream - virtual bool write(std::ostream& os) const = 0; - //! the internal ID of the edge [[nodiscard]] int64_t internalId() const { return internalId_; } @@ -557,6 +559,9 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { return parameterIds_; } + //! return the number of vertices at compile time + [[nodiscard]] virtual int numVerticesAtCompileTime() const { return -1; } + protected: int dimension_ = -1; int level_ = 0; @@ -687,18 +692,31 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { //! load the graph from a stream. Uses the Factory singleton for creating the //! vertices and edges. - virtual bool load(std::istream& is); - bool load(const char* filename); - //! save the graph to a stream. Again uses the Factory system. - virtual bool save(std::ostream& os, int level = 0) const; + virtual bool load(std::istream& is, io::Format format = io::Format::kG2O); + bool load(const char* filename, io::Format format = io::Format::kG2O); + /** + * @brief Save the graph into a stream + * + * @param os Output stream. Note: Has to be opened with std::ios::binary for + * writing in binary format. + * @param format Format for saving the data, see also io::Format + * @param level Level of the graph to save + * @return true if successful + * @return false otherwise. + */ + virtual bool save(std::ostream& os, io::Format format = io::Format::kG2O, + int level = 0) const; //! function provided for convenience, see save() above - bool save(const char* filename, int level = 0) const; + bool save(const char* filename, io::Format format = io::Format::kG2O, + int level = 0) const; //! save a subgraph to a stream. Again uses the Factory system. - bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0); + bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, + io::Format format = io::Format::kG2O, int level = 0); //! save a subgraph to a stream. Again uses the Factory system. - bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset); + bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset, + io::Format format = io::Format::kG2O); //! push the estimate of a subset of the variables onto a stack virtual void push(HyperGraph::VertexSet& vset); @@ -776,7 +794,7 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { const std::function& fn); protected: - std::map renamedTypesLookup_; + std::unordered_map renamedTypesLookup_; int64_t nextEdgeId_; std::vector graphActions_; @@ -786,16 +804,10 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { void performActions(int iter, HyperGraphActionSet& actions); // helper functions to save an individual vertex - static bool saveVertex(std::ostream& os, Vertex* v); - - // helper function to save an individual parameter - static bool saveParameter(std::ostream& os, Parameter* p); + static bool saveVertex(AbstractGraph& abstract_graph, Vertex* v); // helper functions to save an individual edge - static bool saveEdge(std::ostream& os, Edge* e); - - // helper functions to save the data packets - static bool saveUserData(std::ostream& os, HyperGraph::Data* d); + static bool saveEdge(AbstractGraph& abstract_graph, Edge* e); }; /** diff --git a/g2o/core/parameter.h b/g2o/core/parameter.h index 2a8a35fbb..fd1bd1482 100644 --- a/g2o/core/parameter.h +++ b/g2o/core/parameter.h @@ -27,11 +27,12 @@ #ifndef G2O_GRAPH_PARAMETER_HH_ #define G2O_GRAPH_PARAMETER_HH_ -#include #include #include +#include "g2o/core/eigen_types.h" #include "g2o/core/g2o_core_api.h" +#include "g2o/core/type_traits.h" #include "hyper_graph.h" namespace g2o { @@ -40,20 +41,67 @@ class G2O_CORE_API Parameter : public HyperGraph::HyperGraphElement { public: Parameter() = default; ~Parameter() override = default; - //! read the data from a stream - virtual bool read(std::istream& is) = 0; - //! write the data to a stream - virtual bool write(std::ostream& os) const = 0; + [[nodiscard]] int id() const { return id_; } void setId(int id_); - [[nodiscard]] HyperGraph::HyperGraphElementType elementType() const override { + [[nodiscard]] HyperGraph::HyperGraphElementType elementType() const final { return HyperGraph::kHgetParameter; } + virtual void update() = 0; + + [[nodiscard]] virtual int parameterDimension() const = 0; + [[nodiscard]] virtual int minimalParameterDimension() const = 0; + + virtual bool getParameterData(std::vector& data) const = 0; + [[nodiscard]] virtual bool setParameterData( + const std::vector& data) = 0; + protected: int id_ = -1; }; +template +class BaseParameter : public Parameter { + public: + using ParameterType = T; + + [[nodiscard]] int parameterDimension() const final { + static_assert(TypeTraits::kMinimalVectorDimension != INT_MIN, + "Forgot to implement TypeTrait for your Estimate"); + return TypeTraits::kVectorDimension; + } + + [[nodiscard]] int minimalParameterDimension() const final { + static_assert(TypeTraits::kMinimalVectorDimension != INT_MIN, + "Forgot to implement TypeTrait for your Estimate"); + return TypeTraits::kMinimalVectorDimension; + } + + bool getParameterData(std::vector& data) const override { + int dim = parameterDimension(); + if (dim < 0) return false; + data.resize(dim); + TypeTraits::toData(param(), data.data()); + return true; + }; + + bool setParameterData(const std::vector& data) override { + VectorX::ConstMapType data_vector(data.data(), data.size()); + setParam(TypeTraits::fromVector(data_vector)); + return true; + }; + + const ParameterType& param() const { return parameter_; } + void setParam(const ParameterType& param) { + parameter_ = param; + update(); + } + + protected: + ParameterType parameter_; +}; + using ParameterVector = std::vector>; } // namespace g2o diff --git a/g2o/core/parameter_container.cpp b/g2o/core/parameter_container.cpp index cb001585a..3c520b308 100644 --- a/g2o/core/parameter_container.cpp +++ b/g2o/core/parameter_container.cpp @@ -26,14 +26,8 @@ #include "parameter_container.h" -#include -#include #include -#include "factory.h" -#include "g2o/core/hyper_graph.h" -#include "g2o/stuff/logger.h" -#include "g2o/stuff/string_tools.h" #include "parameter.h" namespace g2o { @@ -60,61 +54,4 @@ std::shared_ptr ParameterContainer::detachParameter(int id) { return p; } -bool ParameterContainer::write(std::ostream& os) const { - Factory* factory = Factory::instance(); - for (const auto& it : *this) { - os << factory->tag(it.second.get()) << " "; - os << it.second->id() << " "; - it.second->write(os); - os << '\n'; - } - return true; -} - -bool ParameterContainer::read( - std::istream& is, - const std::map* renamedTypesLookup) { - std::stringstream currentLine; - std::string token; - - Factory* factory = Factory::instance(); - HyperGraph::GraphElemBitset elemBitset; - elemBitset[HyperGraph::kHgetParameter] = true; - - while (true) { - const int bytesRead = readLine(is, currentLine); - if (bytesRead == -1) break; - currentLine >> token; - if (bytesRead == 0 || token.empty() || token[0] == '#') continue; - if (renamedTypesLookup && !renamedTypesLookup->empty()) { - auto foundIt = renamedTypesLookup->find(token); - if (foundIt != renamedTypesLookup->end()) { - token = foundIt->second; - } - } - - const std::shared_ptr element = - factory->construct(token, elemBitset); - if (!element) // not a parameter or otherwise unknown tag - continue; - assert(element->elementType() == HyperGraph::kHgetParameter && - "Should be a param"); - - auto p = std::static_pointer_cast(element); - int pid; - currentLine >> pid; - p->setId(pid); - const bool r = p->read(currentLine); - if (!r) { - G2O_ERROR("Error reading data {} for parameter {}", token, pid); - } else { - if (!addParameter(p)) { - G2O_ERROR("Parameter of type: {} id: {} already defined", token, pid); - } - } - } // while read line - - return true; -} - } // namespace g2o diff --git a/g2o/core/parameter_container.h b/g2o/core/parameter_container.h index 42b649724..bab1f7ce5 100644 --- a/g2o/core/parameter_container.h +++ b/g2o/core/parameter_container.h @@ -55,15 +55,11 @@ class ParameterContainer : protected std::map> { //! remove a parameter from the container and returns the formerly stored //! parameter std::shared_ptr detachParameter(int id); - //! read parameters from a stream - virtual bool read( - std::istream& is, - const std::map* renamedTypesLookup = nullptr); - //! write the data to a stream - virtual bool write(std::ostream& os) const; // stuff of the base class that should re-appear + using BaseClass::begin; using BaseClass::clear; + using BaseClass::end; using BaseClass::size; }; diff --git a/g2o/core/sparse_optimizer.cpp b/g2o/core/sparse_optimizer.cpp index 7ca848731..3eac326aa 100644 --- a/g2o/core/sparse_optimizer.cpp +++ b/g2o/core/sparse_optimizer.cpp @@ -298,13 +298,6 @@ bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset) { return indexMappingStatus; } -void SparseOptimizer::setToOrigin() { - for (auto& it : vertices()) { - auto* v = static_cast(it.second.get()); - v->setToOrigin(); - } -} - void SparseOptimizer::computeInitialGuess() { EstimatePropagator::PropagateCost costFunction(this); computeInitialGuess(costFunction); diff --git a/g2o/core/sparse_optimizer.h b/g2o/core/sparse_optimizer.h index 217c37e35..7b6085c8c 100644 --- a/g2o/core/sparse_optimizer.h +++ b/g2o/core/sparse_optimizer.h @@ -117,11 +117,6 @@ class G2O_CORE_API SparseOptimizer : public OptimizableGraph { */ virtual void computeInitialGuess(EstimatePropagatorCost& propagator); - /** - * sets all vertices to their origin. - */ - virtual void setToOrigin(); - /** * starts one optimization run given the current configuration of the graph, * and the current settings stored in the class instance. diff --git a/g2o/core/type_traits.h b/g2o/core/type_traits.h index 003ca6320..1527dc14b 100644 --- a/g2o/core/type_traits.h +++ b/g2o/core/type_traits.h @@ -40,31 +40,13 @@ namespace g2o { */ template struct TypeTraits { - // enum { - // kVectorDimension = INT_MIN, ///< dimension of the type as vector - // kMinimalVectorDimension = - // INT_MIN, ///< dimension of the type as minimal vector - // kIsVector = 0, ///< type is a vector - // kIsScalar = 0, ///< type is a scalar value - // }; - - // using Type = T; - // using VectorType = VectorN; - // using MinimalVectorType = VectorN; - - // static VectorType toVector(const Type& t); - // static void toData(const Type& t, double* data); - - // static VectorType toMinimalVector(const Type& t); - // static void toMinimalData(const Type& t, double* data); - - // template - // static Type fromVector(const Eigen::DenseBase& v); - - // template - // static Type fromMinimalVector(const Eigen::DenseBase& v); - - // static Type Identity(); + enum { + kVectorDimension = INT_MIN, ///< dimension of the type as vector + kMinimalVectorDimension = + INT_MIN, ///< dimension of the type as minimal vector + kIsVector = 0, ///< type is a vector + kIsScalar = 0, ///< type is a scalar value + }; }; /** @@ -86,13 +68,13 @@ struct TypeTraits> { static VectorType toVector(const Type& t) { return t; } static void toData(const Type& t, double* data) { // NOLINT - typename VectorType::MapType v(data, kVectorDimension); + typename VectorType::MapType v(data, t.size()); v = t; } static MinimalVectorType toMinimalVector(const Type& t) { return t; } static void toMinimalData(const Type& t, double* data) { // NOLINT - typename MinimalVectorType::MapType v(data, kMinimalVectorDimension); + typename MinimalVectorType::MapType v(data, t.size()); v = t; } @@ -105,8 +87,6 @@ struct TypeTraits> { static Type fromMinimalVector(const Eigen::DenseBase& v) { return v; } - - static Type Identity() { return Type::Zero(kVectorDimension); } }; /** @@ -153,8 +133,6 @@ struct TypeTraits { static Type fromMinimalVector(const Eigen::DenseBase& v) { return v[0]; } - - static Type Identity() { return 0.; } }; /** @@ -182,30 +160,29 @@ struct DimensionTraits { //! for a scalar value template ::kIsScalar> - static typename std::enable_if::type dimension(const T&) { + static std::enable_if_t dimension(const T&) { return 1; } //! for a statically known type template ::kIsVector, int IsScalar = TypeTraits::kIsScalar> - static typename std::enable_if::type - minimalDimension(const T&) { + static std::enable_if_t minimalDimension( + const T&) { return TypeTraits::kMinimalVectorDimension; } //! for a vector type template ::kIsVector, int IsScalar = TypeTraits::kIsScalar> - static typename std::enable_if::type - minimalDimension(const T& t) { + static std::enable_if_t minimalDimension( + const T& t) { return t.size(); } //! for a scalar value template ::kIsScalar> - static typename std::enable_if::type minimalDimension( - const T&) { + static std::enable_if_t minimalDimension(const T&) { return 1; } }; diff --git a/g2o/examples/bal/bal_example.cpp b/g2o/examples/bal/bal_example.cpp index 04281f4d4..884c14f4e 100644 --- a/g2o/examples/bal/bal_example.cpp +++ b/g2o/examples/bal/bal_example.cpp @@ -42,7 +42,6 @@ #include "g2o/core/sparse_optimizer.h" #include "g2o/solvers/pcg/linear_solver_pcg.h" #include "g2o/stuff/command_args.h" -#include "g2o/stuff/logger.h" #if defined G2O_HAVE_CHOLMOD #include "g2o/solvers/cholmod/linear_solver_cholmod.h" @@ -68,16 +67,6 @@ class VertexCameraBAL : public g2o::BaseVertex<9, g2o::bal::Vector9> { public: VertexCameraBAL() = default; - bool read(std::istream& /*is*/) override { - G2O_ERROR("not implemented yet"); - return false; - } - - bool write(std::ostream& /*os*/) const override { - G2O_ERROR("not implemented yet"); - return false; - } - void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update; } @@ -92,16 +81,6 @@ class VertexPointBAL : public g2o::BaseVertex<3, g2o::Vector3> { public: VertexPointBAL() = default; - bool read(std::istream& /*is*/) override { - G2O_ERROR("not implemented yet"); - return false; - } - - bool write(std::ostream& /*os*/) const override { - G2O_ERROR("not implemented yet"); - return false; - } - void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update; } @@ -134,14 +113,6 @@ class EdgeObservationBAL VertexPointBAL> { public: EdgeObservationBAL() = default; - bool read(std::istream& /*is*/) override { - G2O_ERROR("not implemented yet"); - return false; - } - bool write(std::ostream& /*os*/) const override { - G2O_ERROR("not implemented yet"); - return false; - } /** * templatized function to compute the error as described in the comment above diff --git a/g2o/examples/calibration_odom_laser/edge_se2_pure_calib.cpp b/g2o/examples/calibration_odom_laser/edge_se2_pure_calib.cpp index d19ad104a..7930be816 100644 --- a/g2o/examples/calibration_odom_laser/edge_se2_pure_calib.cpp +++ b/g2o/examples/calibration_odom_laser/edge_se2_pure_calib.cpp @@ -28,16 +28,6 @@ namespace g2o { -bool EdgeSE2PureCalib::read(std::istream& is) { - (void)is; - return false; -} - -bool EdgeSE2PureCalib::write(std::ostream& os) const { - (void)os; - return false; -} - void EdgeSE2PureCalib::computeError() { const VertexSE2* laserOffset = vertexXnRaw<0>(); const VertexOdomDifferentialParams* odomParams = vertexXnRaw<1>(); diff --git a/g2o/examples/calibration_odom_laser/edge_se2_pure_calib.h b/g2o/examples/calibration_odom_laser/edge_se2_pure_calib.h index db4dcf808..a32b80578 100644 --- a/g2o/examples/calibration_odom_laser/edge_se2_pure_calib.h +++ b/g2o/examples/calibration_odom_laser/edge_se2_pure_calib.h @@ -102,9 +102,6 @@ class G2O_CALIBRATION_ODOM_LASER_API EdgeSE2PureCalib EdgeSE2PureCalib() = default; void computeError() override; - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; }; } // namespace g2o diff --git a/g2o/examples/calibration_odom_laser/gm2dl_io.cpp b/g2o/examples/calibration_odom_laser/gm2dl_io.cpp index c31397c79..eeb91c9ae 100644 --- a/g2o/examples/calibration_odom_laser/gm2dl_io.cpp +++ b/g2o/examples/calibration_odom_laser/gm2dl_io.cpp @@ -54,7 +54,7 @@ bool Gm2dlIO::readGm2dl(const std::string& filename, SparseOptimizer& optimizer, // laserOffset->fixed() = true; laserOffset->setId(kIdLaserpose); if (!optimizer.addVertex(laserOffset)) { - std::cerr << "Unable to add laser offset" << std::endl; + std::cerr << "Unable to add laser offset\n"; return false; } @@ -76,7 +76,7 @@ bool Gm2dlIO::readGm2dl(const std::string& filename, SparseOptimizer& optimizer, // std::cerr << "Read vertex id " << id << std::endl; if (!optimizer.addVertex(v)) { std::cerr << "vertex " << id << " is already in the graph, reassigning " - << std::endl; + << '\n'; v = std::dynamic_pointer_cast(optimizer.vertex(id)); assert(v); } @@ -85,7 +85,7 @@ bool Gm2dlIO::readGm2dl(const std::string& filename, SparseOptimizer& optimizer, } else if (tag == "EDGE" || tag == "EDGE2" || tag == "EDGE_SE2") { if (!laserOffsetInitDone) { - std::cerr << "Error: need laser offset" << std::endl; + std::cerr << "Error: need laser offset\n"; return false; } int id1; @@ -112,12 +112,12 @@ bool Gm2dlIO::readGm2dl(const std::string& filename, SparseOptimizer& optimizer, auto v2 = std::dynamic_pointer_cast(optimizer.vertex(id2)); if (!v1) { std::cerr << "vertex " << id1 << " is not existing, cannot add edge (" - << id1 << "," << id2 << ")" << std::endl; + << id1 << "," << id2 << ")\n"; continue; } if (!v2) { std::cerr << "vertex " << id2 << " is not existing, cannot add edge (" - << id1 << "," << id2 << ")" << std::endl; + << id1 << "," << id2 << ")\n"; continue; } @@ -137,7 +137,7 @@ bool Gm2dlIO::readGm2dl(const std::string& filename, SparseOptimizer& optimizer, e->setVertex(1, v2); e->setVertex(2, laserOffset); if (!optimizer.addEdge(e)) { - std::cerr << "error in adding edge " << id1 << "," << id2 << std::endl; + std::cerr << "error in adding edge " << id1 << "," << id2 << '\n'; } // std::cerr << PVAR(e->inverseMeasurement().toVector().transpose()) << // std::endl; std::cerr << PVAR(e->information()) << std::endl; @@ -171,16 +171,18 @@ bool Gm2dlIO::writeGm2dl(const std::string& filename, for (const auto& it : optimizer.vertices()) { auto* v = static_cast(it.second.get()); - fout << "VERTEX2 " << v->id() << " "; - v->write(fout); - fout << std::endl; + fout << "VERTEX2 " << v->id(); + std::vector vertex_estimate; + v->getEstimateData(vertex_estimate); + for (const auto value : vertex_estimate) fout << " " << value; + fout << '\n'; auto data = v->userData(); if (data) { // writing the data via the factory const std::string tag = factory->tag(data.get()); if (!tag.empty()) { fout << tag << " "; data->write(fout); - fout << std::endl; + fout << '\n'; } } } @@ -205,7 +207,7 @@ bool Gm2dlIO::writeGm2dl(const std::string& filename, const Eigen::Matrix3d& m = calibEdge->information(); fout << " " << m(0, 0) << " " << m(0, 1) << " " << m(1, 1) << " " << m(2, 2) << " " << m(0, 2) << " " << m(1, 2); - fout << std::endl; + fout << '\n'; } else { // std::cerr << "Strange Edge Type: " << factory->tag(e) << std::endl; } @@ -218,7 +220,7 @@ bool Gm2dlIO::updateLaserData(SparseOptimizer& optimizer) { auto laserOffset = std::dynamic_pointer_cast(optimizer.vertex(kIdLaserpose)); if (!laserOffset) { - std::cerr << "Laser offset not found" << std::endl; + std::cerr << "Laser offset not found\n"; return false; } diff --git a/g2o/examples/calibration_odom_laser/sclam_helpers.cpp b/g2o/examples/calibration_odom_laser/sclam_helpers.cpp index e71f1450c..25856e953 100644 --- a/g2o/examples/calibration_odom_laser/sclam_helpers.cpp +++ b/g2o/examples/calibration_odom_laser/sclam_helpers.cpp @@ -49,7 +49,8 @@ static constexpr double kInformationScalingOdometry = 100; void addOdometryCalibLinksDifferential(SparseOptimizer& optimizer, const DataQueue& odomData) { auto odomParamsVertex = std::make_shared(); - odomParamsVertex->setToOrigin(); + odomParamsVertex->setEstimate( + VertexOdomDifferentialParams::EstimateType::Ones()); odomParamsVertex->setId(Gm2dlIO::kIdOdomcalib); optimizer.addVertex(odomParamsVertex); @@ -74,14 +75,13 @@ void addOdometryCalibLinksDifferential(SparseOptimizer& optimizer, odomData.findClosestData(rl2->timestamp())); if (fabs(rl1->timestamp() - rl2->timestamp()) < 1e-7) { - std::cerr << "strange edge " << r1->id() << " <-> " << r2->id() - << std::endl; + std::cerr << "strange edge " << r1->id() << " <-> " << r2->id() << '\n'; std::cerr << FIXED(PVAR(rl1->timestamp()) << "\t " << PVAR(rl2->timestamp())) - << std::endl; + << '\n'; std::cerr << FIXED(PVAR(odom1->timestamp()) << "\t " << PVAR(odom2->timestamp())) - << std::endl; + << '\n'; } // cerr << PVAR(odom1->odomPose().toVector().transpose()) << endl; diff --git a/g2o/examples/calibration_odom_laser/sclam_pure_calibration.cpp b/g2o/examples/calibration_odom_laser/sclam_pure_calibration.cpp index 71a1665ca..b4f569477 100644 --- a/g2o/examples/calibration_odom_laser/sclam_pure_calibration.cpp +++ b/g2o/examples/calibration_odom_laser/sclam_pure_calibration.cpp @@ -25,22 +25,15 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include #include #include -#include #include #include "closed_form_calibration.h" #include "edge_se2_pure_calib.h" -#include "g2o/core/hyper_dijkstra.h" #include "g2o/core/sparse_optimizer.h" -#include "g2o/stuff/color_macros.h" #include "g2o/stuff/command_args.h" -#include "g2o/stuff/filesys_tools.h" #include "g2o/stuff/macros.h" -#include "g2o/stuff/string_tools.h" -#include "g2o/stuff/timeutil.h" #include "g2o/types/data/data_queue.h" #include "g2o/types/data/robot_laser.h" #include "g2o/types/sclam2d/odometry_measurement.h" @@ -51,7 +44,6 @@ #include "sclam_helpers.h" using std::cerr; -using std::endl; using std::string; namespace { @@ -67,8 +59,6 @@ class VertexBaseline : public BaseVertex<1, double> { void oplusImpl(const VectorX::MapType& update) override { estimate_ += update[0]; } - bool read(std::istream&) override { return false; } - bool write(std::ostream&) const override { return false; } }; class EdgeCalib @@ -98,9 +88,6 @@ class EdgeCalib const SE2 delta = Ku_ij.inverse() * laserMotionInRobotFrame; error_ = delta.toVector(); } - - bool read(std::istream&) override { return false; } - bool write(std::ostream&) const override { return false; } }; namespace { @@ -143,10 +130,10 @@ int run_sclam_pure_calibration(int argc, char** argv) { DataQueue odometryQueue; const int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, odometryQueue); if (numLaserOdom == 0) { - cerr << "No raw information read" << endl; + cerr << "No raw information read\n"; return 0; } - cerr << "Read " << numLaserOdom << " laser readings from file" << endl; + cerr << "Read " << numLaserOdom << " laser readings from file\n"; Eigen::Vector3d odomCalib(1., 1., 1.); SE2 initialLaserPose; @@ -154,14 +141,14 @@ int run_sclam_pure_calibration(int argc, char** argv) { const int numRobotLaser = Gm2dlIO::readRobotLaser(inputFilename, robotLaserQueue); if (numRobotLaser == 0) { - cerr << "No robot laser read" << endl; + cerr << "No robot laser read\n"; return 0; } { auto rl = std::dynamic_pointer_cast( robotLaserQueue.buffer().begin()->second); initialLaserPose = rl->odomPose().inverse() * rl->laserPose(); - cerr << PVAR(initialLaserPose.toVector().transpose()) << endl; + cerr << PVAR(initialLaserPose.toVector().transpose()) << '\n'; } // adding the measurements @@ -212,24 +199,24 @@ int run_sclam_pure_calibration(int argc, char** argv) { calibEdge->setInformation(Eigen::Matrix3d::Identity()); calibEdge->setMeasurement(meas); if (!optimizer.addEdge(calibEdge)) { - cerr << "Error adding calib edge" << endl; + cerr << "Error adding calib edge\n"; } } if (fixLaser) { - cerr << "Fix position of the laser offset" << endl; + cerr << "Fix position of the laser offset\n"; laserOffset->setFixed(true); } - cerr << "\nPerforming full non-linear estimation" << endl; + cerr << "\nPerforming full non-linear estimation\n"; optimizer.initializeOptimization(); optimizer.computeActiveErrors(); optimizer.optimize(maxIterations); cerr << "Calibrated laser offset (x, y, theta):" - << laserOffset->estimate().toVector().transpose() << endl; + << laserOffset->estimate().toVector().transpose() << '\n'; odomCalib = odomParamsVertex->estimate(); cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " - << odomParamsVertex->estimate().transpose() << endl; + << odomParamsVertex->estimate().transpose() << '\n'; optimizer.clear(); } @@ -251,7 +238,7 @@ int run_sclam_pure_calibration(int argc, char** argv) { } // linearSolution = (A.transpose() * A).inverse() * A.transpose() * x; linearSolution = A.colPivHouseholderQr().solve(x); - // cout << PVAR(linearSolution.transpose()) << endl; + // cout << PVAR(linearSolution.transpose()) << '\n'; } // constructing non-linear least squares @@ -280,42 +267,42 @@ int run_sclam_pure_calibration(int argc, char** argv) { calibEdge->setInformation(Eigen::Matrix3d::Identity()); calibEdge->setMeasurement(meas); if (!optimizer.addEdge(calibEdge)) { - cerr << "Error adding calib edge" << endl; + cerr << "Error adding calib edge\n"; } } if (fixLaser) { - cerr << "Fix position of the laser offset" << endl; + cerr << "Fix position of the laser offset\n"; laserOffset->setFixed(true); } - cerr << "\nPerforming partial non-linear estimation" << endl; + cerr << "\nPerforming partial non-linear estimation\n"; optimizer.initializeOptimization(); optimizer.computeActiveErrors(); optimizer.optimize(maxIterations); cerr << "Calibrated laser offset (x, y, theta):" - << laserOffset->estimate().toVector().transpose() << endl; + << laserOffset->estimate().toVector().transpose() << '\n'; odomCalib(0) = -1. * linearSolution(0) * odomParamsVertex->estimate(); odomCalib(1) = linearSolution(1) * odomParamsVertex->estimate(); odomCalib(2) = odomParamsVertex->estimate(); cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " - << odomCalib.transpose() << endl; + << odomCalib.transpose() << '\n'; { SE2 closedFormLaser; Eigen::Vector3d closedFormOdom; ClosedFormCalibration::calibrate(motions, closedFormLaser, closedFormOdom); - cerr << "\nObtaining closed form solution" << endl; + cerr << "\nObtaining closed form solution\n"; cerr << "Calibrated laser offset (x, y, theta):" - << closedFormLaser.toVector().transpose() << endl; + << closedFormLaser.toVector().transpose() << '\n'; cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " - << closedFormOdom.transpose() << endl; + << closedFormOdom.transpose() << '\n'; } if (!dumpGraphFilename.empty()) { cerr << "Writing " << dumpGraphFilename << " ... "; optimizer.save(dumpGraphFilename.c_str()); - cerr << "done." << endl; + cerr << "done.\n"; } // optional input of a separate file for applying the odometry calibration @@ -324,7 +311,7 @@ int run_sclam_pure_calibration(int argc, char** argv) { const int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue); if (numTestOdom == 0) { - cerr << "Unable to read test data" << endl; + cerr << "Unable to read test data\n"; } else { std::ofstream rawStream("odometry_raw.txt"); std::ofstream calibratedStream("odometry_calibrated.txt"); @@ -363,14 +350,16 @@ int run_sclam_pure_calibration(int argc, char** argv) { // write output rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " - << prev->odomPose().rotation().angle() << endl; + << prev->odomPose().rotation().angle() << '\n'; calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " - << calOdomPose.rotation().angle() << endl; + << calOdomPose.rotation().angle() << '\n'; prevCalibratedPose = calOdomPose; prev = cur; } + rawStream.close(); + calibratedStream.close(); } } diff --git a/g2o/examples/convert_segment_line/convertSegmentLine.cpp b/g2o/examples/convert_segment_line/convertSegmentLine.cpp index 893998055..8f6d62b47 100644 --- a/g2o/examples/convert_segment_line/convertSegmentLine.cpp +++ b/g2o/examples/convert_segment_line/convertSegmentLine.cpp @@ -48,7 +48,17 @@ #include "g2o/stuff/macros.h" #include "g2o/stuff/string_tools.h" #include "g2o/stuff/timeutil.h" -#include "g2o/types/slam2d_addons/types_slam2d_addons.h" +#include "g2o/types/slam2d/edge_se2.h" +#include "g2o/types/slam2d/edge_se2_pointxy.h" +#include "g2o/types/slam2d/vertex_point_xy.h" +#include "g2o/types/slam2d/vertex_se2.h" +#include "g2o/types/slam2d_addons/edge_line2d_pointxy.h" +#include "g2o/types/slam2d_addons/edge_se2_line2d.h" +#include "g2o/types/slam2d_addons/edge_se2_segment2d.h" +#include "g2o/types/slam2d_addons/edge_se2_segment2d_line.h" +#include "g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h" +#include "g2o/types/slam2d_addons/vertex_line2d.h" +#include "g2o/types/slam2d_addons/vertex_segment2d.h" using std::cerr; using std::endl; diff --git a/g2o/examples/data_convert/convert_sba_slam3d.cpp b/g2o/examples/data_convert/convert_sba_slam3d.cpp index 437fe8aa0..b1e0f999d 100644 --- a/g2o/examples/data_convert/convert_sba_slam3d.cpp +++ b/g2o/examples/data_convert/convert_sba_slam3d.cpp @@ -25,24 +25,22 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include #include #include "g2o/core/optimizable_graph.h" #include "g2o/stuff/command_args.h" -#include "g2o/stuff/macros.h" -#include "g2o/types/sba/types_sba.h" +#include "g2o/types/sba/edge_project_p2sc.h" +#include "g2o/types/sba/vertex_cam.h" #include "g2o/types/slam3d/edge_se3_pointxyz_disparity.h" #include "g2o/types/slam3d/parameter_camera.h" using std::cerr; using std::cout; -using std::endl; using std::string; namespace g2o { - -static int convert_sba_slam3d(int argc, char** argv) { +namespace { +int convert_sba_slam3d(int argc, char** argv) { string inputFilename; string outputFilename; // command line parsing @@ -57,7 +55,7 @@ static int convert_sba_slam3d(int argc, char** argv) { OptimizableGraph inputGraph; const bool loadStatus = inputGraph.load(inputFilename.c_str()); if (!loadStatus) { - cerr << "Error while loading input data" << endl; + cerr << "Error while loading input data\n"; return 1; } @@ -77,8 +75,9 @@ static int convert_sba_slam3d(int argc, char** argv) { const SBACam& c = v->estimate(); baseline = c.baseline; fx = c.Kcam(0, 0); - camParams->setKcam(c.Kcam(0, 0), c.Kcam(1, 1), c.Kcam(0, 2), - c.Kcam(1, 2)); + CameraWithOffset cam; + cam.setKcam(c.Kcam(0, 0), c.Kcam(1, 1), c.Kcam(0, 2), c.Kcam(1, 2)); + camParams->setParam(cam); outputGraph.addParameter(camParams); } @@ -126,15 +125,16 @@ static int convert_sba_slam3d(int argc, char** argv) { } cout << "Vertices in/out:\t" << inputGraph.vertices().size() << " " - << outputGraph.vertices().size() << endl; + << outputGraph.vertices().size() << '\n'; cout << "Edges in/out:\t" << inputGraph.edges().size() << " " - << outputGraph.edges().size() << endl; + << outputGraph.edges().size() << '\n'; cout << "Writing output ... " << std::flush; outputGraph.save(outputFilename.c_str()); - cout << "done." << endl; + cout << "done.\n"; return 0; } +} // namespace } // namespace g2o int main(int argc, char** argv) { return g2o::convert_sba_slam3d(argc, argv); } diff --git a/g2o/examples/data_fitting/circle_fit.cpp b/g2o/examples/data_fitting/circle_fit.cpp index b54a6a89a..30084fb02 100644 --- a/g2o/examples/data_fitting/circle_fit.cpp +++ b/g2o/examples/data_fitting/circle_fit.cpp @@ -62,10 +62,6 @@ double errorOfSolution(const PointVector& points, */ class VertexCircle : public g2o::BaseVertex<3, Eigen::Vector3d> { public: - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } - void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update.head(); } @@ -81,9 +77,6 @@ class VertexCircle : public g2o::BaseVertex<3, Eigen::Vector3d> { class EdgePointOnCircle : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexCircle> { public: - bool read(std::istream& /*is*/) override { return false; } - bool write(std::ostream& /*os*/) const override { return false; } - template bool operator()(const T* circle, T* error) const { typename g2o::VectorN<2, T>::ConstMapType center(circle); diff --git a/g2o/examples/data_fitting/curve_fit.cpp b/g2o/examples/data_fitting/curve_fit.cpp index b00b046a3..3a680f325 100644 --- a/g2o/examples/data_fitting/curve_fit.cpp +++ b/g2o/examples/data_fitting/curve_fit.cpp @@ -33,7 +33,6 @@ #include "g2o/core/optimization_algorithm_factory.h" #include "g2o/core/sparse_optimizer.h" #include "g2o/stuff/command_args.h" -#include "g2o/stuff/logger.h" #include "g2o/stuff/sampler.h" G2O_USE_OPTIMIZATION_LIBRARY(dense); @@ -43,10 +42,6 @@ G2O_USE_OPTIMIZATION_LIBRARY(dense); */ class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d> { public: - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } - void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update.head(); } @@ -62,15 +57,6 @@ class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d> { class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams> { public: - bool read(std::istream& /*is*/) override { - G2O_ERROR("not implemented yet"); - return false; - } - bool write(std::ostream& /*os*/) const override { - G2O_ERROR("not implemented yet"); - return false; - } - template bool operator()(const T* params, T* error) const { const T& a = params[0]; diff --git a/g2o/examples/dynamic_vertex/polynomial_fit.cpp b/g2o/examples/dynamic_vertex/polynomial_fit.cpp index d0b53c82b..00fd07e18 100644 --- a/g2o/examples/dynamic_vertex/polynomial_fit.cpp +++ b/g2o/examples/dynamic_vertex/polynomial_fit.cpp @@ -9,7 +9,6 @@ #include -#include "g2o/core/base_binary_edge.h" #include "g2o/core/base_dynamic_vertex.h" #include "g2o/core/base_unary_edge.h" #include "g2o/core/block_solver.h" @@ -29,29 +28,6 @@ class PolynomialCoefficientVertex // Create the vertex PolynomialCoefficientVertex() = default; - // Read the vertex - bool read(std::istream& is) override { - // Read the dimension - int dimension; - is >> dimension; - if (!is.good()) { - return false; - } - - // Set the dimension; we call the method here to ensure stuff like - // cache and the workspace is setup - setDimension(dimension); - - // Read the state - return g2o::internal::readVector(is, estimate_); - } - - // Write the vertex - bool write(std::ostream& os) const override { - os << estimate_.size() << " "; - return g2o::internal::writeVector(os, estimate_); - } - // Direct linear add void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update; @@ -98,18 +74,6 @@ class PolynomialSingleValueEdge setInformation(omega); } - bool read(std::istream& is) override { - double z; - is >> x_ >> z; - setMeasurement(z); - return readInformationMatrix(is); - } - - bool write(std::ostream& os) const override { - os << x_ << " " << measurement_; - return writeInformationMatrix(os); - } - // Compute the measurement from the eigen polynomial module void computeError() override { const PolynomialCoefficientVertex* vertex = vertexXnRaw<0>(); @@ -134,7 +98,7 @@ int main(int argc, const char* argv[]) { p[i] = g2o::sampleUniform(-1, 1); } - std::cout << "Ground truth vector=" << p.transpose() << std::endl; + std::cout << "Ground truth vector=" << p.transpose() << '\n'; // The number of observations in the polynomial; the default is 6 int obs = 6; @@ -199,15 +163,13 @@ int main(int argc, const char* argv[]) { pv->setDimension(testDimension); optimizer->initializeOptimization(); optimizer->optimize(10); - std::cout << "Computed parameters = " << pv->estimate().transpose() - << std::endl; + std::cout << "Computed parameters = " << pv->estimate().transpose() << '\n'; } for (int testDimension = polynomialDimension - 1; testDimension >= 1; --testDimension) { pv->setDimension(testDimension); optimizer->initializeOptimization(); optimizer->optimize(10); - std::cout << "Computed parameters = " << pv->estimate().transpose() - << std::endl; + std::cout << "Computed parameters = " << pv->estimate().transpose() << '\n'; } } diff --git a/g2o/examples/dynamic_vertex/static_dynamic_function_fit.cpp b/g2o/examples/dynamic_vertex/static_dynamic_function_fit.cpp index 3d5ae2bf3..734bdcab1 100644 --- a/g2o/examples/dynamic_vertex/static_dynamic_function_fit.cpp +++ b/g2o/examples/dynamic_vertex/static_dynamic_function_fit.cpp @@ -18,9 +18,9 @@ #include "g2o/core/base_binary_edge.h" #include "g2o/core/base_dynamic_vertex.h" -#include "g2o/core/base_unary_edge.h" #include "g2o/core/base_vertex.h" #include "g2o/core/block_solver.h" +#include "g2o/core/eigen_types.h" #include "g2o/core/optimization_algorithm_levenberg.h" #include "g2o/core/sparse_optimizer.h" #include "g2o/solvers/eigen/linear_solver_eigen.h" @@ -31,22 +31,10 @@ // This vertex stores the coefficients of the f(x) polynomial. This is // quadratic, and always has a degree of three. -class FPolynomialCoefficientVertex - : public g2o::BaseVertex<3, Eigen::Vector3d> { +class FPolynomialCoefficientVertex : public g2o::BaseVertex<3, g2o::Vector3> { public: // Create the vertex - FPolynomialCoefficientVertex() { setToOrigin(); } - - // Read the vertex - bool read(std::istream& is) override { - // Read the state - return g2o::internal::readVector(is, estimate_); - } - - // Write the vertex - bool write(std::ostream& os) const override { - return g2o::internal::writeVector(os, estimate_); - } + FPolynomialCoefficientVertex() { setEstimate(g2o::Vector3::Zero()); } // Direct linear add void oplusImpl(const g2o::VectorX::MapType& update) override { @@ -63,29 +51,6 @@ class PPolynomialCoefficientVertex // Create the vertex PPolynomialCoefficientVertex() = default; - // Read the vertex - bool read(std::istream& is) override { - // Read the dimension - int dimension; - is >> dimension; - if (!is.good()) { - return false; - } - - // Set the dimension; we call the method here to ensure stuff like - // cache and the workspace is setup - setDimension(dimension); - - // Read the state - return g2o::internal::readVector(is, estimate_); - } - - // Write the vertex - bool write(std::ostream& os) const override { - os << estimate_.size() << " "; - return g2o::internal::writeVector(os, estimate_); - } - // Direct linear add void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update; @@ -122,21 +87,6 @@ class MultipleValueEdge setInformation(I); } - bool read(std::istream& is) override { - Eigen::VectorXd z; - g2o::internal::readVector(is, x_); - g2o::internal::readVector(is, z); - setMeasurement(z); - - return readInformationMatrix(is); - } - - bool write(std::ostream& os) const override { - g2o::internal::writeVector(os, x_); - g2o::internal::writeVector(os, measurement_); - return writeInformationMatrix(os); - } - // Compute the measurement from the eigen polynomial module void computeError() override { const FPolynomialCoefficientVertex* fvertex = vertexXnRaw<0>(); @@ -177,7 +127,7 @@ int main(int argc, const char* argv[]) { } std::cout << "Ground truth vectors f=" << f.transpose() - << "; p=" << p.transpose() << std::endl; + << "; p=" << p.transpose() << '\n'; // The number of observations in the polynomial; the default is 6 int obs = 6; @@ -258,7 +208,7 @@ int main(int argc, const char* argv[]) { optimizer->initializeOptimization(); optimizer->optimize(10); std::cout << "Computed parameters: f=" << pf->estimate().transpose() - << "; p=" << pv->estimate().transpose() << std::endl; + << "; p=" << pv->estimate().transpose() << '\n'; } for (int testDimension = polynomialDimension - 1; testDimension >= 1; --testDimension) { @@ -266,6 +216,6 @@ int main(int argc, const char* argv[]) { optimizer->initializeOptimization(); optimizer->optimize(10); std::cout << "Computed parameters: f= " << pf->estimate().transpose() - << "; p=" << pv->estimate().transpose() << std::endl; + << "; p=" << pv->estimate().transpose() << '\n'; } } diff --git a/g2o/examples/g2o_hierarchical/simple_star_ops.cpp b/g2o/examples/g2o_hierarchical/simple_star_ops.cpp index 6977b3991..8aa6d4346 100644 --- a/g2o/examples/g2o_hierarchical/simple_star_ops.cpp +++ b/g2o/examples/g2o_hierarchical/simple_star_ops.cpp @@ -29,7 +29,6 @@ #include #include #include -#include #include #include "backbone_tree_action.h" @@ -37,7 +36,6 @@ #include "g2o/core/optimization_algorithm_with_hessian.h" using std::cerr; -using std::endl; namespace g2o { @@ -234,7 +232,7 @@ void computeSimpleStars(StarSet& stars, SparseOptimizer* optimizer, HyperGraph::EdgeSet& backboneEdges = s->lowLevelEdges(); if (backboneEdges.empty()) continue; - // cerr << "optimizing backbone" << endl; + // cerr << "optimizing backbone\n"; // one of these should be the gauge, to be simple we select the first one // in the backbone OptimizableGraph::VertexSet gauge; @@ -247,7 +245,7 @@ void computeSimpleStars(StarSet& stars, SparseOptimizer* optimizer, s->optimizer()->optimize(backboneIterations); s->optimizer()->setFixed(backboneVertices, true); - // cerr << "assignind edges.vertices not in bbone" << endl; + // cerr << "assignind edges.vertices not in bbone\n"; HyperGraph::EdgeSet otherEdges; HyperGraph::VertexSet otherVertices; for (auto bit = backboneVertices.begin(); bit != backboneVertices.end(); @@ -275,27 +273,26 @@ void computeSimpleStars(StarSet& stars, SparseOptimizer* optimizer, s->optimizer()->solver().get()); if (solverWithHessian) { s->optimizer()->push(otherVertices); - // cerr << "optimizing vertices out of bbone" << endl; - // cerr << "push" << endl; - // cerr << "init" << endl; + // cerr << "optimizing vertices out of bbone\n"; + // cerr << "push\n"; + // cerr << "init\n"; s->optimizer()->initializeOptimization(otherEdges); - // cerr << "guess" << endl; + // cerr << "guess\n"; s->optimizer()->computeInitialGuess(); - // cerr << "solver init" << endl; + // cerr << "solver init\n"; s->optimizer()->solver()->init(); - // cerr << "structure" << endl; + // cerr << "structure\n"; if (!solverWithHessian->buildLinearStructure()) - cerr << "FATAL: failure while building linear structure" << endl; - // cerr << "errors" << endl; + cerr << "FATAL: failure while building linear structure\n"; + // cerr << "errors\n"; s->optimizer()->computeActiveErrors(); - // cerr << "system" << endl; + // cerr << "system\n"; solverWithHessian->updateLinearSystem(); - // cerr << "directSolove" << endl; + // cerr << "directSolove\n"; } else { cerr << "FATAL: hierarchical thing cannot be used with a solver that " "does not support the system structure " - "construction" - << endl; + "construction\n"; } // // then optimize the vertices one at a time to check if a solution is @@ -316,24 +313,23 @@ void computeSimpleStars(StarSet& stars, SparseOptimizer* optimizer, // cerr << endl; // relax the backbone and optimize it all - // cerr << "relax bbone" << endl; + // cerr << "relax bbone\n"; s->optimizer()->setFixed(backboneVertices, false); - // cerr << "fox gauge bbone" << endl; + // cerr << "fox gauge bbone\n"; s->optimizer()->setFixed(s->gauge(), true); - // cerr << "opt init" << endl; + // cerr << "opt init\n"; s->optimizer()->initializeOptimization(s->lowLevelEdges()); optimizer->computeActiveErrors(); const int starOptResult = s->optimizer()->optimize(starIterations); - // cerr << starOptResult << "(" << starIterations << ") " << endl; + // cerr << starOptResult << "(" << starIterations << ") \n"; if (!starIterations || starOptResult > 0) { optimizer->computeActiveErrors(); #if 1 - s->optimizer()->computeActiveErrors(); - // cerr << "system" << endl; + // cerr << "system\n"; if (solverWithHessian) solverWithHessian->updateLinearSystem(); HyperGraph::EdgeSet prunedStarEdges = backboneEdges; HyperGraph::VertexSet prunedStarVertices = backboneVertices; @@ -370,9 +366,9 @@ void computeSimpleStars(StarSet& stars, SparseOptimizer* optimizer, } s->lowLevelEdges() = prunedStarEdges; s->lowLevelVertices() = prunedStarVertices; - #endif - // cerr << "addHedges" << endl; + + // cerr << "addHedges\n"; // now add to the star the hierarchical edges OptimizableGraph::VertexContainer vertices(2); vertices[0] = std::static_pointer_cast( diff --git a/g2o/examples/g2o_hierarchical/simple_star_ops.h b/g2o/examples/g2o_hierarchical/simple_star_ops.h index 9a80a344b..b9b27a29b 100644 --- a/g2o/examples/g2o_hierarchical/simple_star_ops.h +++ b/g2o/examples/g2o_hierarchical/simple_star_ops.h @@ -27,12 +27,10 @@ #ifndef G2O_SIMPLE_STAR_OPS_ #define G2O_SIMPLE_STAR_OPS_ -#include #include #include "edge_creator.h" #include "edge_labeler.h" -#include "g2o/core/hyper_dijkstra.h" #include "g2o/core/sparse_optimizer.h" #include "g2o_hierarchical_api.h" #include "star.h" diff --git a/g2o/examples/line_slam/simulator_3d_line.cpp b/g2o/examples/line_slam/simulator_3d_line.cpp index 622905ea0..b4aad9f46 100644 --- a/g2o/examples/line_slam/simulator_3d_line.cpp +++ b/g2o/examples/line_slam/simulator_3d_line.cpp @@ -4,14 +4,17 @@ #include "g2o/core/sparse_optimizer.h" #include "g2o/stuff/command_args.h" -#include "g2o/stuff/macros.h" #include "g2o/stuff/sampler.h" -#include "g2o/types/slam3d/types_slam3d.h" -#include "g2o/types/slam3d_addons/types_slam3d_addons.h" +#include "g2o/types/slam3d/edge_se3.h" +#include "g2o/types/slam3d/edge_se3_prior.h" +#include "g2o/types/slam3d/vertex_se3.h" +#include "g2o/types/slam3d_addons/edge_se3_line.h" +#include "g2o/types/slam3d_addons/vertex_line3d.h" namespace g2o { +namespace { -static Isometry3 sample_noise_from_se3(const Vector6& cov) { +Isometry3 sample_noise_from_se3(const Vector6& cov) { double nx = Sampler::gaussRand(0., cov(0)); double ny = Sampler::gaussRand(0., cov(1)); double nz = Sampler::gaussRand(0., cov(2)); @@ -30,7 +33,7 @@ static Isometry3 sample_noise_from_se3(const Vector6& cov) { return retval; } -static Vector4 sample_noise_from_line(const Vector4& cov) { +Vector4 sample_noise_from_line(const Vector4& cov) { return Vector4(Sampler::gaussRand(0., cov(0)), Sampler::gaussRand(0., cov(1)), Sampler::gaussRand(0., cov(2)), Sampler::gaussRand(0., cov(3))); @@ -226,7 +229,7 @@ struct LineSensor : public Sensor { Vector4 nline; }; -static int simulator_3d_line(int argc, char** argv) { +int simulator_3d_line(int argc, char** argv) { bool fixLines; bool planarMotion; CommandArgs arg; @@ -240,13 +243,13 @@ static int simulator_3d_line(int argc, char** argv) { odomOffset->setId(0); g->addParameter(odomOffset); - std::cout << "Creating simulator" << std::endl; + std::cout << "Creating simulator\n"; auto* sim = new Simulator(g); - std::cout << "Creating robot" << std::endl; + std::cout << "Creating robot\n"; auto* r = new Robot(g); - std::cout << "Creating line sensor" << std::endl; + std::cout << "Creating line sensor\n"; Isometry3 sensorPose = Isometry3::Identity(); auto* ls = new LineSensor(r, 0, sensorPose); ls->nline = Vector4(0.001, 0.001, 0.001, 0.0001); @@ -255,7 +258,7 @@ static int simulator_3d_line(int argc, char** argv) { sim->robots.push_back(r); Line3D line; - std::cout << "Creating landmark line 1" << std::endl; + std::cout << "Creating landmark line 1\n"; auto* li = new LineItem(g, 1); Vector6 liv; liv << 0.0, 0.0, 5.0, 0.0, 1.0, 0.0; @@ -264,7 +267,7 @@ static int simulator_3d_line(int argc, char** argv) { li->vertex()->setFixed(fixLines); sim->world.insert(li); - std::cout << "Creating landmark line 2" << std::endl; + std::cout << "Creating landmark line 2\n"; liv << 5.0, 0.0, 0.0, 0.0, 0.0, 1.0; line = Line3D::fromCartesian(liv); li = new LineItem(g, 2); @@ -272,7 +275,7 @@ static int simulator_3d_line(int argc, char** argv) { li->vertex()->setFixed(fixLines); sim->world.insert(li); - std::cout << "Creating landmark line 3" << std::endl; + std::cout << "Creating landmark line 3\n"; liv << 0.0, 5.0, 0.0, 1.0, 0.0, 0.0; line = Line3D::fromCartesian(liv); li = new LineItem(g, 3); @@ -331,7 +334,7 @@ static int simulator_3d_line(int argc, char** argv) { } } } - std::cout << std::endl; + std::cout << '\n'; ls->offsetVertex->setFixed(true); auto gauge = g->vertex(4); @@ -341,11 +344,12 @@ static int simulator_3d_line(int argc, char** argv) { std::ofstream osp("line3d.g2o"); g->save(osp); - std::cout << "Saved graph on file line3d.g2o, use g2o_viewer to work with it." - << std::endl; + std::cout + << "Saved graph on file line3d.g2o, use g2o_viewer to work with it.\n"; return 0; } +} // namespace } // namespace g2o int main(int argc, char** argv) { return g2o::simulator_3d_line(argc, argv); } diff --git a/g2o/examples/plane_slam/simulator_3d_plane.cpp b/g2o/examples/plane_slam/simulator_3d_plane.cpp index 3983aa60f..617a40ee3 100644 --- a/g2o/examples/plane_slam/simulator_3d_plane.cpp +++ b/g2o/examples/plane_slam/simulator_3d_plane.cpp @@ -30,14 +30,17 @@ #include "g2o/core/sparse_optimizer.h" #include "g2o/stuff/command_args.h" -#include "g2o/stuff/macros.h" #include "g2o/stuff/sampler.h" -#include "g2o/types/slam3d/types_slam3d.h" -#include "g2o/types/slam3d_addons/types_slam3d_addons.h" +#include "g2o/types/slam3d/edge_se3.h" +#include "g2o/types/slam3d/edge_se3_prior.h" +#include "g2o/types/slam3d/vertex_se3.h" +#include "g2o/types/slam3d_addons/edge_se3_plane_calib.h" +#include "g2o/types/slam3d_addons/vertex_plane.h" namespace g2o { +namespace { -static Isometry3 sample_noise_from_se3(const Vector6& cov) { +Isometry3 sample_noise_from_se3(const Vector6& cov) { double nx = g2o::Sampler::gaussRand(0., cov(0)); double ny = g2o::Sampler::gaussRand(0., cov(1)); double nz = g2o::Sampler::gaussRand(0., cov(2)); @@ -56,7 +59,7 @@ static Isometry3 sample_noise_from_se3(const Vector6& cov) { return retval; } -static Vector3 sample_noise_from_plane(const Vector3& cov) { +Vector3 sample_noise_from_plane(const Vector3& cov) { return Vector3(g2o::Sampler::gaussRand(0., cov(0)), g2o::Sampler::gaussRand(0., cov(1)), g2o::Sampler::gaussRand(0., cov(2))); @@ -250,14 +253,14 @@ struct PlaneSensor : public Sensor { Vector3 _nplane; }; -static int simulator_3d_plane(int argc, char** argv) { +int simulator_3d_plane(int argc, char** argv) { CommandArgs arg; bool fixSensor; bool fixPlanes; bool fixFirstPose; bool fixTrajectory; bool planarMotion; - std::cerr << "graph" << std::endl; + std::cerr << "graph\n"; arg.param("fixSensor", fixSensor, false, "fix the sensor position on the robot"); arg.param("fixTrajectory", fixTrajectory, false, "fix the trajectory"); @@ -272,13 +275,13 @@ static int simulator_3d_plane(int argc, char** argv) { odomOffset->setId(0); g->addParameter(odomOffset); - std::cerr << "sim" << std::endl; + std::cerr << "sim\n"; auto* sim = new Simulator(g); - std::cerr << "robot" << std::endl; + std::cerr << "robot\n"; auto* r = new Robot(g); - std::cerr << "planeSensor" << std::endl; + std::cerr << "planeSensor\n"; Matrix3 R = Matrix3::Identity(); R << 0, 0, 1, -1, 0, 0, 0, -1, 0; @@ -290,7 +293,7 @@ static int simulator_3d_plane(int argc, char** argv) { r->sensors.push_back(ps); sim->robots.push_back(r); - std::cerr << "p1" << std::endl; + std::cerr << "p1\n"; Plane3D plane; auto* pi = new PlaneItem(g, 1); plane.fromVector(Eigen::Vector4d(0., 0., 1., 5.)); @@ -304,7 +307,7 @@ static int simulator_3d_plane(int argc, char** argv) { pi->vertex()->setFixed(fixPlanes); sim->world.insert(pi); - std::cerr << "p2" << std::endl; + std::cerr << "p2\n"; pi = new PlaneItem(g, 3); plane.fromVector(Eigen::Vector4d(0., 1., 0., 5.)); static_cast(pi->vertex().get())->setEstimate(plane); @@ -407,7 +410,7 @@ static int simulator_3d_plane(int argc, char** argv) { return 0; } - +} // namespace } // namespace g2o int main(int argc, char** argv) { return g2o::simulator_3d_plane(argc, argv); } diff --git a/g2o/examples/sim3/optimize_sphere_by_sim3.cpp b/g2o/examples/sim3/optimize_sphere_by_sim3.cpp index 82c17e0cb..1e3229c03 100644 --- a/g2o/examples/sim3/optimize_sphere_by_sim3.cpp +++ b/g2o/examples/sim3/optimize_sphere_by_sim3.cpp @@ -25,24 +25,17 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include #include #include -#include #include "g2o/core/block_solver.h" #include "g2o/core/factory.h" -#include "g2o/core/optimization_algorithm_gauss_newton.h" #include "g2o/core/optimization_algorithm_levenberg.h" -#include "g2o/core/robust_kernel_impl.h" #include "g2o/core/solver.h" #include "g2o/core/sparse_optimizer.h" -#include "g2o/solvers/dense/linear_solver_dense.h" #include "g2o/solvers/eigen/linear_solver_eigen.h" #include "g2o/types/sim3/types_seven_dof_expmap.h" #include "g2o/types/slam3d/edge_se3.h" -#include "g2o/types/slam3d/types_slam3d.h" #include "g2o/types/slam3d/vertex_se3.h" G2O_USE_TYPE_GROUP(slam3d); @@ -74,7 +67,7 @@ void ToVertexSE3(const g2o::VertexSim3Expmap& v_sim3, v_se3->setEstimate(se3); } -// Converte EdgeSE3 to EdgeSim3 +// Convert EdgeSE3 to EdgeSim3 void ToEdgeSim3(const g2o::EdgeSE3& e_se3, g2o::EdgeSim3* const e_sim3) { Eigen::Isometry3d se3 = e_se3.measurement().inverse(); Eigen::Matrix3d r = se3.rotation(); @@ -86,18 +79,18 @@ void ToEdgeSim3(const g2o::EdgeSE3& e_se3, g2o::EdgeSim3* const e_sim3) { // Using VertexSim3 and EdgeSim3 is the core of this example. // This example optimize the data created by create_sphere. -// Because the data is recore by VertexSE3 and EdgeSE3, SE3 is used for +// Because the data is recorded by VertexSE3 and EdgeSE3, SE3 is used for // interface and Sim is used for optimization. // g2o_viewer is available to the result. int optimize_by_sim3(int argc, char** argv) { if (argc != 2) { - std::cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << std::endl; + std::cout << "Usage: pose_graph_g2o_SE3 sphere.g2o\n"; return 1; } std::ifstream fin(argv[1]); if (!fin) { - std::cout << "file " << argv[1] << " does not exist." << std::endl; + std::cout << "file " << argv[1] << " does not exist.\n"; return 1; } @@ -149,11 +142,11 @@ int optimize_by_sim3(int argc, char** argv) { optimizer.addEdge(e_sim3); } - std::cout << "optimizing ..." << std::endl; + std::cout << "optimizing ...\n"; optimizer.initializeOptimization(); optimizer.optimize(30); - std::cout << "saving optimization results in VertexSE3..." << std::endl; + std::cout << "saving optimization results in VertexSE3...\n"; auto vertices_sim3 = optimizer.vertices(); auto vertices_se3 = interface.vertices(); diff --git a/g2o/examples/sphere/create_sphere.cpp b/g2o/examples/sphere/create_sphere.cpp index 2c7b68a39..76e90b72f 100644 --- a/g2o/examples/sphere/create_sphere.cpp +++ b/g2o/examples/sphere/create_sphere.cpp @@ -30,7 +30,10 @@ #include #include +#include "g2o/core/abstract_graph.h" +#include "g2o/core/eigen_types.h" #include "g2o/core/factory.h" +#include "g2o/core/io/io_format.h" #include "g2o/stuff/command_args.h" #include "g2o/stuff/sampler.h" #include "g2o/types/slam3d/edge_se3.h" @@ -38,7 +41,8 @@ namespace g2o { -static int create_sphere(int argc, char** argv) { +namespace { +int create_sphere(int argc, char** argv) { // command line parsing int nodesPerLevel; int numLaps; @@ -67,23 +71,23 @@ static int create_sphere(int argc, char** argv) { arg.parseArgs(argc, argv); if (noiseTranslation.empty()) { - std::cerr << "using default noise for the translation" << std::endl; + std::cerr << "using default noise for the translation\n"; noiseTranslation.push_back(0.01); noiseTranslation.push_back(0.01); noiseTranslation.push_back(0.01); } std::cerr << "Noise for the translation:"; for (const double i : noiseTranslation) std::cerr << " " << i; - std::cerr << std::endl; + std::cerr << '\n'; if (noiseRotation.empty()) { - std::cerr << "using default noise for the rotation" << std::endl; + std::cerr << "using default noise for the rotation\n"; noiseRotation.push_back(0.005); noiseRotation.push_back(0.005); noiseRotation.push_back(0.005); } std::cerr << "Noise for the rotation:"; for (const double i : noiseRotation) std::cerr << " " << i; - std::cerr << std::endl; + std::cerr << '\n'; Eigen::Matrix3d transNoise = Eigen::Matrix3d::Zero(); for (int i = 0; i < 3; ++i) @@ -164,7 +168,7 @@ static int create_sphere(int argc, char** argv) { seedSeq.generate(seeds.begin(), seeds.end()); std::cerr << "using seeds:"; for (const int seed : seeds) std::cerr << " " << seed; - std::cerr << std::endl; + std::cerr << '\n'; transSampler.seed(seeds[0]); rotSampler.seed(seeds[1]); } @@ -187,7 +191,7 @@ static int create_sphere(int argc, char** argv) { rot = gtQuat * rot; trans = gtTrans + trans; - Eigen::Isometry3d noisyMeasurement = Eigen::Isometry3d(rot); + Eigen::Isometry3d noisyMeasurement(rot); noisyMeasurement.translation() = trans; e->setMeasurement(noisyMeasurement); } @@ -201,35 +205,52 @@ static int create_sphere(int argc, char** argv) { e->initialEstimate(aux, to.get()); } - // write output - std::ofstream fileOutputStream; - if (outFilename != "-") { - std::cerr << "Writing into " << outFilename << std::endl; - fileOutputStream.open(outFilename.c_str()); - } else { - std::cerr << "writing to stdout" << std::endl; - } - const std::string vertexTag = Factory::instance()->tag(vertices[0].get()); const std::string edgeTag = Factory::instance()->tag(edges[0].get()); - std::ostream& fout = outFilename != "-" ? fileOutputStream : std::cout; + g2o::AbstractGraph output; for (auto& vertex : vertices) { - fout << vertexTag << " " << vertex->id() << " "; - vertex->write(fout); - fout << std::endl; + std::vector vertex_estimate; + vertex->getEstimateData(vertex_estimate); + g2o::AbstractGraph output; + output.vertices().emplace_back(vertexTag, vertex->id(), vertex_estimate); } for (auto& edge : edges) { auto from = edge->vertexXn<0>(); auto to = edge->vertexXn<1>(); - fout << edgeTag << " " << from->id() << " " << to->id() << " "; - edge->write(fout); - fout << std::endl; + + std::vector ids = {from->id(), to->id()}; + std::vector data(edge->measurementDimension()); + edge->getMeasurementData(data.data()); + MatrixX::MapType information(edge->informationData(), edge->dimension(), + edge->dimension()); + std::vector upper_triangle; + upper_triangle.reserve((edge->dimension() * (edge->dimension() + 1)) / 2); + for (int r = 0; r < edge->dimension(); ++r) + for (int c = r; c < edge->dimension(); ++c) + upper_triangle.push_back(information(r, c)); + output.edges().emplace_back(edgeTag, ids, data, upper_triangle, + edge->parameterIds()); + } + + // write output + std::ofstream fileOutputStream; + if (outFilename != "-") { + std::cerr << "Writing into " << outFilename << '\n'; + fileOutputStream.open(outFilename.c_str()); + } else { + std::cerr << "writing to stdout\n"; + } + + std::ostream& fout = outFilename != "-" ? fileOutputStream : std::cout; + if (!output.save(fout, g2o::io::Format::kG2O)) { + std::cerr << "Error while writing\n"; } return 0; } +} // namespace } // namespace g2o int main(int argc, char** argv) { return g2o::create_sphere(argc, argv); } diff --git a/g2o/examples/target/targetTypes3D.hpp b/g2o/examples/target/targetTypes3D.hpp index a8fcf0643..06e32d7fc 100644 --- a/g2o/examples/target/targetTypes3D.hpp +++ b/g2o/examples/target/targetTypes3D.hpp @@ -16,10 +16,6 @@ class VertexPosition3D : public g2o::BaseVertex<3, Eigen::Vector3d> { void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update; } - - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } }; // Store velocity separately from position? @@ -30,10 +26,6 @@ class VertexVelocity3D : public g2o::BaseVertex<3, Eigen::Vector3d> { void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update; } - - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } }; // The idealised GPS measurement; this is 3D and linear @@ -46,10 +38,6 @@ class GPSObservationPosition3DEdge const VertexPosition3D* v = vertexXnRaw<0>(); error_ = v->estimate() - measurement_; } - - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } }; #endif // __TARGET_TYPES_3D_HPP__ diff --git a/g2o/examples/target/targetTypes6D.hpp b/g2o/examples/target/targetTypes6D.hpp index c1c943245..6802634ee 100644 --- a/g2o/examples/target/targetTypes6D.hpp +++ b/g2o/examples/target/targetTypes6D.hpp @@ -21,10 +21,6 @@ class VertexPosition3D : public g2o::BaseVertex<3, Eigen::Vector3d> { void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update; } - - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } }; class PositionVelocity3DEdge {}; @@ -36,10 +32,6 @@ class VertexPositionVelocity3D : public g2o::BaseVertex<6, Vector6d> { void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update; } - - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } }; // The odometry which links pairs of nodes together @@ -112,10 +104,6 @@ class TargetOdometry3DEdge } } - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } - private: double dt_; }; @@ -134,10 +122,6 @@ class GPSObservationEdgePositionVelocity3D const VertexPositionVelocity3D* v = vertexXnRaw<0>(); error_ = v->estimate().head<3>() - measurement_; } - - bool read(std::istream& /*is*/) override { return false; } - - bool write(std::ostream& /*os*/) const override { return false; } }; #endif // __TARGET_TYPES_6D_HPP__ diff --git a/g2o/examples/tutorial_slam2d/CMakeLists.txt b/g2o/examples/tutorial_slam2d/CMakeLists.txt index 1adcc5c4d..de27387e1 100644 --- a/g2o/examples/tutorial_slam2d/CMakeLists.txt +++ b/g2o/examples/tutorial_slam2d/CMakeLists.txt @@ -3,7 +3,7 @@ add_library(tutorial_slam2d_library ${G2O_LIB_TYPE} edge_se2.cpp edge_se2_pointxy.h vertex_se2.cpp edge_se2.h vertex_point_xy.cpp vertex_se2.h parameter_se2_offset.h parameter_se2_offset.cpp - types_tutorial_slam2d.h types_tutorial_slam2d.cpp + types_tutorial_slam2d.cpp simulator.h simulator.cpp g2o_tutorial_slam2d_api.h ) diff --git a/g2o/examples/tutorial_slam2d/edge_se2.cpp b/g2o/examples/tutorial_slam2d/edge_se2.cpp index fd7091116..1d6d11edb 100644 --- a/g2o/examples/tutorial_slam2d/edge_se2.cpp +++ b/g2o/examples/tutorial_slam2d/edge_se2.cpp @@ -26,29 +26,18 @@ #include "edge_se2.h" -namespace g2o { -namespace tutorial { +namespace g2o::tutorial { -bool EdgeSE2::read(std::istream& is) { - Vector3 p; - is >> p[0] >> p[1] >> p[2]; - measurement_.fromVector(p); - inverseMeasurement_ = measurement().inverse(); - for (int i = 0; i < 3; ++i) - for (int j = i; j < 3; ++j) { - is >> information()(i, j); - if (i != j) information()(j, i) = information()(i, j); - } - return true; +void EdgeSE2::computeError() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSE2* v2 = vertexXnRaw<1>(); + SE2 delta = inverseMeasurement_ * (v1->estimate().inverse() * v2->estimate()); + error_ = delta.toVector(); } -bool EdgeSE2::write(std::ostream& os) const { - Vector3 p = measurement().toVector(); - os << p.x() << " " << p.y() << " " << p.z(); - for (int i = 0; i < 3; ++i) - for (int j = i; j < 3; ++j) os << " " << information()(i, j); - return os.good(); +void EdgeSE2::setMeasurement(const SE2& m) { + measurement_ = m; + inverseMeasurement_ = m.inverse(); } -} // namespace tutorial -} // namespace g2o +} // namespace g2o::tutorial diff --git a/g2o/examples/tutorial_slam2d/edge_se2.h b/g2o/examples/tutorial_slam2d/edge_se2.h index ae9e70b16..780510cba 100644 --- a/g2o/examples/tutorial_slam2d/edge_se2.h +++ b/g2o/examples/tutorial_slam2d/edge_se2.h @@ -31,9 +31,7 @@ #include "g2o_tutorial_slam2d_api.h" #include "vertex_se2.h" -namespace g2o { - -namespace tutorial { +namespace g2o::tutorial { /** * \brief 2D edge between two Vertex2, i.e., the odometry @@ -43,28 +41,14 @@ class G2O_TUTORIAL_SLAM2D_API EdgeSE2 public: EdgeSE2() = default; - void computeError() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSE2* v2 = vertexXnRaw<1>(); - SE2 delta = - inverseMeasurement_ * (v1->estimate().inverse() * v2->estimate()); - error_ = delta.toVector(); - } - - void setMeasurement(const SE2& m) override { - measurement_ = m; - inverseMeasurement_ = m.inverse(); - } + void computeError() override; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + void setMeasurement(const SE2& m) override; protected: SE2 inverseMeasurement_; }; -} // namespace tutorial - -} // namespace g2o +} // namespace g2o::tutorial #endif diff --git a/g2o/examples/tutorial_slam2d/edge_se2_pointxy.cpp b/g2o/examples/tutorial_slam2d/edge_se2_pointxy.cpp index ee5407411..4332364a7 100644 --- a/g2o/examples/tutorial_slam2d/edge_se2_pointxy.cpp +++ b/g2o/examples/tutorial_slam2d/edge_se2_pointxy.cpp @@ -34,18 +34,6 @@ EdgeSE2PointXY::EdgeSE2PointXY() { installParameter(0); } -bool EdgeSE2PointXY::read(std::istream& is) { - readParamIds(is); - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeSE2PointXY::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE2PointXY::computeError() { const VertexPointXY* l2 = vertexXnRaw<1>(); error_ = (sensorCache_->w2n() * l2->estimate()) - measurement_; diff --git a/g2o/examples/tutorial_slam2d/edge_se2_pointxy.h b/g2o/examples/tutorial_slam2d/edge_se2_pointxy.h index 3fa16a866..a789de917 100644 --- a/g2o/examples/tutorial_slam2d/edge_se2_pointxy.h +++ b/g2o/examples/tutorial_slam2d/edge_se2_pointxy.h @@ -35,9 +35,7 @@ #include "vertex_point_xy.h" #include "vertex_se2.h" -namespace g2o { - -namespace tutorial { +namespace g2o::tutorial { class CacheSE2Offset; @@ -48,16 +46,12 @@ class G2O_TUTORIAL_SLAM2D_API EdgeSE2PointXY void computeError() override; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - protected: std::shared_ptr sensorCache_; bool resolveCaches() override; }; -} // namespace tutorial -} // namespace g2o +} // namespace g2o::tutorial #endif diff --git a/g2o/examples/tutorial_slam2d/parameter_se2_offset.cpp b/g2o/examples/tutorial_slam2d/parameter_se2_offset.cpp index a725b4829..70e76d41d 100644 --- a/g2o/examples/tutorial_slam2d/parameter_se2_offset.cpp +++ b/g2o/examples/tutorial_slam2d/parameter_se2_offset.cpp @@ -30,25 +30,7 @@ namespace g2o::tutorial { -void ParameterSE2Offset::setOffset(const SE2& offset) { - offset_ = offset; - inverseOffset_ = offset.inverse(); -} - -bool ParameterSE2Offset::read(std::istream& is) { - double x; - double y; - double th; - is >> x >> y >> th; - setOffset(SE2(x, y, th)); - return true; -} - -bool ParameterSE2Offset::write(std::ostream& os) const { - os << offset_.translation().x() << " " << offset_.translation().y() << " " - << offset_.rotation().angle(); - return os.good(); -} +void ParameterSE2Offset::update() { inverseOffset_ = param().inverse(); } void CacheSE2Offset::updateImpl() { #ifndef NDEBUG diff --git a/g2o/examples/tutorial_slam2d/parameter_se2_offset.h b/g2o/examples/tutorial_slam2d/parameter_se2_offset.h index 1037e2c61..130641c04 100644 --- a/g2o/examples/tutorial_slam2d/parameter_se2_offset.h +++ b/g2o/examples/tutorial_slam2d/parameter_se2_offset.h @@ -28,25 +28,22 @@ #define G2O_TUTORIAL_PARAMETER_SE2_OFFSET_H #include "g2o/core/cache.h" +#include "g2o/core/parameter.h" #include "g2o_tutorial_slam2d_api.h" #include "se2.h" namespace g2o::tutorial { -class G2O_TUTORIAL_SLAM2D_API ParameterSE2Offset : public Parameter { +class G2O_TUTORIAL_SLAM2D_API ParameterSE2Offset : public BaseParameter { public: ParameterSE2Offset() = default; - void setOffset(const SE2& offset = SE2()); - - [[nodiscard]] const SE2& offset() const { return offset_; } + [[nodiscard]] const SE2& offset() const { return parameter_; } [[nodiscard]] const SE2& inverseOffset() const { return inverseOffset_; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + void update() override; protected: - SE2 offset_; SE2 inverseOffset_; }; diff --git a/g2o/examples/tutorial_slam2d/se2.h b/g2o/examples/tutorial_slam2d/se2.h index 18d3d575c..2b8028b2b 100644 --- a/g2o/examples/tutorial_slam2d/se2.h +++ b/g2o/examples/tutorial_slam2d/se2.h @@ -32,7 +32,6 @@ #include #include "g2o/core/type_traits.h" -#include "g2o/stuff/macros.h" #include "g2o/stuff/misc.h" #include "g2o_tutorial_slam2d_api.h" @@ -147,8 +146,6 @@ struct TypeTraits { static Type fromMinimalVector(const Eigen::DenseBase& v) { return Type(v[0], v[1], v[2]); } - - static Type Identity() { return Type(); } }; } // namespace g2o diff --git a/g2o/examples/tutorial_slam2d/simulator.cpp b/g2o/examples/tutorial_slam2d/simulator.cpp index 497762a82..1d8f0c44f 100644 --- a/g2o/examples/tutorial_slam2d/simulator.cpp +++ b/g2o/examples/tutorial_slam2d/simulator.cpp @@ -30,6 +30,7 @@ #include #include +#include "g2o/stuff/macros.h" #include "g2o/stuff/sampler.h" using std::cerr; diff --git a/g2o/examples/tutorial_slam2d/simulator.h b/g2o/examples/tutorial_slam2d/simulator.h index 6acb04ed6..d4555a3e3 100644 --- a/g2o/examples/tutorial_slam2d/simulator.h +++ b/g2o/examples/tutorial_slam2d/simulator.h @@ -27,7 +27,6 @@ #ifndef G2O_SIMULATOR_H #define G2O_SIMULATOR_H -#include #include #include "g2o_tutorial_slam2d_api.h" diff --git a/g2o/examples/tutorial_slam2d/tutorial_slam2d.cpp b/g2o/examples/tutorial_slam2d/tutorial_slam2d.cpp index b42e3b6ae..e815986a3 100644 --- a/g2o/examples/tutorial_slam2d/tutorial_slam2d.cpp +++ b/g2o/examples/tutorial_slam2d/tutorial_slam2d.cpp @@ -24,29 +24,25 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include #include #include "edge_se2.h" #include "edge_se2_pointxy.h" #include "g2o/core/block_solver.h" -#include "g2o/core/factory.h" #include "g2o/core/optimization_algorithm_factory.h" #include "g2o/core/optimization_algorithm_gauss_newton.h" #include "g2o/core/sparse_optimizer.h" #include "g2o/solvers/eigen/linear_solver_eigen.h" #include "simulator.h" -#include "types_tutorial_slam2d.h" #include "vertex_point_xy.h" #include "vertex_se2.h" using std::cerr; -using std::endl; -namespace g2o { -namespace tutorial { +namespace g2o::tutorial { -static int run_slam2d_tutorial() { +namespace { +int run_slam2d_tutorial() { // TODO(Rainer): simulate different sensor offset // simulate a robot observing landmarks while traveling on a grid const SE2 sensorOffsetTransf(0.2, 0.1, -0.1); @@ -73,7 +69,7 @@ static int run_slam2d_tutorial() { // add the parameter representing the sensor offset auto sensorOffset = std::make_shared(); - sensorOffset->setOffset(sensorOffsetTransf); + sensorOffset->setParam(sensorOffsetTransf); sensorOffset->setId(0); optimizer.addParameter(sensorOffset); @@ -87,7 +83,7 @@ static int run_slam2d_tutorial() { robot->setEstimate(t); optimizer.addVertex(robot); } - cerr << "done." << endl; + cerr << "done.\n"; // second add the odometry constraints cerr << "Optimization: Adding odometry measurements ... "; @@ -99,7 +95,7 @@ static int run_slam2d_tutorial() { odometry->setInformation(simEdge.information); optimizer.addEdge(odometry); } - cerr << "done." << endl; + cerr << "done.\n"; // add the landmark observations cerr << "Optimization: add landmark vertices ... "; @@ -109,7 +105,7 @@ static int run_slam2d_tutorial() { landmark->setEstimate(l.simulatedPose); optimizer.addVertex(landmark); } - cerr << "done." << endl; + cerr << "done.\n"; cerr << "Optimization: add landmark observations ... "; for (const auto& simEdge : simulator.landmarkObservations()) { @@ -121,7 +117,7 @@ static int run_slam2d_tutorial() { landmarkObservation->setParameterId(0, sensorOffset->id()); optimizer.addEdge(landmarkObservation); } - cerr << "done." << endl; + cerr << "done.\n"; /********************************************************************************* * optimization @@ -137,10 +133,10 @@ static int run_slam2d_tutorial() { firstRobotPose->setFixed(true); optimizer.setVerbose(true); - cerr << "Optimizing" << endl; + cerr << "Optimizing\n"; optimizer.initializeOptimization(); optimizer.optimize(10); - cerr << "done." << endl; + cerr << "done.\n"; optimizer.save("tutorial_after.g2o"); @@ -149,8 +145,8 @@ static int run_slam2d_tutorial() { return 0; } +} // namespace -} // namespace tutorial -} // namespace g2o +} // namespace g2o::tutorial int main() { return g2o::tutorial::run_slam2d_tutorial(); } diff --git a/g2o/examples/tutorial_slam2d/types_tutorial_slam2d.cpp b/g2o/examples/tutorial_slam2d/types_tutorial_slam2d.cpp index 51abf4a24..292db64e9 100644 --- a/g2o/examples/tutorial_slam2d/types_tutorial_slam2d.cpp +++ b/g2o/examples/tutorial_slam2d/types_tutorial_slam2d.cpp @@ -24,15 +24,14 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include "types_tutorial_slam2d.h" - -#include - #include "g2o/core/factory.h" -#include "g2o/stuff/macros.h" +#include "g2o/examples/tutorial_slam2d/edge_se2.h" +#include "g2o/examples/tutorial_slam2d/edge_se2_pointxy.h" +#include "g2o/examples/tutorial_slam2d/parameter_se2_offset.h" +#include "g2o/examples/tutorial_slam2d/vertex_point_xy.h" +#include "g2o/examples/tutorial_slam2d/vertex_se2.h" -namespace g2o { -namespace tutorial { +namespace g2o::tutorial { G2O_REGISTER_TYPE_GROUP(tutorial_slam2d); @@ -45,5 +44,4 @@ G2O_REGISTER_TYPE(TUTORIAL_CACHE_SE2_OFFSET, CacheSE2Offset); G2O_REGISTER_TYPE(TUTORIAL_EDGE_SE2, EdgeSE2); G2O_REGISTER_TYPE(TUTORIAL_EDGE_SE2_POINT_XY, EdgeSE2PointXY); -} // namespace tutorial -} // namespace g2o +} // namespace g2o::tutorial diff --git a/g2o/examples/tutorial_slam2d/vertex_point_xy.cpp b/g2o/examples/tutorial_slam2d/vertex_point_xy.cpp index efb6e2410..fefeffc8a 100644 --- a/g2o/examples/tutorial_slam2d/vertex_point_xy.cpp +++ b/g2o/examples/tutorial_slam2d/vertex_point_xy.cpp @@ -30,14 +30,8 @@ namespace g2o::tutorial { VertexPointXY::VertexPointXY() { estimate_.setZero(); } -bool VertexPointXY::read(std::istream& is) { - is >> estimate_[0] >> estimate_[1]; - return true; -} - -bool VertexPointXY::write(std::ostream& os) const { - os << estimate()(0) << " " << estimate()(1); - return os.good(); +void VertexPointXY::oplusImpl(const g2o::VectorX::MapType& update) { + estimate_ += update; } } // namespace g2o::tutorial diff --git a/g2o/examples/tutorial_slam2d/vertex_point_xy.h b/g2o/examples/tutorial_slam2d/vertex_point_xy.h index 55a6ceb24..646604215 100644 --- a/g2o/examples/tutorial_slam2d/vertex_point_xy.h +++ b/g2o/examples/tutorial_slam2d/vertex_point_xy.h @@ -30,7 +30,6 @@ #include #include "g2o/core/base_vertex.h" -#include "g2o/core/hyper_graph_action.h" #include "g2o_tutorial_slam2d_api.h" namespace g2o::tutorial { @@ -40,12 +39,7 @@ class G2O_TUTORIAL_SLAM2D_API VertexPointXY public: VertexPointXY(); - void oplusImpl(const g2o::VectorX::MapType& update) override { - estimate_ += update; - } - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + void oplusImpl(const g2o::VectorX::MapType& update) override; }; } // namespace g2o::tutorial diff --git a/g2o/examples/tutorial_slam2d/vertex_se2.cpp b/g2o/examples/tutorial_slam2d/vertex_se2.cpp index c70495254..464f29a54 100644 --- a/g2o/examples/tutorial_slam2d/vertex_se2.cpp +++ b/g2o/examples/tutorial_slam2d/vertex_se2.cpp @@ -28,19 +28,9 @@ namespace g2o::tutorial { -VertexSE2::VertexSE2() = default; - -bool VertexSE2::read(std::istream& is) { - Eigen::Vector3d p; - is >> p[0] >> p[1] >> p[2]; - estimate_.fromVector(p); - return true; -} - -bool VertexSE2::write(std::ostream& os) const { - const Eigen::Vector3d p = estimate().toVector(); - os << p[0] << " " << p[1] << " " << p[2]; - return os.good(); +void VertexSE2::oplusImpl(const g2o::VectorX::MapType& update) { + SE2 up(update[0], update[1], update[2]); + estimate_ *= up; } } // namespace g2o::tutorial diff --git a/g2o/examples/tutorial_slam2d/vertex_se2.h b/g2o/examples/tutorial_slam2d/vertex_se2.h index 8d5e3b4e7..2e2eeb52c 100644 --- a/g2o/examples/tutorial_slam2d/vertex_se2.h +++ b/g2o/examples/tutorial_slam2d/vertex_se2.h @@ -28,7 +28,6 @@ #define G2O_TUTORIAL_VERTEX_SE2_H #include "g2o/core/base_vertex.h" -#include "g2o/core/hyper_graph_action.h" #include "g2o_tutorial_slam2d_api.h" #include "se2.h" @@ -39,15 +38,7 @@ namespace g2o::tutorial { */ class G2O_TUTORIAL_SLAM2D_API VertexSE2 : public BaseVertex<3, SE2> { public: - VertexSE2(); - - void oplusImpl(const g2o::VectorX::MapType& update) override { - SE2 up(update[0], update[1], update[2]); - estimate_ *= up; - } - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + void oplusImpl(const g2o::VectorX::MapType& update) override; }; } // namespace g2o::tutorial diff --git a/g2o/solvers/slam2d_linear/solver_slam2d_linear.cpp b/g2o/solvers/slam2d_linear/solver_slam2d_linear.cpp index d919402a3..bf0eebb70 100644 --- a/g2o/solvers/slam2d_linear/solver_slam2d_linear.cpp +++ b/g2o/solvers/slam2d_linear/solver_slam2d_linear.cpp @@ -214,7 +214,7 @@ bool SolverSLAM2DLinear::solveOrientation() { // update the orientation of the 2D poses and set translation to 0, GN shall // solve that - root->setToOrigin(); + root->setEstimate(SE2()); for (auto* vv : optimizer_->indexMapping()) { auto* v = static_cast(vv); const int poseIdx = v->hessianIndex(); diff --git a/g2o/stuff/logger_format.h b/g2o/stuff/logger_format.h index e02b00d93..195e48880 100644 --- a/g2o/stuff/logger_format.h +++ b/g2o/stuff/logger_format.h @@ -30,8 +30,10 @@ #include "g2o/config.h" #ifdef G2O_HAVE_LOGGING -#include // IWYU pragma: export -#include // IWYU pragma: export +// IWYU pragma: begin_exports +#include +#include +// IWYU pragma: end_exports #if FMT_VERSION >= 90000 // see https://fmt.dev/9.0.0/api.html#std-ostream-support diff --git a/g2o/stuff/tuple_tools.h b/g2o/stuff/tuple_tools.h index 01ab663e7..bc6354c61 100644 --- a/g2o/stuff/tuple_tools.h +++ b/g2o/stuff/tuple_tools.h @@ -30,15 +30,32 @@ namespace g2o { +namespace internal { +template +auto tail_impl(std::index_sequence, std::tuple t) { + return std::make_tuple(std::get(t)...); +} + template -void tuple_apply_i_(F&& f, T& t, int i, std::index_sequence) { +void tuple_apply_i_impl(F&& f, T& t, int i, std::index_sequence) { (..., (I == i ? f(std::get(t)) : void())); } +} // namespace internal template void tuple_apply_i(F&& f, T& t, int i) { - tuple_apply_i_( + internal::tuple_apply_i_impl( f, t, i, std::make_index_sequence>>()); } +template +auto tupple_head(std::tuple t) { + return std::get<0>(t); +} + +template +auto tuple_tail(std::tuple t) { + return internal::tail_impl(std::make_index_sequence(), t); +} + } // namespace g2o diff --git a/g2o/types/icp/edge_gicp.h b/g2o/types/icp/edge_gicp.h index 6d153ee44..b0bdcf953 100644 --- a/g2o/types/icp/edge_gicp.h +++ b/g2o/types/icp/edge_gicp.h @@ -133,8 +133,6 @@ struct TypeTraits { static Type fromMinimalVector(const Eigen::DenseBase& v) { return fromVector(v); } - - static Type Identity() { return Type(); } }; } // namespace g2o diff --git a/g2o/types/icp/types_icp.cpp b/g2o/types/icp/types_icp.cpp index 3ce18b885..d821e5d64 100644 --- a/g2o/types/icp/types_icp.cpp +++ b/g2o/types/icp/types_icp.cpp @@ -28,13 +28,10 @@ #include #include -#include -#include #include "g2o/core/eigen_types.h" #include "g2o/core/factory.h" #include "g2o/stuff/misc.h" -#include "g2o/types/icp/edge_gicp.h" namespace g2o { @@ -67,40 +64,6 @@ double VertexSCam::baseline_; // Rigid 3D constraint between poses, given fixed point offsets // -// input two matched points between the frames -// first point belongs to the first frame, position and normal -// second point belongs to the second frame, position and normal -// -// the measurement variable has type EdgeGICP (see types_icp.h) - -bool EdgeVVGicp::read(std::istream& is) { - // measured point and normal - for (int i = 0; i < 3; i++) is >> measurement_.pos0[i]; - for (int i = 0; i < 3; i++) is >> measurement_.normal0[i]; - - // measured point and normal - for (int i = 0; i < 3; i++) is >> measurement_.pos1[i]; - for (int i = 0; i < 3; i++) is >> measurement_.normal1[i]; - - measurement_.makeRot0(); // set up rotation matrices - - // GICP info matrices - - // point-plane only - Matrix3 prec; - double v = cst(.01); - prec << v, 0, 0, 0, v, 0, 0, 0, 1; - const Matrix3& R = measurement().R0; // plane of the point in vp0 - information() = R.transpose() * prec * R; - - // information().setIdentity(); - - // setRobustKernel(true); - // setHuberWidth(0.01); // units? m? - - return true; -} - // return the error estimate as a 3-vector void EdgeVVGicp::computeError() { // from to @@ -166,18 +129,6 @@ void EdgeVVGicp::linearizeOplus() { } #endif -bool EdgeVVGicp::write(std::ostream& os) const { - // first point - for (int i = 0; i < 3; i++) os << measurement().pos0[i] << " "; - for (int i = 0; i < 3; i++) os << measurement().normal0[i] << " "; - - // second point - for (int i = 0; i < 3; i++) os << measurement().pos1[i] << " "; - for (int i = 0; i < 3; i++) os << measurement().normal1[i] << " "; - - return os.good(); -} - // // stereo camera functions // @@ -276,9 +227,6 @@ void Edge_XYZ_VSC::linearizeOplus() { (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px } #endif -bool EdgeXyzVsc::read(std::istream&) { return false; } - -bool EdgeXyzVsc::write(std::ostream&) const { return false; } void EdgeXyzVsc::computeError() { // from to @@ -295,10 +243,6 @@ void EdgeXyzVsc::computeError() { error_ = kp - measurement_; } -bool VertexSCam::read(std::istream&) { return false; } - -bool VertexSCam::write(std::ostream&) const { return false; } - void VertexSCam::oplusImpl(const VectorX::MapType& update) { VertexSE3::oplusImpl(update); setAll(); diff --git a/g2o/types/icp/types_icp.h b/g2o/types/icp/types_icp.h index e075706f7..22e73d20e 100644 --- a/g2o/types/icp/types_icp.h +++ b/g2o/types/icp/types_icp.h @@ -36,16 +36,9 @@ #include #include -#include -#include #include "edge_gicp.h" #include "g2o/core/base_binary_edge.h" -#include "g2o/core/base_variable_sized_edge.h" -#include "g2o/core/base_vertex.h" -#include "g2o/core/type_traits.h" -#include "g2o/types/sba/types_sba.h" -#include "g2o/types/slam3d/types_slam3d.h" #include "g2o_types_icp_api.h" namespace g2o { @@ -63,10 +56,6 @@ class G2O_TYPES_ICP_API EdgeVVGicp bool pl_pl = false; Matrix3 cov0, cov1; - // Custom I/O functions - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - // return the error estimate as a 3-vector void computeError() override; @@ -90,10 +79,6 @@ class G2O_TYPES_ICP_API EdgeVVGicp */ class G2O_TYPES_ICP_API VertexSCam : public VertexSE3 { public: - // I/O - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - // capture the update function to reset aux transforms void oplusImpl(const VectorX::MapType& update) override; @@ -152,9 +137,6 @@ class G2O_TYPES_ICP_API VertexSCam : public VertexSE3 { class G2O_TYPES_ICP_API EdgeXyzVsc : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSCam> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - // return the error estimate as a 3-vector void computeError() override; diff --git a/g2o/types/sba/edge_project_p2mc.cpp b/g2o/types/sba/edge_project_p2mc.cpp index d867e24f6..d2c8a6cd0 100644 --- a/g2o/types/sba/edge_project_p2mc.cpp +++ b/g2o/types/sba/edge_project_p2mc.cpp @@ -26,13 +26,7 @@ #include "edge_project_p2mc.h" -#include - -#include -#include - -#include "g2o/core/io_helper.h" -#include "g2o/stuff/macros.h" +#include "g2o/stuff/logger.h" #include "g2o/types/sba/sbacam.h" #include "g2o/types/sba/vertex_cam.h" @@ -41,18 +35,6 @@ namespace g2o { // point to camera projection, monocular EdgeProjectP2MC::EdgeProjectP2MC() { information().setIdentity(); } -bool EdgeProjectP2MC::read(std::istream& is) { - // measured keypoint - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeProjectP2MC::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - writeInformationMatrix(os); - return os.good(); -} - void EdgeProjectP2MC::computeError() { // from to const VertexPointXYZ* point = vertexXnRaw<0>(); @@ -92,7 +74,7 @@ void EdgeProjectP2MC::linearizeOplus() { double pz = pc(2); double ipz2 = 1.0 / (pz * pz); if (std::isnan(ipz2)) { - std::cout << "[SetJac] infinite jac" << std::endl; + G2O_CRITICAL("[SetJac] infinite jac"); abort(); } diff --git a/g2o/types/sba/edge_project_p2mc.h b/g2o/types/sba/edge_project_p2mc.h index e865e3625..0db0df38b 100644 --- a/g2o/types/sba/edge_project_p2mc.h +++ b/g2o/types/sba/edge_project_p2mc.h @@ -27,9 +27,6 @@ #ifndef G2O_SBA_EDGEPROJECTP2MC_H #define G2O_SBA_EDGEPROJECTP2MC_H -#include -#include - #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/types/slam3d/vertex_pointxyz.h" @@ -44,8 +41,6 @@ class G2O_TYPES_SBA_API EdgeProjectP2MC : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexCam> { public: EdgeProjectP2MC(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; //! return the error estimate as a 2-vector void computeError() override; diff --git a/g2o/types/sba/edge_project_p2sc.cpp b/g2o/types/sba/edge_project_p2sc.cpp index 57c7ba47d..441f6d184 100644 --- a/g2o/types/sba/edge_project_p2sc.cpp +++ b/g2o/types/sba/edge_project_p2sc.cpp @@ -26,11 +26,8 @@ #include "edge_project_p2sc.h" -#include #include -#include -#include "g2o/core/io_helper.h" #include "g2o/stuff/logger.h" #include "g2o/stuff/misc.h" #include "g2o/types/sba/sbacam.h" @@ -38,18 +35,6 @@ namespace g2o { -// point to camera projection, stereo -bool EdgeProjectP2SC::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeProjectP2SC::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - writeInformationMatrix(os); - return os.good(); -} - // return the error estimate as a 2-vector void EdgeProjectP2SC::computeError() { // from to diff --git a/g2o/types/sba/edge_project_p2sc.h b/g2o/types/sba/edge_project_p2sc.h index a647f7844..e544783b8 100644 --- a/g2o/types/sba/edge_project_p2sc.h +++ b/g2o/types/sba/edge_project_p2sc.h @@ -27,9 +27,6 @@ #ifndef G2O_SBA_EDGEPROJECTP2SC_H #define G2O_SBA_EDGEPROJECTP2SC_H -#include -#include - #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/types/slam3d/vertex_pointxyz.h" @@ -43,9 +40,6 @@ namespace g2o { class G2O_TYPES_SBA_API EdgeProjectP2SC : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexCam> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - //! return the error estimate as a 2-vector void computeError() override; diff --git a/g2o/types/sba/edge_project_psi2uv.cpp b/g2o/types/sba/edge_project_psi2uv.cpp index 86d65ba36..c935d48f2 100644 --- a/g2o/types/sba/edge_project_psi2uv.cpp +++ b/g2o/types/sba/edge_project_psi2uv.cpp @@ -27,9 +27,7 @@ #include "edge_project_psi2uv.h" #include -#include -#include "g2o/core/io_helper.h" #include "g2o/types/sba/parameter_cameraparameters.h" #include "g2o/types/sba/vertex_se3_expmap.h" #include "g2o/types/slam3d/se3quat.h" @@ -43,18 +41,6 @@ EdgeProjectPSI2UV::EdgeProjectPSI2UV() { installParameter(0); } -bool EdgeProjectPSI2UV::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - -bool EdgeProjectPSI2UV::read(std::istream& is) { - readParamIds(is); - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - void EdgeProjectPSI2UV::computeError() { const VertexPointXYZ* psi = vertexXnRaw<0>(); const VertexSE3Expmap* T_p_from_world = vertexXnRaw<1>(); @@ -74,15 +60,15 @@ void EdgeProjectPSI2UV::linearizeOplus() { VertexSE3Expmap* vpose = vertexXnRaw<1>(); SE3Quat T_cw = vpose->estimate(); VertexSE3Expmap* vanchor = vertexXnRaw<2>(); - const CameraParameters* cam = - static_cast(parameter(0).get()); + const StereoCameraParameters& cam = + static_cast(parameter(0).get())->param(); SE3Quat A_aw = vanchor->estimate(); SE3Quat T_ca = T_cw * A_aw.inverse(); Vector3 x_a = internal::invert_depth(psi_a); Vector3 y = T_ca * x_a; Eigen::Matrix Jcam = - internal::d_proj_d_y(cam->focal_length, y); + internal::d_proj_d_y(cam.focal_length, y); auto& jacobianOplus0 = std::get<0>(this->jacobianOplus_); auto& jacobianOplus1 = std::get<1>(this->jacobianOplus_); diff --git a/g2o/types/sba/edge_project_psi2uv.h b/g2o/types/sba/edge_project_psi2uv.h index adb9b0b6c..127e4dcb9 100644 --- a/g2o/types/sba/edge_project_psi2uv.h +++ b/g2o/types/sba/edge_project_psi2uv.h @@ -27,7 +27,6 @@ #ifndef G2O_SBA_EDGEPROJECTPSI2UV_H #define G2O_SBA_EDGEPROJECTPSI2UV_H -#include #include #include "g2o/core/base_fixed_sized_edge.h" @@ -48,8 +47,6 @@ class G2O_TYPES_SBA_API EdgeProjectPSI2UV public: EdgeProjectPSI2UV(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; void linearizeOplus() override; diff --git a/g2o/types/sba/edge_project_stereo_xyz.cpp b/g2o/types/sba/edge_project_stereo_xyz.cpp index 31bbf5fba..a2eb62470 100644 --- a/g2o/types/sba/edge_project_stereo_xyz.cpp +++ b/g2o/types/sba/edge_project_stereo_xyz.cpp @@ -26,7 +26,6 @@ #include "edge_project_stereo_xyz.h" -#include "g2o/core/io_helper.h" #include "g2o/types/sba/vertex_se3_expmap.h" namespace g2o { @@ -41,16 +40,6 @@ Vector3 EdgeStereoSE3ProjectXYZ::cam_project(const Vector3& trans_xyz, return res; } -bool EdgeStereoSE3ProjectXYZ::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeStereoSE3ProjectXYZ::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeStereoSE3ProjectXYZ::linearizeOplus() { VertexSE3Expmap* vj = vertexXnRaw<1>(); SE3Quat T(vj->estimate()); diff --git a/g2o/types/sba/edge_project_stereo_xyz.h b/g2o/types/sba/edge_project_stereo_xyz.h index 5b934a7a9..5df1a3ad7 100644 --- a/g2o/types/sba/edge_project_stereo_xyz.h +++ b/g2o/types/sba/edge_project_stereo_xyz.h @@ -27,13 +27,8 @@ #ifndef G2O_SBA_EDGEPROJECTSTEREOXYZ_H #define G2O_SBA_EDGEPROJECTSTEREOXYZ_H -#include -#include -#include - #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" -#include "g2o/types/slam3d/se3quat.h" #include "g2o/types/slam3d/vertex_pointxyz.h" #include "g2o_types_sba_api.h" #include "vertex_se3_expmap.h" @@ -44,9 +39,6 @@ namespace g2o { class G2O_TYPES_SBA_API EdgeStereoSE3ProjectXYZ : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void computeError() override { const VertexSE3Expmap* v1 = vertexXnRaw<1>(); const VertexPointXYZ* v2 = vertexXnRaw<0>(); diff --git a/g2o/types/sba/edge_project_stereo_xyz_onlypose.cpp b/g2o/types/sba/edge_project_stereo_xyz_onlypose.cpp index 47b4d153e..313bc8b69 100644 --- a/g2o/types/sba/edge_project_stereo_xyz_onlypose.cpp +++ b/g2o/types/sba/edge_project_stereo_xyz_onlypose.cpp @@ -28,22 +28,10 @@ #include -#include "g2o/core/io_helper.h" #include "g2o/types/sba/vertex_se3_expmap.h" -#include "g2o/types/slam3d/se3quat.h" namespace g2o { -bool EdgeStereoSE3ProjectXYZOnlyPose::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeStereoSE3ProjectXYZOnlyPose::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeStereoSE3ProjectXYZOnlyPose::linearizeOplus() { VertexSE3Expmap* vi = vertexXnRaw<0>(); Vector3 xyz_trans = vi->estimate().map(Xw); diff --git a/g2o/types/sba/edge_project_stereo_xyz_onlypose.h b/g2o/types/sba/edge_project_stereo_xyz_onlypose.h index 6b87d639f..d953a0e88 100644 --- a/g2o/types/sba/edge_project_stereo_xyz_onlypose.h +++ b/g2o/types/sba/edge_project_stereo_xyz_onlypose.h @@ -27,9 +27,6 @@ #ifndef G2O_SBA_EDGEPROJECTSTEREOXYZ_ONLYPOSE_H #define G2O_SBA_EDGEPROJECTSTEREOXYZ_ONLYPOSE_H -#include -#include - #include "g2o/core/base_unary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o_types_sba_api.h" @@ -42,9 +39,6 @@ namespace g2o { class G2O_TYPES_SBA_API EdgeStereoSE3ProjectXYZOnlyPose : public BaseUnaryEdge<3, Vector3, VertexSE3Expmap> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void computeError() override; void linearizeOplus() override; diff --git a/g2o/types/sba/edge_project_xyz.cpp b/g2o/types/sba/edge_project_xyz.cpp index 0f2d29272..ccfbc5f30 100644 --- a/g2o/types/sba/edge_project_xyz.cpp +++ b/g2o/types/sba/edge_project_xyz.cpp @@ -28,23 +28,12 @@ #include -#include "g2o/core/io_helper.h" #include "g2o/types/sba/vertex_se3_expmap.h" #include "g2o/types/slam3d/se3_ops.h" #include "g2o/types/slam3d/se3quat.h" namespace g2o { -bool EdgeSE3ProjectXYZ::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeSE3ProjectXYZ::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE3ProjectXYZ::computeError() { const VertexSE3Expmap* v1 = vertexXnRaw<1>(); const VertexPointXYZ* v2 = vertexXnRaw<0>(); diff --git a/g2o/types/sba/edge_project_xyz.h b/g2o/types/sba/edge_project_xyz.h index b43518d48..ea496aff2 100644 --- a/g2o/types/sba/edge_project_xyz.h +++ b/g2o/types/sba/edge_project_xyz.h @@ -27,9 +27,6 @@ #ifndef G2O_SBA_EDGEPROJECTXYZ_H #define G2O_SBA_EDGEPROJECTXYZ_H -#include -#include - #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/types/slam3d/vertex_pointxyz.h" @@ -42,8 +39,6 @@ namespace g2o { class G2O_TYPES_SBA_API EdgeSE3ProjectXYZ : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; bool isDepthPositive(); diff --git a/g2o/types/sba/edge_project_xyz2uv.cpp b/g2o/types/sba/edge_project_xyz2uv.cpp index c6fb15f30..98997a057 100644 --- a/g2o/types/sba/edge_project_xyz2uv.cpp +++ b/g2o/types/sba/edge_project_xyz2uv.cpp @@ -28,7 +28,6 @@ #include -#include "g2o/core/io_helper.h" #include "g2o/types/sba/parameter_cameraparameters.h" #include "g2o/types/sba/vertex_se3_expmap.h" #include "g2o/types/slam3d/se3quat.h" @@ -40,18 +39,6 @@ EdgeProjectXYZ2UV::EdgeProjectXYZ2UV() { installParameter(0); } -bool EdgeProjectXYZ2UV::read(std::istream& is) { - readParamIds(is); - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeProjectXYZ2UV::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeProjectXYZ2UV::computeError() { const VertexSE3Expmap* v1 = vertexXnRaw<1>(); const VertexPointXYZ* v2 = vertexXnRaw<0>(); @@ -71,32 +58,33 @@ void EdgeProjectXYZ2UV::linearizeOplus() { double z = xyz_trans[2]; double z_2 = z * z; - auto cam = std::static_pointer_cast(parameter(0)); + const StereoCameraParameters& cam = + static_cast(parameter(0).get())->param(); Eigen::Matrix tmp; - tmp(0, 0) = cam->focal_length; + tmp(0, 0) = cam.focal_length; tmp(0, 1) = 0; - tmp(0, 2) = -x / z * cam->focal_length; + tmp(0, 2) = -x / z * cam.focal_length; tmp(1, 0) = 0; - tmp(1, 1) = cam->focal_length; - tmp(1, 2) = -y / z * cam->focal_length; + tmp(1, 1) = cam.focal_length; + tmp(1, 2) = -y / z * cam.focal_length; jacobianOplusXi_ = -1. / z * tmp * T.rotation().toRotationMatrix(); - jacobianOplusXj_(0, 0) = x * y / z_2 * cam->focal_length; - jacobianOplusXj_(0, 1) = -(1 + (x * x / z_2)) * cam->focal_length; - jacobianOplusXj_(0, 2) = y / z * cam->focal_length; - jacobianOplusXj_(0, 3) = -1. / z * cam->focal_length; + jacobianOplusXj_(0, 0) = x * y / z_2 * cam.focal_length; + jacobianOplusXj_(0, 1) = -(1 + (x * x / z_2)) * cam.focal_length; + jacobianOplusXj_(0, 2) = y / z * cam.focal_length; + jacobianOplusXj_(0, 3) = -1. / z * cam.focal_length; jacobianOplusXj_(0, 4) = 0; - jacobianOplusXj_(0, 5) = x / z_2 * cam->focal_length; + jacobianOplusXj_(0, 5) = x / z_2 * cam.focal_length; - jacobianOplusXj_(1, 0) = (1 + y * y / z_2) * cam->focal_length; - jacobianOplusXj_(1, 1) = -x * y / z_2 * cam->focal_length; - jacobianOplusXj_(1, 2) = -x / z * cam->focal_length; + jacobianOplusXj_(1, 0) = (1 + y * y / z_2) * cam.focal_length; + jacobianOplusXj_(1, 1) = -x * y / z_2 * cam.focal_length; + jacobianOplusXj_(1, 2) = -x / z * cam.focal_length; jacobianOplusXj_(1, 3) = 0; - jacobianOplusXj_(1, 4) = -1. / z * cam->focal_length; - jacobianOplusXj_(1, 5) = y / z_2 * cam->focal_length; + jacobianOplusXj_(1, 4) = -1. / z * cam.focal_length; + jacobianOplusXj_(1, 5) = y / z_2 * cam.focal_length; } } // namespace g2o diff --git a/g2o/types/sba/edge_project_xyz2uv.h b/g2o/types/sba/edge_project_xyz2uv.h index b8e86b1d3..08f628dd6 100644 --- a/g2o/types/sba/edge_project_xyz2uv.h +++ b/g2o/types/sba/edge_project_xyz2uv.h @@ -27,9 +27,7 @@ #ifndef G2O_SBA_EDGEPROJECTXYZ2UV_H #define G2O_SBA_EDGEPROJECTXYZ2UV_H -#include #include -#include #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" @@ -45,8 +43,6 @@ class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap> { public: EdgeProjectXYZ2UV(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; void linearizeOplus() override; diff --git a/g2o/types/sba/edge_project_xyz2uvu.cpp b/g2o/types/sba/edge_project_xyz2uvu.cpp index a93591ce6..523c55466 100644 --- a/g2o/types/sba/edge_project_xyz2uvu.cpp +++ b/g2o/types/sba/edge_project_xyz2uvu.cpp @@ -28,10 +28,8 @@ #include -#include "g2o/core/io_helper.h" #include "g2o/types/sba/parameter_cameraparameters.h" #include "g2o/types/sba/vertex_se3_expmap.h" -#include "g2o/types/slam3d/se3quat.h" namespace g2o { @@ -48,16 +46,4 @@ void EdgeProjectXYZ2UVU::computeError() { cam->stereocam_uvu_map(v1->estimate().map(v2->estimate())); } -bool EdgeProjectXYZ2UVU::read(std::istream& is) { - readParamIds(is); - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeProjectXYZ2UVU::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - } // namespace g2o diff --git a/g2o/types/sba/edge_project_xyz2uvu.h b/g2o/types/sba/edge_project_xyz2uvu.h index 34c386ef0..5b84464ef 100644 --- a/g2o/types/sba/edge_project_xyz2uvu.h +++ b/g2o/types/sba/edge_project_xyz2uvu.h @@ -27,9 +27,7 @@ #ifndef G2O_SBA_EDGEPROJECTXYZ2UVU_H #define G2O_SBA_EDGEPROJECTXYZ2UVU_H -#include #include -#include #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" @@ -49,8 +47,6 @@ class G2O_TYPES_SBA_API EdgeProjectXYZ2UVU : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap> { public: EdgeProjectXYZ2UVU(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; // virtual void linearizeOplus(); protected: diff --git a/g2o/types/sba/edge_project_xyz_onlypose.cpp b/g2o/types/sba/edge_project_xyz_onlypose.cpp index 7100f42ed..c61557ab5 100644 --- a/g2o/types/sba/edge_project_xyz_onlypose.cpp +++ b/g2o/types/sba/edge_project_xyz_onlypose.cpp @@ -28,23 +28,11 @@ #include -#include "g2o/core/io_helper.h" #include "g2o/types/sba/vertex_se3_expmap.h" #include "g2o/types/slam3d/se3_ops.h" -#include "g2o/types/slam3d/se3quat.h" namespace g2o { -bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() { VertexSE3Expmap* vi = vertexXnRaw<0>(); Vector3 xyz_trans = vi->estimate().map(Xw); diff --git a/g2o/types/sba/edge_project_xyz_onlypose.h b/g2o/types/sba/edge_project_xyz_onlypose.h index a7e11d053..25c8af8ee 100644 --- a/g2o/types/sba/edge_project_xyz_onlypose.h +++ b/g2o/types/sba/edge_project_xyz_onlypose.h @@ -27,9 +27,6 @@ #ifndef G2O_SBA_EDGEPROJECTXYZONLYPOSE_H #define G2O_SBA_EDGEPROJECTXYZONLYPOSE_H -#include -#include - #include "g2o/core/base_unary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o_types_sba_api.h" @@ -42,8 +39,6 @@ namespace g2o { class G2O_TYPES_SBA_API EdgeSE3ProjectXYZOnlyPose : public BaseUnaryEdge<2, Vector2, VertexSE3Expmap> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; bool isDepthPositive(); void linearizeOplus() override; diff --git a/g2o/types/sba/edge_sba_cam.cpp b/g2o/types/sba/edge_sba_cam.cpp index 3eabd76f7..6da06918f 100644 --- a/g2o/types/sba/edge_sba_cam.cpp +++ b/g2o/types/sba/edge_sba_cam.cpp @@ -27,27 +27,13 @@ #include "edge_sba_cam.h" #include -#include #include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" #include "g2o/types/sba/sbacam.h" #include "g2o/types/sba/vertex_cam.h" namespace g2o { -bool EdgeSBACam::read(std::istream& is) { - Vector7 meas; - internal::readVector(is, meas); - setMeasurement(SE3Quat(meas)); - return readInformationMatrix(is); -} - -bool EdgeSBACam::write(std::ostream& os) const { - internal::writeVector(os, measurement().toVector()); - return writeInformationMatrix(os); -} - void EdgeSBACam::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) { auto from = vertexXn<0>(); diff --git a/g2o/types/sba/edge_sba_cam.h b/g2o/types/sba/edge_sba_cam.h index 043d03248..a4f1fd0aa 100644 --- a/g2o/types/sba/edge_sba_cam.h +++ b/g2o/types/sba/edge_sba_cam.h @@ -27,9 +27,6 @@ #ifndef G2O_SBA_EDGESBACAM_H #define G2O_SBA_EDGESBACAM_H -#include -#include - #include "g2o/core/base_binary_edge.h" #include "g2o/core/optimizable_graph.h" #include "g2o/stuff/misc.h" @@ -45,8 +42,6 @@ namespace g2o { class G2O_TYPES_SBA_API EdgeSBACam : public BaseBinaryEdge<6, SE3Quat, VertexCam, VertexCam> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; void setMeasurement(const SE3Quat& meas) override; diff --git a/g2o/types/sba/edge_sba_scale.cpp b/g2o/types/sba/edge_sba_scale.cpp index 66df3d245..5331ba8df 100644 --- a/g2o/types/sba/edge_sba_scale.cpp +++ b/g2o/types/sba/edge_sba_scale.cpp @@ -27,8 +27,6 @@ #include "edge_sba_scale.h" #include -#include -#include #include "g2o/core/eigen_types.h" #include "g2o/types/sba/sbacam.h" @@ -37,20 +35,6 @@ namespace g2o { -bool EdgeSBAScale::read(std::istream& is) { - double meas; - is >> meas; - setMeasurement(meas); - information().setIdentity(); - is >> information()(0, 0); - return true; -} - -bool EdgeSBAScale::write(std::ostream& os) const { - os << measurement() << " " << information()(0, 0); - return os.good(); -} - void EdgeSBAScale::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) { auto v1 = vertexXn<0>(); diff --git a/g2o/types/sba/edge_sba_scale.h b/g2o/types/sba/edge_sba_scale.h index 6452a8b7b..7fce06c39 100644 --- a/g2o/types/sba/edge_sba_scale.h +++ b/g2o/types/sba/edge_sba_scale.h @@ -27,9 +27,6 @@ #ifndef G2O_SBA_EDGESBASCALE_H #define G2O_SBA_EDGESBASCALE_H -#include -#include - #include "g2o/core/base_binary_edge.h" #include "g2o/core/optimizable_graph.h" #include "g2o/stuff/misc.h" @@ -44,8 +41,6 @@ namespace g2o { class G2O_TYPES_SBA_API EdgeSBAScale : public BaseBinaryEdge<1, double, VertexCam, VertexCam> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; void setMeasurement(const double& m) override { measurement_ = m; } double initialEstimatePossible(const OptimizableGraph::VertexSet&, diff --git a/g2o/types/sba/edge_se3_expmap.cpp b/g2o/types/sba/edge_se3_expmap.cpp index cf8a0f6f8..fe22785cc 100644 --- a/g2o/types/sba/edge_se3_expmap.cpp +++ b/g2o/types/sba/edge_se3_expmap.cpp @@ -26,26 +26,10 @@ #include "edge_se3_expmap.h" -#include - -#include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" #include "g2o/types/sba/vertex_se3_expmap.h" namespace g2o { -bool EdgeSE3Expmap::read(std::istream& is) { - Vector7 meas; - internal::readVector(is, meas); - setMeasurement(SE3Quat(meas).inverse()); - return readInformationMatrix(is); -} - -bool EdgeSE3Expmap::write(std::ostream& os) const { - internal::writeVector(os, measurement().inverse().toVector()); - return writeInformationMatrix(os); -} - void EdgeSE3Expmap::computeError() { const VertexSE3Expmap* v1 = vertexXnRaw<0>(); const VertexSE3Expmap* v2 = vertexXnRaw<1>(); @@ -55,21 +39,21 @@ void EdgeSE3Expmap::computeError() { error_ = err.log(); } -void EdgeSE3Expmap::linearizeOplus() { - VertexSE3Expmap* vi = vertexXnRaw<0>(); - SE3Quat Ti(vi->estimate()); +// void EdgeSE3Expmap::linearizeOplus() { +// VertexSE3Expmap* vi = vertexXnRaw<0>(); +// SE3Quat Ti(vi->estimate()); - VertexSE3Expmap* vj = vertexXnRaw<1>(); - SE3Quat Tj(vj->estimate()); +// VertexSE3Expmap* vj = vertexXnRaw<1>(); +// SE3Quat Tj(vj->estimate()); - const SE3Quat& Tij = measurement_; - SE3Quat invTij = Tij.inverse(); +// const SE3Quat& Tij = measurement_; +// SE3Quat invTij = Tij.inverse(); - SE3Quat invTj_Tij = Tj.inverse() * Tij; - SE3Quat infTi_invTij = Ti.inverse() * invTij; +// SE3Quat invTj_Tij = Tj.inverse() * Tij; +// SE3Quat infTi_invTij = Ti.inverse() * invTij; - jacobianOplusXi_ = invTj_Tij.adj(); - jacobianOplusXj_ = -infTi_invTij.adj(); -} +// jacobianOplusXi_ = invTj_Tij.adj(); +// jacobianOplusXj_ = -infTi_invTij.adj(); +// } } // namespace g2o diff --git a/g2o/types/sba/edge_se3_expmap.h b/g2o/types/sba/edge_se3_expmap.h index 852cb2d95..ae232b9ae 100644 --- a/g2o/types/sba/edge_se3_expmap.h +++ b/g2o/types/sba/edge_se3_expmap.h @@ -27,9 +27,6 @@ #ifndef G2O_SBA_EDGE_SE3_EXPMAP_H #define G2O_SBA_EDGE_SE3_EXPMAP_H -#include -#include - #include "g2o/core/base_binary_edge.h" #include "g2o/types/slam3d/se3quat.h" #include "g2o_types_sba_api.h" @@ -43,10 +40,9 @@ namespace g2o { class G2O_TYPES_SBA_API EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; - void linearizeOplus() override; + // TODO(Rainer): Jacobian seems wrong, see #505 (but PR seems also wrong) + // void linearizeOplus() override; }; } // namespace g2o diff --git a/g2o/types/sba/parameter_cameraparameters.cpp b/g2o/types/sba/parameter_cameraparameters.cpp index 74c72f441..af71d1434 100644 --- a/g2o/types/sba/parameter_cameraparameters.cpp +++ b/g2o/types/sba/parameter_cameraparameters.cpp @@ -26,51 +26,34 @@ #include "parameter_cameraparameters.h" -#include -#include - #include "g2o/types/slam3d/se3_ops.h" namespace g2o { -CameraParameters::CameraParameters() : principle_point(Vector2(0., 0.)) {} - CameraParameters::CameraParameters( double focalLength, const Eigen::Ref& principlePoint, - double baseLine) - : focal_length(focalLength), - principle_point(principlePoint), - baseline(baseLine) {} - -bool CameraParameters::read(std::istream& is) { - is >> focal_length; - is >> principle_point[0]; - is >> principle_point[1]; - is >> baseline; - return true; -} - -bool CameraParameters::write(std::ostream& os) const { - os << focal_length << " "; - os << principle_point.x() << " "; - os << principle_point.y() << " "; - os << baseline << " "; - return true; + double baseLine) { + parameter_.focal_length = focalLength; + parameter_.principle_point = principlePoint; + parameter_.baseline = baseLine; } Vector2 CameraParameters::cam_map(const Vector3& trans_xyz) const { Vector2 proj = project(trans_xyz); Vector2 res; - res[0] = proj[0] * focal_length + principle_point[0]; - res[1] = proj[1] * focal_length + principle_point[1]; + res[0] = proj[0] * parameter_.focal_length + parameter_.principle_point[0]; + res[1] = proj[1] * parameter_.focal_length + parameter_.principle_point[1]; return res; } Vector3 CameraParameters::stereocam_uvu_map(const Vector3& trans_xyz) const { Vector2 uv_left = cam_map(trans_xyz); - double proj_x_right = (trans_xyz[0] - baseline) / trans_xyz[2]; - double u_right = proj_x_right * focal_length + principle_point[0]; + double proj_x_right = (trans_xyz[0] - parameter_.baseline) / trans_xyz[2]; + double u_right = + proj_x_right * parameter_.focal_length + parameter_.principle_point[0]; return Vector3(uv_left[0], uv_left[1], u_right); } +void CameraParameters::update() {} + } // namespace g2o diff --git a/g2o/types/sba/parameter_cameraparameters.h b/g2o/types/sba/parameter_cameraparameters.h index 2d4792a71..03ecb5cad 100644 --- a/g2o/types/sba/parameter_cameraparameters.h +++ b/g2o/types/sba/parameter_cameraparameters.h @@ -28,29 +28,76 @@ #define G2O_SBA_CAMERAPARAMETERS_H #include -#include #include "g2o/core/eigen_types.h" #include "g2o/core/parameter.h" +#include "g2o/core/type_traits.h" #include "g2o_types_sba_api.h" namespace g2o { -class G2O_TYPES_SBA_API CameraParameters : public g2o::Parameter { +struct StereoCameraParameters { + double focal_length = 1.; + Vector2 principle_point; + double baseline = 0.5; +}; + +template <> +struct TypeTraits { + enum { + kVectorDimension = 4, + kMinimalVectorDimension = 4, + kIsVector = 0, + kIsScalar = 0, + }; + using Type = StereoCameraParameters; + using VectorType = VectorN; + using MinimalVectorType = VectorN; + + static VectorType toVector(const Type& t) { + VectorType result; + result << t.focal_length, t.principle_point(0), t.principle_point(1), + t.baseline; + return result; + } + static void toData(const Type& t, double* data) { + typename VectorType::MapType v(data, kVectorDimension); + v = toVector(t); + } + + static MinimalVectorType toMinimalVector(const Type& t) { + return toVector(t); + } + static void toMinimalData(const Type& t, double* data) { toData(t, data); } + + template + static Type fromVector(const Eigen::DenseBase& v) { + Type result; + result.focal_length = v(0); + result.principle_point(0) = v(1); + result.principle_point(1) = v(2); + result.baseline = v(3); + return result; + } + + template + static Type fromMinimalVector(const Eigen::DenseBase& v) { + return fromVector(v); + } +}; + +class G2O_TYPES_SBA_API CameraParameters + : public g2o::BaseParameter { public: - CameraParameters(); + CameraParameters() = default; CameraParameters(double focalLength, const Eigen::Ref& principlePoint, double baseLine); [[nodiscard]] Vector2 cam_map(const Vector3& trans_xyz) const; [[nodiscard]] Vector3 stereocam_uvu_map(const Vector3& trans_xyz) const; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - double focal_length = 1.; - Vector2 principle_point; - double baseline = 0.5; + void update() override; }; } // namespace g2o diff --git a/g2o/types/sba/sba_utils.h b/g2o/types/sba/sba_utils.h index 9feade6ab..d5297b8a6 100644 --- a/g2o/types/sba/sba_utils.h +++ b/g2o/types/sba/sba_utils.h @@ -28,11 +28,9 @@ #define G2O_SBA_UTILS_H #include "g2o/types/slam3d/se3_ops.h" -#include "g2o_types_sba_api.h" +#include "g2o/types/slam3d/se3quat.h" -namespace g2o { - -namespace internal { +namespace g2o::internal { inline Vector3 invert_depth(const Vector3& x) { Vector2 aux = x.head<2>(); @@ -69,7 +67,6 @@ inline Matrix3 d_Tinvpsi_d_psi(const SE3Quat& T, const Vector3& psi) { return J; } -} // namespace internal -} // namespace g2o +} // namespace g2o::internal #endif diff --git a/g2o/types/sba/sbacam.cpp b/g2o/types/sba/sbacam.cpp index a213b5cb2..9514b902d 100644 --- a/g2o/types/sba/sbacam.cpp +++ b/g2o/types/sba/sbacam.cpp @@ -127,11 +127,11 @@ void SBACam::setDr() { // human-readable SBACam object std::ostream& operator<<(std::ostream& out_str, const SBACam& cam) { - out_str << cam.translation().transpose() << std::endl; - out_str << cam.rotation().coeffs().transpose() << std::endl << std::endl; - out_str << cam.Kcam << std::endl << std::endl; - out_str << cam.w2n << std::endl << std::endl; - out_str << cam.w2i << std::endl << std::endl; + out_str << cam.translation().transpose() << '\n'; + out_str << cam.rotation().coeffs().transpose() << '\n' << '\n'; + out_str << cam.Kcam << '\n' << '\n'; + out_str << cam.w2n << '\n' << '\n'; + out_str << cam.w2i << '\n' << '\n'; return out_str; } diff --git a/g2o/types/sba/sbacam.h b/g2o/types/sba/sbacam.h index 8cf6e456a..64625fdb7 100644 --- a/g2o/types/sba/sbacam.h +++ b/g2o/types/sba/sbacam.h @@ -142,8 +142,6 @@ struct TypeTraits { res.fromMinimalVector(v); return res; } - - static Type Identity() { return SBACam(); } }; } // namespace g2o diff --git a/g2o/types/sba/types_six_dof_expmap.h b/g2o/types/sba/types_six_dof_expmap.h index 8b40e39a8..41875e435 100644 --- a/g2o/types/sba/types_six_dof_expmap.h +++ b/g2o/types/sba/types_six_dof_expmap.h @@ -27,14 +27,6 @@ #ifndef G2O_SIX_DOF_TYPES_EXPMAP #define G2O_SIX_DOF_TYPES_EXPMAP -// #include "g2o/core/base_vertex.h" -// #include "g2o/core/base_binary_edge.h" -// #include "g2o/core/base_unary_edge.h" -// #include "g2o/core/base_variable_sized_edge.h" -// #include "g2o/types/slam3d/se3_ops.h" -// #include "types_sba.h" -// #include - #include "edge_project_psi2uv.h" #include "edge_project_stereo_xyz.h" #include "edge_project_stereo_xyz_onlypose.h" diff --git a/g2o/types/sba/vertex_cam.cpp b/g2o/types/sba/vertex_cam.cpp index 5ac945b63..dcb1b83c3 100644 --- a/g2o/types/sba/vertex_cam.cpp +++ b/g2o/types/sba/vertex_cam.cpp @@ -27,62 +27,72 @@ #include "vertex_cam.h" #include -#include -#include "g2o/core/io_helper.h" -#include "g2o/stuff/logger.h" -#include "g2o/stuff/misc.h" #include "g2o/types/sba/sbacam.h" namespace g2o { -bool VertexCam::read(std::istream& is) { - // first the position and orientation (vector3 and quaternion) - Vector3 t; - internal::readVector(is, t); - Quaternion r; - internal::readVector(is, r.coeffs()); - r.normalize(); // recover nummeric precision +void VertexCam::setEstimate(const SBACam& cam) { + BaseVertex<6, SBACam>::setEstimate(cam); + estimate_.setTransform(); + estimate_.setProjection(); + estimate_.setDr(); +} - // form the camera object - SBACam cam(r, t); +void VertexCam::oplusImpl(const VectorX::MapType& update) { + estimate_.update(update.head()); + estimate_.setTransform(); + estimate_.setProjection(); + estimate_.setDr(); +} - // now fx, fy, cx, cy, baseline - double fx; +// bool VertexCam::read(std::istream& is) { +// // first the position and orientation (vector3 and quaternion) +// Vector3 t; +// internal::readVector(is, t); +// Quaternion r; +// internal::readVector(is, r.coeffs()); +// r.normalize(); // recover nummeric precision - // try to read one value - is >> fx; - if (is.good()) { - double fy; - double cx; - double cy; - double tx; - is >> fy >> cx >> cy >> tx; - cam.setKcam(fx, fy, cx, cy, tx); - } else { - is.clear(); - G2O_WARN("cam not defined, using defaults"); - cam.setKcam(300, 300, 320, 320, cst(0.1)); - } +// // form the camera object +// SBACam cam(r, t); - setEstimate(cam); - return true; -} +// // now fx, fy, cx, cy, baseline +// double fx; -bool VertexCam::write(std::ostream& os) const { - const SBACam& cam = estimate(); +// // try to read one value +// is >> fx; +// if (is.good()) { +// double fy; +// double cx; +// double cy; +// double tx; +// is >> fy >> cx >> cy >> tx; +// cam.setKcam(fx, fy, cx, cy, tx); +// } else { +// is.clear(); +// G2O_WARN("cam not defined, using defaults"); +// cam.setKcam(300, 300, 320, 320, cst(0.1)); +// } - // first the position and orientation (vector3 and quaternion) - internal::writeVector(os, cam.translation()); - internal::writeVector(os, cam.rotation().coeffs()); +// setEstimate(cam); +// return true; +// } - // now fx, fy, cx, cy, baseline - os << cam.Kcam(0, 0) << " "; - os << cam.Kcam(1, 1) << " "; - os << cam.Kcam(0, 2) << " "; - os << cam.Kcam(1, 2) << " "; - os << cam.baseline << " "; - return os.good(); -} +// bool VertexCam::write(std::ostream& os) const { +// const SBACam& cam = estimate(); + +// // first the position and orientation (vector3 and quaternion) +// internal::writeVector(os, cam.translation()); +// internal::writeVector(os, cam.rotation().coeffs()); + +// // now fx, fy, cx, cy, baseline +// os << cam.Kcam(0, 0) << " "; +// os << cam.Kcam(1, 1) << " "; +// os << cam.Kcam(0, 2) << " "; +// os << cam.Kcam(1, 2) << " "; +// os << cam.baseline << " "; +// return os.good(); +// } } // namespace g2o diff --git a/g2o/types/sba/vertex_cam.h b/g2o/types/sba/vertex_cam.h index e0b6c0836..ade369394 100644 --- a/g2o/types/sba/vertex_cam.h +++ b/g2o/types/sba/vertex_cam.h @@ -27,8 +27,6 @@ #ifndef G2O_SBA_VERTEX_CAM_H #define G2O_SBA_VERTEX_CAM_H -#include - #include "g2o/core/base_vertex.h" #include "g2o/core/eigen_types.h" #include "g2o_types_sba_api.h" @@ -45,24 +43,8 @@ namespace g2o { */ class G2O_TYPES_SBA_API VertexCam : public BaseVertex<6, SBACam> { public: - //! reimplement reading VertexCam to support custom format - bool read(std::istream& is) override; - //! reimplement writing VertexCam to support custom format - bool write(std::ostream& os) const override; - - virtual void setEstimate(const SBACam& cam) { - BaseVertex<6, SBACam>::setEstimate(cam); - estimate_.setTransform(); - estimate_.setProjection(); - estimate_.setDr(); - } - - void oplusImpl(const VectorX::MapType& update) override { - estimate_.update(update.head()); - estimate_.setTransform(); - estimate_.setProjection(); - estimate_.setDr(); - } + virtual void setEstimate(const SBACam& cam); + void oplusImpl(const VectorX::MapType& update) override; }; } // namespace g2o diff --git a/g2o/types/sba/vertex_intrinsics.cpp b/g2o/types/sba/vertex_intrinsics.cpp index 4e8a9431f..bdce4f139 100644 --- a/g2o/types/sba/vertex_intrinsics.cpp +++ b/g2o/types/sba/vertex_intrinsics.cpp @@ -26,7 +26,6 @@ #include "vertex_intrinsics.h" -#include "g2o/core/io_helper.h" #include "g2o/stuff/misc.h" namespace g2o { @@ -35,14 +34,6 @@ VertexIntrinsics::VertexIntrinsics() { estimate_.values << cst(1.), cst(1.), cst(.5), cst(.5), cst(.1); } -bool VertexIntrinsics::read(std::istream& is) { - return internal::readVector(is, estimate_.values); -} - -bool VertexIntrinsics::write(std::ostream& os) const { - return internal::writeVector(os, estimate().values); -} - void VertexIntrinsics::oplusImpl(const VectorX::MapType& update) { estimate_.values.head<4>() += update.head(); } diff --git a/g2o/types/sba/vertex_intrinsics.h b/g2o/types/sba/vertex_intrinsics.h index f912f9137..fa0b61bda 100644 --- a/g2o/types/sba/vertex_intrinsics.h +++ b/g2o/types/sba/vertex_intrinsics.h @@ -28,7 +28,6 @@ #define G2O_SBA_VERTEX_INTRINSICS_H #include -#include #include "g2o/core/base_vertex.h" #include "g2o/core/eigen_types.h" @@ -49,8 +48,6 @@ class G2O_TYPES_SBA_API VertexIntrinsics : public BaseVertex<4, VertexIntrinsicsEstimate> { public: VertexIntrinsics(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void oplusImpl(const VectorX::MapType& update) override; }; @@ -95,12 +92,6 @@ struct TypeTraits { res.values(4) = 0.1; return res; } - - static Type Identity() { - Type res; - res.values << 1., 1., .5, .5, .1; - return res; - } }; } // namespace g2o diff --git a/g2o/types/sba/vertex_se3_expmap.cpp b/g2o/types/sba/vertex_se3_expmap.cpp index b591ad653..7ee9a644b 100644 --- a/g2o/types/sba/vertex_se3_expmap.cpp +++ b/g2o/types/sba/vertex_se3_expmap.cpp @@ -26,21 +26,8 @@ #include "vertex_se3_expmap.h" -#include "g2o/core/io_helper.h" - namespace g2o { -bool VertexSE3Expmap::read(std::istream& is) { - Vector7 est; - internal::readVector(is, est); - setEstimate(SE3Quat(est).inverse()); - return true; -} - -bool VertexSE3Expmap::write(std::ostream& os) const { - return internal::writeVector(os, estimate().inverse().toVector()); -} - void VertexSE3Expmap::oplusImpl(const VectorX::MapType& update) { setEstimate(SE3Quat::exp(update.head()) * estimate()); } diff --git a/g2o/types/sba/vertex_se3_expmap.h b/g2o/types/sba/vertex_se3_expmap.h index efa0195c1..e87513e06 100644 --- a/g2o/types/sba/vertex_se3_expmap.h +++ b/g2o/types/sba/vertex_se3_expmap.h @@ -27,8 +27,6 @@ #ifndef G2O_SBA_VERTEXSE3EXPMAP_H #define G2O_SBA_VERTEXSE3EXPMAP_H -#include - #include "g2o/core/base_vertex.h" #include "g2o/core/eigen_types.h" #include "g2o/types/slam3d/se3quat.h" @@ -42,10 +40,6 @@ namespace g2o { */ class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat> { public: - //! special implementation of reading - bool read(std::istream& is) override; - //! special implementation of writing - bool write(std::ostream& os) const override; void oplusImpl(const VectorX::MapType& update) override; }; diff --git a/g2o/types/sclam2d/CMakeLists.txt b/g2o/types/sclam2d/CMakeLists.txt index 215665fb9..a818e5a24 100644 --- a/g2o/types/sclam2d/CMakeLists.txt +++ b/g2o/types/sclam2d/CMakeLists.txt @@ -3,7 +3,7 @@ add_library(types_sclam2d ${G2O_LIB_TYPE} vertex_odom_differential_params.cpp vertex_odom_differential_params.h edge_se2_odom_differential_calib.cpp edge_se2_odom_differential_calib.h odometry_measurement.cpp odometry_measurement.h - types_sclam2d.cpp types_sclam2d.h + types_sclam2d.cpp g2o_types_sclam2d_api.h ) diff --git a/g2o/types/sclam2d/edge_se2_odom_differential_calib.cpp b/g2o/types/sclam2d/edge_se2_odom_differential_calib.cpp index ca02ce9b9..556947c1c 100644 --- a/g2o/types/sclam2d/edge_se2_odom_differential_calib.cpp +++ b/g2o/types/sclam2d/edge_se2_odom_differential_calib.cpp @@ -26,9 +26,7 @@ #include "edge_se2_odom_differential_calib.h" -#include #include -#include #include "g2o/types/sclam2d/odometry_measurement.h" @@ -38,20 +36,24 @@ namespace g2o { -bool EdgeSE2OdomDifferentialCalib::read(std::istream& is) { - double vl; - double vr; - double dt; - is >> vl >> vr >> dt; - VelocityMeasurement vm(vl, vr, dt); - setMeasurement(vm); - return readInformationMatrix(is); -} +void EdgeSE2OdomDifferentialCalib::computeError() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSE2* v2 = vertexXnRaw<1>(); + const VertexOdomDifferentialParams* params = vertexXnRaw<2>(); + const SE2& x1 = v1->estimate(); + const SE2& x2 = v2->estimate(); + + // get the calibrated motion given by the odometry + VelocityMeasurement calibratedVelocityMeasurement( + measurement().vl() * params->estimate()(0), + measurement().vr() * params->estimate()(1), measurement().dt()); + MotionMeasurement mm = OdomConvert::convertToMotion( + calibratedVelocityMeasurement, params->estimate()(2)); + SE2 Ku_ij; + Ku_ij.fromVector(mm.measurement()); -bool EdgeSE2OdomDifferentialCalib::write(std::ostream& os) const { - os << measurement().vl() << " " << measurement().vr() << " " - << measurement().dt() << " "; - return writeInformationMatrix(os); + SE2 delta = Ku_ij.inverse() * x1.inverse() * x2; + error_ = delta.toVector(); } #ifdef G2O_HAVE_OPENGL diff --git a/g2o/types/sclam2d/edge_se2_odom_differential_calib.h b/g2o/types/sclam2d/edge_se2_odom_differential_calib.h index 0a853a5ca..bc29628ff 100644 --- a/g2o/types/sclam2d/edge_se2_odom_differential_calib.h +++ b/g2o/types/sclam2d/edge_se2_odom_differential_calib.h @@ -27,15 +27,12 @@ #ifndef G2O_EDGE_SE2_ODOM_CALIB_DIFFERENTIAL_H #define G2O_EDGE_SE2_ODOM_CALIB_DIFFERENTIAL_H -#include -#include #include #include "g2o/config.h" #include "g2o/core/base_fixed_sized_edge.h" #include "g2o/core/hyper_graph.h" #include "g2o/core/hyper_graph_action.h" -#include "g2o/types/slam2d/se2.h" #include "g2o/types/slam2d/vertex_se2.h" #include "g2o_types_sclam2d_api.h" #include "odometry_measurement.h" @@ -47,28 +44,7 @@ class G2O_TYPES_SCLAM2D_API EdgeSE2OdomDifferentialCalib : public BaseFixedSizedEdge<3, VelocityMeasurement, VertexSE2, VertexSE2, VertexOdomDifferentialParams> { public: - void computeError() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSE2* v2 = vertexXnRaw<1>(); - const VertexOdomDifferentialParams* params = vertexXnRaw<2>(); - const SE2& x1 = v1->estimate(); - const SE2& x2 = v2->estimate(); - - // get the calibrated motion given by the odometry - VelocityMeasurement calibratedVelocityMeasurement( - measurement().vl() * params->estimate()(0), - measurement().vr() * params->estimate()(1), measurement().dt()); - MotionMeasurement mm = OdomConvert::convertToMotion( - calibratedVelocityMeasurement, params->estimate()(2)); - SE2 Ku_ij; - Ku_ij.fromVector(mm.measurement()); - - SE2 delta = Ku_ij.inverse() * x1.inverse() * x2; - error_ = delta.toVector(); - } - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + void computeError() override; }; #ifdef G2O_HAVE_OPENGL diff --git a/g2o/types/sclam2d/edge_se2_sensor_calib.cpp b/g2o/types/sclam2d/edge_se2_sensor_calib.cpp index 40d0c339b..a00de7b2c 100644 --- a/g2o/types/sclam2d/edge_se2_sensor_calib.cpp +++ b/g2o/types/sclam2d/edge_se2_sensor_calib.cpp @@ -30,12 +30,37 @@ #include #include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" #ifdef G2O_HAVE_OPENGL #include "g2o/stuff/opengl_wrapper.h" #endif namespace g2o { +void EdgeSE2SensorCalib::computeError() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSE2* v2 = vertexXnRaw<1>(); + const VertexSE2* laserOffset = vertexXnRaw<2>(); + const SE2& x1 = v1->estimate(); + const SE2& x2 = v2->estimate(); + SE2 delta = inverseMeasurement_ * ((x1 * laserOffset->estimate()).inverse() * + x2 * laserOffset->estimate()); + error_ = delta.toVector(); +} + +void EdgeSE2SensorCalib::setMeasurement(const SE2& m) { + measurement_ = m; + inverseMeasurement_ = m.inverse(); +} + +double EdgeSE2SensorCalib::initialEstimatePossible( + const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { + if (from.count(vertices_[2]) == 1 // need the laser offset + && ((from.count(vertices_[0]) == 1 && to == vertices_[1].get()) || + ((from.count(vertices_[1]) == 1 && to == vertices_[0].get())))) { + return 1.0; + } + return -1.0; +} + void EdgeSE2SensorCalib::initialEstimate( const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void)to; @@ -52,19 +77,6 @@ void EdgeSE2SensorCalib::initialEstimate( } } -bool EdgeSE2SensorCalib::read(std::istream& is) { - Vector3 p; - internal::readVector(is, p); - measurement_.fromVector(p); - inverseMeasurement_ = measurement().inverse(); - return readInformationMatrix(is); -} - -bool EdgeSE2SensorCalib::write(std::ostream& os) const { - internal::writeVector(os, measurement().toVector()); - return writeInformationMatrix(os); -} - #ifdef G2O_HAVE_OPENGL EdgeSE2SensorCalibDrawAction::EdgeSE2SensorCalibDrawAction() : DrawAction(typeid(EdgeSE2SensorCalib).name()) {} diff --git a/g2o/types/sclam2d/edge_se2_sensor_calib.h b/g2o/types/sclam2d/edge_se2_sensor_calib.h index e084f27fe..4e44f35c6 100644 --- a/g2o/types/sclam2d/edge_se2_sensor_calib.h +++ b/g2o/types/sclam2d/edge_se2_sensor_calib.h @@ -27,7 +27,6 @@ #ifndef G2O_EDGE_SE2_SENSOR_CALIB_H #define G2O_EDGE_SE2_SENSOR_CALIB_H -#include #include #include "g2o/config.h" @@ -47,38 +46,15 @@ namespace g2o { class G2O_TYPES_SCLAM2D_API EdgeSE2SensorCalib : public BaseFixedSizedEdge<3, SE2, VertexSE2, VertexSE2, VertexSE2> { public: - void computeError() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSE2* v2 = vertexXnRaw<1>(); - const VertexSE2* laserOffset = vertexXnRaw<2>(); - const SE2& x1 = v1->estimate(); - const SE2& x2 = v2->estimate(); - SE2 delta = - inverseMeasurement_ * ((x1 * laserOffset->estimate()).inverse() * x2 * - laserOffset->estimate()); - error_ = delta.toVector(); - } + void computeError() override; - void setMeasurement(const SE2& m) override { - measurement_ = m; - inverseMeasurement_ = m.inverse(); - } + void setMeasurement(const SE2& m) override; double initialEstimatePossible(const OptimizableGraph::VertexSet& from, - OptimizableGraph::Vertex* to) override { - if (from.count(vertices_[2]) == 1 // need the laser offset - && ((from.count(vertices_[0]) == 1 && to == vertices_[1].get()) || - ((from.count(vertices_[1]) == 1 && to == vertices_[0].get())))) { - return 1.0; - } - return -1.0; - } + OptimizableGraph::Vertex* to) override; void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) override; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - protected: SE2 inverseMeasurement_; }; diff --git a/g2o/types/sclam2d/vertex_odom_differential_params.cpp b/g2o/types/sclam2d/vertex_odom_differential_params.cpp index 7be1f539c..87865c568 100644 --- a/g2o/types/sclam2d/vertex_odom_differential_params.cpp +++ b/g2o/types/sclam2d/vertex_odom_differential_params.cpp @@ -26,16 +26,10 @@ #include "vertex_odom_differential_params.h" -#include "g2o/core/io_helper.h" - namespace g2o { -bool VertexOdomDifferentialParams::read(std::istream& is) { - return internal::readVector(is, estimate_); -} - -bool VertexOdomDifferentialParams::write(std::ostream& os) const { - return internal::writeVector(os, estimate()); +void VertexOdomDifferentialParams::oplusImpl(const VectorX::MapType& update) { + estimate_ += update.head<3>(); } } // namespace g2o diff --git a/g2o/types/sclam2d/vertex_odom_differential_params.h b/g2o/types/sclam2d/vertex_odom_differential_params.h index b030d60da..4f36f6042 100644 --- a/g2o/types/sclam2d/vertex_odom_differential_params.h +++ b/g2o/types/sclam2d/vertex_odom_differential_params.h @@ -27,9 +27,6 @@ #ifndef G2O_VERTEX_ODOM_DIFFERENTIAL_PARAMS_H #define G2O_VERTEX_ODOM_DIFFERENTIAL_PARAMS_H -#include -#include - #include "g2o/core/base_vertex.h" #include "g2o/core/eigen_types.h" #include "g2o_types_sclam2d_api.h" @@ -39,12 +36,7 @@ namespace g2o { class G2O_TYPES_SCLAM2D_API VertexOdomDifferentialParams : public BaseVertex<3, Vector3> { public: - void oplusImpl(const VectorX::MapType& update) override { - estimate_ += update.head<3>(); - } - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + void oplusImpl(const VectorX::MapType& update) override; }; } // namespace g2o diff --git a/g2o/types/sim3/sim3.cpp b/g2o/types/sim3/sim3.cpp index 41d7066ff..32f083c52 100644 --- a/g2o/types/sim3/sim3.cpp +++ b/g2o/types/sim3/sim3.cpp @@ -239,10 +239,10 @@ void Sim3::normalizeRotation() { r_.normalize(); } -inline std::ostream& operator<<(std::ostream& out_str, const Sim3& sim3) { - out_str << sim3.rotation().coeffs() << std::endl; - out_str << sim3.translation() << std::endl; - out_str << sim3.scale() << std::endl; +std::ostream& operator<<(std::ostream& out_str, const Sim3& sim3) { + out_str << sim3.rotation().coeffs() << '\n'; + out_str << sim3.translation() << '\n'; + out_str << sim3.scale() << '\n'; return out_str; } diff --git a/g2o/types/sim3/sim3.h b/g2o/types/sim3/sim3.h index 8f3a162c6..c58824317 100644 --- a/g2o/types/sim3/sim3.h +++ b/g2o/types/sim3/sim3.h @@ -130,8 +130,6 @@ struct TypeTraits { static Type fromMinimalVector(const Eigen::DenseBase& v) { return Sim3(v); } - - static Type Identity() { return Sim3(); } }; } // namespace g2o diff --git a/g2o/types/sim3/types_seven_dof_expmap.cpp b/g2o/types/sim3/types_seven_dof_expmap.cpp index 1ecb91a5e..6bb5fa15a 100644 --- a/g2o/types/sim3/types_seven_dof_expmap.cpp +++ b/g2o/types/sim3/types_seven_dof_expmap.cpp @@ -27,10 +27,8 @@ #include "types_seven_dof_expmap.h" #include -#include #include "g2o/core/factory.h" -#include "g2o/core/io_helper.h" #include "g2o/types/sim3/sim3.h" #include "g2o/types/slam3d/se3_ops.h" @@ -71,25 +69,6 @@ void VertexSim3Expmap::oplusImpl(const VectorX::MapType& update) { setEstimate(s * estimate()); } -bool VertexSim3Expmap::read(std::istream& is) { - Vector7 cam2world; - bool state = true; - state &= internal::readVector(is, cam2world); - state &= internal::readVector(is, _focal_length1); - state &= internal::readVector(is, _principle_point1); - setEstimate(Sim3(cam2world).inverse()); - return state; -} - -bool VertexSim3Expmap::write(std::ostream& os) const { - Sim3 cam2world(estimate().inverse()); - Vector7 lv = cam2world.log(); - internal::writeVector(os, lv); - internal::writeVector(os, _focal_length1); - internal::writeVector(os, _principle_point1); - return os.good(); -} - Vector2 VertexSim3Expmap::cam_map1(const Vector2& v) const { Vector2 res; res[0] = v[0] * _focal_length1[0] + _principle_point1[0]; @@ -104,20 +83,6 @@ Vector2 VertexSim3Expmap::cam_map2(const Vector2& v) const { return res; } -bool EdgeSim3::read(std::istream& is) { - Vector7 v7; - internal::readVector(is, v7); - Sim3 cam2world(v7); - setMeasurement(cam2world.inverse()); - return readInformationMatrix(is); -} - -bool EdgeSim3::write(std::ostream& os) const { - Sim3 cam2world(measurement().inverse()); - internal::writeVector(os, cam2world.log()); - return writeInformationMatrix(os); -} - void EdgeSim3::computeError() { const VertexSim3Expmap* v1 = vertexXnRaw<0>(); const VertexSim3Expmap* v2 = vertexXnRaw<1>(); @@ -193,16 +158,6 @@ void EdgeSim3::linearizeOplus() { /**Sim3ProjectXYZ*/ -bool EdgeSim3ProjectXYZ::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeSim3ProjectXYZ::write(std::ostream& os) const { - internal::writeVector(os, measurement_); - return writeInformationMatrix(os); -} - void EdgeSim3ProjectXYZ::computeError() { const VertexSim3Expmap* v1 = vertexXnRaw<1>(); const VertexPointXYZ* v2 = vertexXnRaw<0>(); @@ -211,16 +166,6 @@ void EdgeSim3ProjectXYZ::computeError() { error_ = obs - v1->cam_map1(project(v1->estimate().map(v2->estimate()))); } -bool EdgeInverseSim3ProjectXYZ::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const { - internal::writeVector(os, measurement_); - return writeInformationMatrix(os); -} - void EdgeInverseSim3ProjectXYZ::computeError() { const VertexSim3Expmap* v1 = vertexXnRaw<1>(); const VertexPointXYZ* v2 = vertexXnRaw<0>(); diff --git a/g2o/types/sim3/types_seven_dof_expmap.h b/g2o/types/sim3/types_seven_dof_expmap.h index 40682909f..f9708fbe4 100644 --- a/g2o/types/sim3/types_seven_dof_expmap.h +++ b/g2o/types/sim3/types_seven_dof_expmap.h @@ -26,16 +26,11 @@ #ifndef G2O_SEVEN_DOF_EXPMAP_TYPES #define G2O_SEVEN_DOF_EXPMAP_TYPES -#include -#include -#include "g2o/config.h" #include "g2o/core/base_binary_edge.h" #include "g2o/core/base_vertex.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" -#include "g2o/types/sba/types_six_dof_expmap.h" -#include "g2o/types/sim3/types_seven_dof_expmap.h" #include "g2o/types/slam3d/vertex_pointxyz.h" #include "sim3.h" #include "types_seven_dof_expmap_api.h" @@ -60,10 +55,6 @@ template class BaseVertex<7, Sim3>; class G2O_TYPES_SIM3_API VertexSim3Expmap : public BaseVertex<7, Sim3> { public: VertexSim3Expmap(); - //! custom read function - bool read(std::istream& is) override; - //! custom write function - bool write(std::ostream& os) const override; void oplusImpl(const VectorX::MapType& update) override; @@ -85,8 +76,6 @@ class G2O_TYPES_SIM3_API VertexSim3Expmap : public BaseVertex<7, Sim3> { class G2O_TYPES_SIM3_API EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; double initialEstimatePossible(const OptimizableGraph::VertexSet&, @@ -102,9 +91,6 @@ class G2O_TYPES_SIM3_API EdgeSim3 class G2O_TYPES_SIM3_API EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSim3Expmap> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void computeError() override; // virtual void linearizeOplus(); @@ -114,9 +100,6 @@ class G2O_TYPES_SIM3_API EdgeSim3ProjectXYZ class G2O_TYPES_SIM3_API EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSim3Expmap> { public: - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void computeError() override; // virtual void linearizeOplus(); diff --git a/g2o/types/slam2d/edge_pointxy.cpp b/g2o/types/slam2d/edge_pointxy.cpp index e4256fc23..87ce2b0d2 100644 --- a/g2o/types/slam2d/edge_pointxy.cpp +++ b/g2o/types/slam2d/edge_pointxy.cpp @@ -26,9 +26,6 @@ #include "edge_pointxy.h" -#include "g2o/config.h" -#include "g2o/core/io_helper.h" - namespace g2o { EdgePointXY::EdgePointXY() @@ -38,19 +35,6 @@ EdgePointXY::EdgePointXY() error_.setZero(); } -bool EdgePointXY::read(std::istream& is) { - Vector2 p; - internal::readVector(is, p); - setMeasurement(p); - readInformationMatrix(is); - return true; -} - -bool EdgePointXY::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES void EdgePointXY::linearizeOplus() { jacobianOplusXi_ = -Matrix2::Identity(); diff --git a/g2o/types/slam2d/edge_pointxy.h b/g2o/types/slam2d/edge_pointxy.h index 3813517c3..5df5c1c23 100644 --- a/g2o/types/slam2d/edge_pointxy.h +++ b/g2o/types/slam2d/edge_pointxy.h @@ -27,11 +27,6 @@ #ifndef G2O_EDGE_POINTXY_H #define G2O_EDGE_POINTXY_H -#include -#include -#include - -#include "g2o/config.h" #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" @@ -50,8 +45,6 @@ class G2O_TYPES_SLAM2D_API EdgePointXY const VertexPointXY* v2 = vertexXnRaw<1>(); error_ = (v2->estimate() - v1->estimate()) - measurement_; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void setMeasurement(const Vector2& m) override { measurement_ = m; } diff --git a/g2o/types/slam2d/edge_se2.cpp b/g2o/types/slam2d/edge_se2.cpp index 55610ea19..e0747a23c 100644 --- a/g2o/types/slam2d/edge_se2.cpp +++ b/g2o/types/slam2d/edge_se2.cpp @@ -26,13 +26,10 @@ #include "edge_se2.h" -#include #include #include -#include #include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" #include "g2o/stuff/macros.h" #include "g2o/stuff/misc.h" #include "g2o/types/slam2d/se2.h" @@ -45,19 +42,6 @@ namespace g2o { -bool EdgeSE2::read(std::istream& is) { - Vector3 p; - internal::readVector(is, p); - setMeasurement(SE2(p)); - readInformationMatrix(is); - return is.good() || is.eof(); -} - -bool EdgeSE2::write(std::ostream& os) const { - internal::writeVector(os, measurement().toVector()); - return writeInformationMatrix(os); -} - void EdgeSE2::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /* to */) { auto fromEdge = vertexXn<0>(); diff --git a/g2o/types/slam2d/edge_se2.h b/g2o/types/slam2d/edge_se2.h index 5c6671f19..0529dbf4e 100644 --- a/g2o/types/slam2d/edge_se2.h +++ b/g2o/types/slam2d/edge_se2.h @@ -27,8 +27,6 @@ #ifndef G2O_EDGE_SE2_H #define G2O_EDGE_SE2_H -#include -#include #include #include "g2o/config.h" @@ -58,8 +56,6 @@ class G2O_TYPES_SLAM2D_API EdgeSE2 inverseMeasurement_ * (v1->estimate().inverse() * v2->estimate()); error_ = delta.toVector(); } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void setMeasurement(const SE2& m) override { measurement_ = m; diff --git a/g2o/types/slam2d/edge_se2_lotsofxy.cpp b/g2o/types/slam2d/edge_se2_lotsofxy.cpp index ab30761d2..e4233c212 100644 --- a/g2o/types/slam2d/edge_se2_lotsofxy.cpp +++ b/g2o/types/slam2d/edge_se2_lotsofxy.cpp @@ -28,9 +28,6 @@ #include #include -#include -#include -#include #include #include "se2.h" @@ -44,62 +41,17 @@ EdgeSE2LotsOfXY::EdgeSE2LotsOfXY() { resize(0); } void EdgeSE2LotsOfXY::computeError() { auto* pose = static_cast(vertexRaw(0)); - for (unsigned int i = 0; i < observedPoints_; i++) { + int observed_points = measurement_.size() / 2; + for (int i = 0; i < observed_points; i++) { auto* xy = static_cast(vertexRaw(1 + i)); Vector2 m = pose->estimate().inverse() * xy->estimate(); - unsigned int index = 2 * i; + const int index = 2 * i; error_[index] = m[0] - measurement_[index]; error_[index + 1] = m[1] - measurement_[index + 1]; } } -bool EdgeSE2LotsOfXY::read(std::istream& is) { - is >> observedPoints_; - setSize(observedPoints_ + 1); - - // read the measurements - for (unsigned int i = 0; i < observedPoints_; i++) { - unsigned int index = 2 * i; - is >> measurement_[index] >> measurement_[index + 1]; - } - - // read the information matrix - for (unsigned int i = 0; i < observedPoints_ * 2; i++) { - // fill the "upper triangle" part of the matrix - for (unsigned int j = i; j < observedPoints_ * 2; j++) { - is >> information()(i, j); - } - - // fill the lower triangle part - for (unsigned int j = 0; j < i; j++) { - information()(i, j) = information()(j, i); - } - } - - return true; -} - -bool EdgeSE2LotsOfXY::write(std::ostream& os) const { - // write number of observed points - os << "|| " << observedPoints_; - - // write measurements - for (unsigned int i = 0; i < observedPoints_; i++) { - unsigned int index = 2 * i; - os << " " << measurement_[index] << " " << measurement_[index + 1]; - } - - // write information matrix - for (unsigned int i = 0; i < observedPoints_ * 2; i++) { - for (unsigned int j = i; j < observedPoints_ * 2; j++) { - os << " " << information()(i, j); - } - } - - return os.good(); -} - void EdgeSE2LotsOfXY::linearizeOplus() { const auto* vi = static_cast(vertexRaw(0)); const double& x1 = vi->estimate().translation()[0]; @@ -151,11 +103,12 @@ void EdgeSE2LotsOfXY::initialEstimate(const OptimizableGraph::VertexSet& fixed, auto* pose = static_cast(vertexRaw(0)); + int observed_points = measurement_.size() / 2; #ifdef _MSC_VER - std::vector estimate_this(observedPoints_, true); + std::vector estimate_this(observed_points, true); #else - bool estimate_this[observedPoints_]; - for (unsigned int i = 0; i < observedPoints_; i++) { + bool estimate_this[observed_points]; + for (int i = 0; i < observed_points; i++) { estimate_this[i] = true; } #endif @@ -194,7 +147,8 @@ double EdgeSE2LotsOfXY::initialEstimatePossible( bool EdgeSE2LotsOfXY::setMeasurementFromState() { auto* pose = static_cast(vertexRaw(0)); - for (unsigned int i = 0; i < observedPoints_; i++) { + int observed_points = measurement_.size() / 2; + for (int i = 0; i < observed_points; i++) { auto* xy = static_cast(vertexRaw(1 + i)); Vector2 m = pose->estimate().inverse() * xy->estimate(); diff --git a/g2o/types/slam2d/edge_se2_lotsofxy.h b/g2o/types/slam2d/edge_se2_lotsofxy.h index ca68b7a56..4e7946fa1 100644 --- a/g2o/types/slam2d/edge_se2_lotsofxy.h +++ b/g2o/types/slam2d/edge_se2_lotsofxy.h @@ -28,38 +28,36 @@ #define G2O_EDGE_SE2_LOTSOF_XY #include -#include -#include "g2o/config.h" #include "g2o/core/base_variable_sized_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" #include "g2o_types_slam2d_api.h" -#include "vertex_point_xy.h" -#include "vertex_se2.h" namespace g2o { +/** + * @brief A pose observing multiple points. + * + * vertex(0) = VertexSE2 + * vertex(1) = VertexPointXY + * ... + * vertex(N) = VertexPointXY + */ class G2O_TYPES_SLAM2D_API EdgeSE2LotsOfXY : public BaseVariableSizedEdge<-1, VectorX> { - protected: - unsigned int observedPoints_ = 0; - public: EdgeSE2LotsOfXY(); - void setSize(int vertices) { - resize(vertices); - observedPoints_ = vertices - 1; - measurement_.resize(observedPoints_ * 2L, 1); - setDimension(observedPoints_ * 2); + void resize(size_t size) override { + BaseVariableSizedEdge<-1, VectorX>::resize(size); + int observed_points = size > 0 ? size - 1 : 0; + measurement_.resize(observed_points * 2L, 1); + setDimension(observed_points * 2); } void computeError() override; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - bool setMeasurementFromState() override; void initialEstimate(const OptimizableGraph::VertexSet&, diff --git a/g2o/types/slam2d/edge_se2_offset.cpp b/g2o/types/slam2d/edge_se2_offset.cpp index e08c3908e..58384bca4 100644 --- a/g2o/types/slam2d/edge_se2_offset.cpp +++ b/g2o/types/slam2d/edge_se2_offset.cpp @@ -28,12 +28,8 @@ #include -#include "g2o/core/cache.h" #include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" -#include "g2o/core/parameter.h" #include "g2o/types/slam2d/se2.h" -#include "g2o/types/slam2d/vertex_se2.h" #include "parameter_se2_offset.h" namespace g2o { @@ -56,22 +52,6 @@ bool EdgeSE2Offset::resolveCaches() { return (cacheFrom_ && cacheTo_); } -bool EdgeSE2Offset::read(std::istream& is) { - bool state = readParamIds(is); - - Vector3 meas; - state &= internal::readVector(is, meas); - setMeasurement(SE2(meas)); - state &= readInformationMatrix(is); - return state; -} - -bool EdgeSE2Offset::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement().toVector()); - return writeInformationMatrix(os); -} - void EdgeSE2Offset::computeError() { SE2 delta = inverseMeasurement_ * cacheFrom_->w2n() * cacheTo_->n2w(); error_.head<2>() = delta.translation(); @@ -89,8 +69,8 @@ void EdgeSE2Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, auto from = vertexXn<0>(); auto to = vertexXn<1>(); - SE2 virtualMeasurement = cacheFrom_->offsetParam()->offset() * measurement() * - cacheTo_->offsetParam()->offset().inverse(); + SE2 virtualMeasurement = cacheFrom_->offsetParam()->param() * measurement() * + cacheTo_->offsetParam()->param().inverse(); if (from_.count(from) > 0) to->setEstimate(from->estimate() * virtualMeasurement); diff --git a/g2o/types/slam2d/edge_se2_offset.h b/g2o/types/slam2d/edge_se2_offset.h index a83b3a6ac..a171f2373 100644 --- a/g2o/types/slam2d/edge_se2_offset.h +++ b/g2o/types/slam2d/edge_se2_offset.h @@ -28,7 +28,6 @@ #define G2O_EDGE_SE2_OFFSET_H_ #include -#include #include #include "g2o/core/base_binary_edge.h" @@ -49,8 +48,6 @@ class G2O_TYPES_SLAM2D_API EdgeSE2Offset : public BaseBinaryEdge<3, SE2, VertexSE2, VertexSE2> { public: EdgeSE2Offset(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; diff --git a/g2o/types/slam2d/edge_se2_pointxy.cpp b/g2o/types/slam2d/edge_se2_pointxy.cpp index 322025760..267b0d575 100644 --- a/g2o/types/slam2d/edge_se2_pointxy.cpp +++ b/g2o/types/slam2d/edge_se2_pointxy.cpp @@ -31,9 +31,7 @@ #include #include -#include "g2o/core/io_helper.h" -#include "g2o/stuff/property.h" -#include "g2o/types/slam2d/se2.h" +#include "g2o/config.h" #include "g2o/types/slam2d/vertex_point_xy.h" #include "g2o/types/slam2d/vertex_se2.h" @@ -44,17 +42,6 @@ namespace g2o { -bool EdgeSE2PointXY::read(std::istream& is) { - internal::readVector(is, measurement_); - readInformationMatrix(is); - return true; -} - -bool EdgeSE2PointXY::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE2PointXY::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { assert(from.size() == 1 && from.count(vertices_[0]) == 1 && diff --git a/g2o/types/slam2d/edge_se2_pointxy.h b/g2o/types/slam2d/edge_se2_pointxy.h index 8e2b970cf..1ae972e18 100644 --- a/g2o/types/slam2d/edge_se2_pointxy.h +++ b/g2o/types/slam2d/edge_se2_pointxy.h @@ -28,7 +28,6 @@ #define G2O_EDGE_SE2_POINT_XY_H #include -#include #include #include "g2o/config.h" @@ -62,9 +61,6 @@ class G2O_TYPES_SLAM2D_API EdgeSE2PointXY return true; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) override; double initialEstimatePossible(const OptimizableGraph::VertexSet& from, diff --git a/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp b/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp index 5382caf48..3498d6f5b 100644 --- a/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp +++ b/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp @@ -27,11 +27,9 @@ #include "edge_se2_pointxy_bearing.h" #include -#include #include #include -#include "g2o/stuff/property.h" #include "g2o/types/slam2d/se2.h" #include "g2o/types/slam2d/vertex_point_xy.h" #include "g2o/types/slam2d/vertex_se2.h" @@ -58,16 +56,6 @@ void EdgeSE2PointXYBearing::initialEstimate( l2->setEstimate(t * vr); } -bool EdgeSE2PointXYBearing::read(std::istream& is) { - is >> measurement_ >> information()(0, 0); - return true; -} - -bool EdgeSE2PointXYBearing::write(std::ostream& os) const { - os << measurement() << " " << information()(0, 0); - return os.good(); -} - #ifdef G2O_HAVE_OPENGL EdgeSE2PointXYBearingDrawAction::EdgeSE2PointXYBearingDrawAction() : DrawAction(typeid(EdgeSE2PointXYBearing).name()) {} diff --git a/g2o/types/slam2d/edge_se2_pointxy_bearing.h b/g2o/types/slam2d/edge_se2_pointxy_bearing.h index 2c96d130d..aad817809 100644 --- a/g2o/types/slam2d/edge_se2_pointxy_bearing.h +++ b/g2o/types/slam2d/edge_se2_pointxy_bearing.h @@ -29,7 +29,6 @@ #include #include -#include #include #include "g2o/config.h" @@ -66,9 +65,6 @@ class G2O_TYPES_SLAM2D_API EdgeSE2PointXYBearing return true; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex*) override { return (from.count(vertices_[0]) == 1 ? 1.0 : -1.0); diff --git a/g2o/types/slam2d/edge_se2_pointxy_calib.cpp b/g2o/types/slam2d/edge_se2_pointxy_calib.cpp index c867c5eae..6f8bfc4e8 100644 --- a/g2o/types/slam2d/edge_se2_pointxy_calib.cpp +++ b/g2o/types/slam2d/edge_se2_pointxy_calib.cpp @@ -28,17 +28,23 @@ #include -#include "g2o/core/io_helper.h" -#include "g2o/types/slam2d/se2.h" #include "g2o/types/slam2d/vertex_point_xy.h" #include "g2o/types/slam2d/vertex_se2.h" namespace g2o { -EdgeSE2PointXYCalib::EdgeSE2PointXYCalib() +void EdgeSE2PointXYCalib::computeError() { + const auto* v1 = vertexXnRaw<0>(); + const auto* l2 = vertexXnRaw<1>(); + const auto* calib = vertexXnRaw<2>(); + error_ = ((v1->estimate() * calib->estimate()).inverse() * l2->estimate()) - + measurement_; +} -{ - resize(3); +double EdgeSE2PointXYCalib::initialEstimatePossible( + const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { + (void)to; + return (from.count(vertices_[0]) == 1 ? 1.0 : -1.0); } void EdgeSE2PointXYCalib::initialEstimate( @@ -47,20 +53,9 @@ void EdgeSE2PointXYCalib::initialEstimate( "Can not initialize VertexSE2 position by VertexPointXY"); if (from.count(vertices_[0]) != 1) return; - auto* vi = static_cast(vertexRaw(0)); - auto* vj = static_cast(vertexRaw(1)); + auto* vi = vertexXnRaw<0>(); + auto* vj = vertexXnRaw<1>(); vj->setEstimate(vi->estimate() * measurement_); } -bool EdgeSE2PointXYCalib::read(std::istream& is) { - internal::readVector(is, measurement_); - readInformationMatrix(is); - return true; -} - -bool EdgeSE2PointXYCalib::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - } // namespace g2o diff --git a/g2o/types/slam2d/edge_se2_pointxy_calib.h b/g2o/types/slam2d/edge_se2_pointxy_calib.h index 458cc73ea..960b5a940 100644 --- a/g2o/types/slam2d/edge_se2_pointxy_calib.h +++ b/g2o/types/slam2d/edge_se2_pointxy_calib.h @@ -27,10 +27,7 @@ #ifndef G2O_EDGE_SE2_XY_CALIB_H #define G2O_EDGE_SE2_XY_CALIB_H -#include -#include - -#include "g2o/core/base_variable_sized_edge.h" +#include "g2o/core/base_fixed_sized_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" #include "g2o_types_slam2d_api.h" @@ -45,26 +42,12 @@ namespace g2o { * measurement */ class G2O_TYPES_SLAM2D_API EdgeSE2PointXYCalib - : public BaseVariableSizedEdge<2, Vector2> { + : public BaseFixedSizedEdge<2, Vector2, VertexSE2, VertexPointXY, + VertexSE2> { public: - EdgeSE2PointXYCalib(); - - void computeError() override { - const auto* v1 = static_cast(vertexRaw(0)); - const auto* l2 = static_cast(vertexRaw(1)); - const auto* calib = static_cast(vertexRaw(2)); - error_ = ((v1->estimate() * calib->estimate()).inverse() * l2->estimate()) - - measurement_; - } - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - + void computeError() override; double initialEstimatePossible(const OptimizableGraph::VertexSet& from, - OptimizableGraph::Vertex* to) override { - (void)to; - return (from.count(vertices_[0]) == 1 ? 1.0 : -1.0); - } + OptimizableGraph::Vertex* to) override; void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) override; }; diff --git a/g2o/types/slam2d/edge_se2_pointxy_offset.cpp b/g2o/types/slam2d/edge_se2_pointxy_offset.cpp index c47de9755..b6228dcc3 100644 --- a/g2o/types/slam2d/edge_se2_pointxy_offset.cpp +++ b/g2o/types/slam2d/edge_se2_pointxy_offset.cpp @@ -27,15 +27,11 @@ #include "edge_se2_pointxy_offset.h" #include -#include -#include "g2o/core/cache.h" -#include "g2o/core/io_helper.h" #include "g2o/core/parameter.h" #include "g2o/types/slam2d/vertex_point_xy.h" #include "g2o/types/slam2d/vertex_se2.h" #include "parameter_se2_offset.h" -#include "se2.h" namespace g2o { @@ -53,25 +49,6 @@ bool EdgeSE2PointXYOffset::resolveCaches() { return cache_ != nullptr; } -bool EdgeSE2PointXYOffset::read(std::istream& is) { - int pId; - is >> pId; - setParameterId(0, pId); - // measured keypoint - internal::readVector(is, measurement_); - if (is.bad()) return false; - readInformationMatrix(is); - // we overwrite the information matrix in case of read errors - if (is.bad()) information().setIdentity(); - return true; -} - -bool EdgeSE2PointXYOffset::write(std::ostream& os) const { - os << parameters_[0]->id() << " "; - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE2PointXYOffset::computeError() { // from cam to point (track) // VertexSE2 *rob = static_cast(vertices_[0]); diff --git a/g2o/types/slam2d/edge_se2_pointxy_offset.h b/g2o/types/slam2d/edge_se2_pointxy_offset.h index fcefa0b5b..2cd0298b6 100644 --- a/g2o/types/slam2d/edge_se2_pointxy_offset.h +++ b/g2o/types/slam2d/edge_se2_pointxy_offset.h @@ -27,8 +27,6 @@ #ifndef G2O_EDGE_SE2_POINT_XY_OFFSET_H_ #define G2O_EDGE_SE2_POINT_XY_OFFSET_H_ -#include -#include #include #include "g2o/core/base_binary_edge.h" @@ -50,8 +48,6 @@ class G2O_TYPES_SLAM2D_API EdgeSE2PointXYOffset : public BaseBinaryEdge<2, Vector2, VertexSE2, VertexPointXY> { public: EdgeSE2PointXYOffset(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; void linearizeOplus() override; diff --git a/g2o/types/slam2d/edge_se2_prior.cpp b/g2o/types/slam2d/edge_se2_prior.cpp index e1246deff..8834d23cf 100644 --- a/g2o/types/slam2d/edge_se2_prior.cpp +++ b/g2o/types/slam2d/edge_se2_prior.cpp @@ -26,10 +26,6 @@ #include "edge_se2_prior.h" -#include - -#include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" #include "g2o/types/slam2d/se2.h" #include "g2o/types/slam2d/vertex_se2.h" @@ -44,20 +40,6 @@ void EdgeSE2Prior::initialEstimate(const OptimizableGraph::VertexSet& from, v1->setEstimate(measurement_); } -bool EdgeSE2Prior::read(std::istream& is) { - Vector3 p; - internal::readVector(is, p); - setMeasurement(SE2(p)); - inverseMeasurement_ = measurement_.inverse(); - readInformationMatrix(is); - return true; -} - -bool EdgeSE2Prior::write(std::ostream& os) const { - internal::writeVector(os, measurement().toVector()); - return writeInformationMatrix(os); -} - void EdgeSE2Prior::setMeasurement(const SE2& m) { measurement_ = m; inverseMeasurement_ = m.inverse(); diff --git a/g2o/types/slam2d/edge_se2_prior.h b/g2o/types/slam2d/edge_se2_prior.h index a3464223f..e993577ad 100644 --- a/g2o/types/slam2d/edge_se2_prior.h +++ b/g2o/types/slam2d/edge_se2_prior.h @@ -27,10 +27,6 @@ #ifndef G2O_EDGE_SE2_PRIOR_H #define G2O_EDGE_SE2_PRIOR_H -#include -#include -#include - #include "g2o/core/base_unary_edge.h" #include "g2o/core/optimizable_graph.h" #include "g2o_types_slam2d_api.h" @@ -62,9 +58,6 @@ class G2O_TYPES_SLAM2D_API EdgeSE2Prior void setMeasurement(const SE2& m) override; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*) override { return 1.; diff --git a/g2o/types/slam2d/edge_se2_twopointsxy.cpp b/g2o/types/slam2d/edge_se2_twopointsxy.cpp index 563075e65..684eb3306 100644 --- a/g2o/types/slam2d/edge_se2_twopointsxy.cpp +++ b/g2o/types/slam2d/edge_se2_twopointsxy.cpp @@ -28,22 +28,16 @@ #include #include -#include -#include -#include -#include "se2.h" #include "vertex_point_xy.h" #include "vertex_se2.h" namespace g2o { -EdgeSE2TwoPointsXY::EdgeSE2TwoPointsXY() { resize(3); } - void EdgeSE2TwoPointsXY::computeError() { - auto* pose = static_cast(vertexRaw(0)); - auto* xy1 = static_cast(vertexRaw(1)); - auto* xy2 = static_cast(vertexRaw(2)); + auto* pose = vertexXnRaw<0>(); + auto* xy1 = vertexXnRaw<1>(); + auto* xy2 = vertexXnRaw<2>(); Vector2 m1 = pose->estimate().inverse() * xy1->estimate(); Vector2 m2 = pose->estimate().inverse() * xy2->estimate(); @@ -54,33 +48,6 @@ void EdgeSE2TwoPointsXY::computeError() { error_[3] = m2[1] - measurement_[3]; } -bool EdgeSE2TwoPointsXY::read(std::istream& is) { - is >> measurement_[0] >> measurement_[1] >> measurement_[2] >> - measurement_[3]; - is >> information()(0, 0) >> information()(0, 1) >> information()(0, 2) >> - information()(0, 3) >> information()(1, 1) >> information()(1, 2) >> - information()(1, 3) >> information()(2, 2) >> information()(2, 3) >> - information()(3, 3); - information()(1, 0) = information()(0, 1); - information()(2, 0) = information()(0, 2); - information()(2, 1) = information()(1, 2); - information()(3, 0) = information()(0, 3); - information()(3, 1) = information()(1, 3); - information()(3, 2) = information()(2, 3); - return true; -} - -bool EdgeSE2TwoPointsXY::write(std::ostream& os) const { - os << measurement()[0] << " " << measurement()[1] << " " << measurement()[2] - << " " << measurement()[3] << " "; - os << information()(0, 0) << " " << information()(0, 1) << " " - << information()(0, 2) << " " << information()(0, 3) << " " - << information()(1, 1) << " " << information()(1, 2) << " " - << information()(1, 3) << " " << information()(2, 2) << " " - << information()(2, 3) << " " << information()(3, 3); - return os.good(); -} - void EdgeSE2TwoPointsXY::initialEstimate( const OptimizableGraph::VertexSet& fixed, OptimizableGraph::Vertex* toEstimate) { @@ -89,9 +56,9 @@ void EdgeSE2TwoPointsXY::initialEstimate( assert(initialEstimatePossible(fixed, toEstimate) && "Bad vertices specified"); - auto* pose = static_cast(vertexRaw(0)); - auto* v1 = static_cast(vertexRaw(1)); - auto* v2 = static_cast(vertexRaw(2)); + auto* pose = vertexXnRaw<0>(); + auto* v1 = vertexXnRaw<1>(); + auto* v2 = vertexXnRaw<2>(); bool estimatev1 = true; bool estimatev2 = true; @@ -129,9 +96,9 @@ double EdgeSE2TwoPointsXY::initialEstimatePossible( } bool EdgeSE2TwoPointsXY::setMeasurementFromState() { - auto* pose = static_cast(vertexRaw(0)); - auto* xy1 = static_cast(vertexRaw(1)); - auto* xy2 = static_cast(vertexRaw(2)); + auto* pose = vertexXnRaw<0>(); + auto* xy1 = vertexXnRaw<1>(); + auto* xy2 = vertexXnRaw<2>(); Vector2 m1 = pose->estimate().inverse() * xy1->estimate(); Vector2 m2 = pose->estimate().inverse() * xy2->estimate(); diff --git a/g2o/types/slam2d/edge_se2_twopointsxy.h b/g2o/types/slam2d/edge_se2_twopointsxy.h index b23d97013..57791f771 100644 --- a/g2o/types/slam2d/edge_se2_twopointsxy.h +++ b/g2o/types/slam2d/edge_se2_twopointsxy.h @@ -27,29 +27,20 @@ #ifndef G2O_EDGE_SE2_TWOPOINTS_XY_H #define G2O_EDGE_SE2_TWOPOINTS_XY_H -#include -#include - -#include "g2o/config.h" -#include "g2o/core/base_variable_sized_edge.h" +#include "g2o/core/base_fixed_sized_edge.h" #include "g2o/core/eigen_types.h" -#include "g2o/core/optimizable_graph.h" +#include "g2o/types/slam2d/vertex_point_xy.h" +#include "g2o/types/slam2d/vertex_se2.h" #include "g2o_types_slam2d_api.h" -#include "vertex_point_xy.h" -#include "vertex_se2.h" namespace g2o { class G2O_TYPES_SLAM2D_API EdgeSE2TwoPointsXY - : public BaseVariableSizedEdge<4, Vector4> { + : public BaseFixedSizedEdge<4, Vector4, VertexSE2, VertexPointXY, + VertexPointXY> { public: - EdgeSE2TwoPointsXY(); - void computeError() override; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - bool setMeasurementFromState() override; void initialEstimate(const OptimizableGraph::VertexSet&, diff --git a/g2o/types/slam2d/edge_se2_xyprior.cpp b/g2o/types/slam2d/edge_se2_xyprior.cpp index 14ce87b07..cac85e3df 100644 --- a/g2o/types/slam2d/edge_se2_xyprior.cpp +++ b/g2o/types/slam2d/edge_se2_xyprior.cpp @@ -26,21 +26,8 @@ #include "edge_se2_xyprior.h" -#include "g2o/core/io_helper.h" - namespace g2o { -bool EdgeSE2XYPrior::read(std::istream& is) { - internal::readVector(is, measurement_); - readInformationMatrix(is); - return true; -} - -bool EdgeSE2XYPrior::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE2XYPrior::linearizeOplus() { jacobianOplusXi_ << 1, 0, 0, 0, 1, 0; } } // namespace g2o diff --git a/g2o/types/slam2d/edge_se2_xyprior.h b/g2o/types/slam2d/edge_se2_xyprior.h index 2501c6244..21b12dc94 100644 --- a/g2o/types/slam2d/edge_se2_xyprior.h +++ b/g2o/types/slam2d/edge_se2_xyprior.h @@ -27,14 +27,9 @@ #ifndef G2O_EDGE_SE2_PRIOR_XY_H #define G2O_EDGE_SE2_PRIOR_XY_H -#include -#include -#include - #include "g2o/core/base_unary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o_types_slam2d_api.h" -#include "se2.h" #include "vertex_se2.h" namespace g2o { @@ -50,9 +45,6 @@ class G2O_TYPES_SLAM2D_API EdgeSE2XYPrior void linearizeOplus() override; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void computeError() override { const VertexSE2* v = vertexXnRaw<0>(); error_ = v->estimate().translation() - measurement_; diff --git a/g2o/types/slam2d/edge_xy_prior.cpp b/g2o/types/slam2d/edge_xy_prior.cpp index 344dbdf79..71893d7a9 100644 --- a/g2o/types/slam2d/edge_xy_prior.cpp +++ b/g2o/types/slam2d/edge_xy_prior.cpp @@ -26,29 +26,13 @@ #include "edge_xy_prior.h" -#include "g2o/core/io_helper.h" -#include "g2o/types/slam2d/vertex_point_xy.h" - namespace g2o { -EdgeXYPrior::EdgeXYPrior() - -{ +EdgeXYPrior::EdgeXYPrior() { information_.setIdentity(); error_.setZero(); } -bool EdgeXYPrior::read(std::istream& is) { - internal::readVector(is, measurement_); - readInformationMatrix(is); - return true; -} - -bool EdgeXYPrior::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES void EdgeXYPrior::linearizeOplus() { jacobianOplusXi_ = Matrix2::Identity(); } #endif diff --git a/g2o/types/slam2d/edge_xy_prior.h b/g2o/types/slam2d/edge_xy_prior.h index ad9c11017..cc663c478 100644 --- a/g2o/types/slam2d/edge_xy_prior.h +++ b/g2o/types/slam2d/edge_xy_prior.h @@ -28,10 +28,7 @@ #define G2O_EDGE_XY_PRIOR_H #include -#include -#include -#include "g2o/config.h" #include "g2o/core/base_unary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" @@ -48,8 +45,6 @@ class G2O_TYPES_SLAM2D_API EdgeXYPrior void computeError() override { error_ = vertexXnRaw<0>()->estimate() - measurement_; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void setMeasurement(const Vector2& m) override { measurement_ = m; } diff --git a/g2o/types/slam2d/parameter_se2_offset.cpp b/g2o/types/slam2d/parameter_se2_offset.cpp index 83d1e7166..231483681 100644 --- a/g2o/types/slam2d/parameter_se2_offset.cpp +++ b/g2o/types/slam2d/parameter_se2_offset.cpp @@ -30,32 +30,19 @@ #include #include -#include "g2o/core/io_helper.h" #include "g2o/types/slam2d/se2.h" #include "vertex_se2.h" namespace g2o { -ParameterSE2Offset::ParameterSE2Offset() { setOffset(); } +ParameterSE2Offset::ParameterSE2Offset() { update(); } -void ParameterSE2Offset::setOffset(const SE2& offset) { - offset_ = offset; - offsetMatrix_ = offset_.rotation().toRotationMatrix(); - offsetMatrix_.translation() = offset_.translation(); +void ParameterSE2Offset::update() { + offsetMatrix_ = parameter_.rotation().toRotationMatrix(); + offsetMatrix_.translation() = parameter_.translation(); inverseOffsetMatrix_ = offsetMatrix_.inverse(); } -bool ParameterSE2Offset::read(std::istream& is) { - Vector3 off; - bool state = g2o::internal::readVector(is, off); - setOffset(SE2(off)); - return state; -} - -bool ParameterSE2Offset::write(std::ostream& os) const { - return internal::writeVector(os, offset().toVector()); -} - void CacheSE2Offset::updateImpl() { #ifndef NDEBUG auto* offsetParam = dynamic_cast(parameters_[0].get()); @@ -64,7 +51,7 @@ void CacheSE2Offset::updateImpl() { #endif const auto& v = static_cast(vertex()); - se2_n2w_ = v.estimate() * offsetParam->offset(); + se2_n2w_ = v.estimate() * offsetParam->param(); n2w_ = se2_n2w_.rotation().toRotationMatrix(); n2w_.translation() = se2_n2w_.translation(); @@ -83,7 +70,7 @@ void CacheSE2Offset::updateImpl() { Matrix2 RInversePrime; RInversePrime << -s, c, -c, -s; RpInverse_RInversePrime_ = - offsetParam->offset().rotation().toRotationMatrix().transpose() * + offsetParam->param().rotation().toRotationMatrix().transpose() * RInversePrime; RpInverse_RInverse_ = w2l.rotation(); } diff --git a/g2o/types/slam2d/parameter_se2_offset.h b/g2o/types/slam2d/parameter_se2_offset.h index 3041beb68..0e56e639b 100644 --- a/g2o/types/slam2d/parameter_se2_offset.h +++ b/g2o/types/slam2d/parameter_se2_offset.h @@ -27,7 +27,6 @@ #ifndef G2O_VERTEX_SE2_OFFSET_PARAMETERS_H_ #define G2O_VERTEX_SE2_OFFSET_PARAMETERS_H_ -#include #include #include "g2o/core/cache.h" @@ -43,20 +42,15 @@ class VertexSE2; /** * \brief offset for an SE2 */ -class G2O_TYPES_SLAM2D_API ParameterSE2Offset : public Parameter { +class G2O_TYPES_SLAM2D_API ParameterSE2Offset : public BaseParameter { public: ParameterSE2Offset(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - /** * update the offset to a new value. * re-calculates the different representations, e.g., the rotation matrix */ - void setOffset(const SE2& offset_ = SE2()); - - [[nodiscard]] const SE2& offset() const { return offset_; } + void update() override; //! rotation of the offset as 2x2 rotation matrix [[nodiscard]] const Isometry2& offsetMatrix() const { return offsetMatrix_; } @@ -67,7 +61,6 @@ class G2O_TYPES_SLAM2D_API ParameterSE2Offset : public Parameter { } protected: - SE2 offset_; Isometry2 offsetMatrix_; Isometry2 inverseOffsetMatrix_; }; diff --git a/g2o/types/slam2d/se2.h b/g2o/types/slam2d/se2.h index aceca1499..64aef87be 100644 --- a/g2o/types/slam2d/se2.h +++ b/g2o/types/slam2d/se2.h @@ -156,8 +156,6 @@ struct TypeTraits { static Type fromMinimalVector(const Eigen::DenseBase& v) { return SE2(v[0], v[1], v[2]); } - - static Type Identity() { return SE2(); } }; } // namespace g2o diff --git a/g2o/types/slam2d/types_slam2d.cpp b/g2o/types/slam2d/types_slam2d.cpp index ba4858225..1255b3175 100644 --- a/g2o/types/slam2d/types_slam2d.cpp +++ b/g2o/types/slam2d/types_slam2d.cpp @@ -24,10 +24,6 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include "types_slam2d.h" - -#include - #include "edge_pointxy.h" #include "edge_se2.h" #include "edge_se2_lotsofxy.h" diff --git a/g2o/types/slam2d/types_slam2d.h b/g2o/types/slam2d/types_slam2d.h deleted file mode 100644 index 385f1cee9..000000000 --- a/g2o/types/slam2d/types_slam2d.h +++ /dev/null @@ -1,47 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_TYPES_SLAM2D_ -#define G2O_TYPES_SLAM2D_ - -#include "edge_pointxy.h" -#include "edge_se2.h" -#include "edge_se2_lotsofxy.h" -#include "edge_se2_offset.h" -#include "edge_se2_pointxy.h" -#include "edge_se2_pointxy_bearing.h" -#include "edge_se2_pointxy_calib.h" -#include "edge_se2_pointxy_offset.h" -#include "edge_se2_prior.h" -#include "edge_se2_twopointsxy.h" -#include "edge_se2_xyprior.h" -#include "edge_xy_prior.h" -#include "g2o/config.h" -#include "parameter_se2_offset.h" -#include "vertex_point_xy.h" -#include "vertex_se2.h" - -#endif diff --git a/g2o/types/slam2d/vertex_se2.cpp b/g2o/types/slam2d/vertex_se2.cpp index 4e5db6808..0d3459465 100644 --- a/g2o/types/slam2d/vertex_se2.cpp +++ b/g2o/types/slam2d/vertex_se2.cpp @@ -32,7 +32,6 @@ #include "g2o/stuff/macros.h" #include "g2o/stuff/misc.h" -#include "g2o/types/slam2d/se2.h" #ifdef G2O_HAVE_OPENGL #include "g2o/stuff/opengl_primitives.h" diff --git a/g2o/types/slam2d_addons/CMakeLists.txt b/g2o/types/slam2d_addons/CMakeLists.txt index cf2300748..b867ffe8a 100644 --- a/g2o/types/slam2d_addons/CMakeLists.txt +++ b/g2o/types/slam2d_addons/CMakeLists.txt @@ -1,6 +1,6 @@ add_library(types_slam2d_addons ${G2O_LIB_TYPE} line_2d.h - types_slam2d_addons.cpp types_slam2d_addons.h + types_slam2d_addons.cpp vertex_segment2d.cpp vertex_segment2d.h edge_se2_segment2d.cpp edge_se2_segment2d.h edge_se2_segment2d_line.cpp edge_se2_segment2d_line.h diff --git a/g2o/types/slam2d_addons/edge_line2d.cpp b/g2o/types/slam2d_addons/edge_line2d.cpp index 5ed6377a3..bce43d431 100644 --- a/g2o/types/slam2d_addons/edge_line2d.cpp +++ b/g2o/types/slam2d_addons/edge_line2d.cpp @@ -26,10 +26,6 @@ #include "edge_line2d.h" -#include "g2o/config.h" -#include "g2o/core/io_helper.h" -#include "g2o/types/slam2d_addons/vertex_line2d.h" - namespace g2o { EdgeLine2D::EdgeLine2D() { @@ -37,16 +33,6 @@ EdgeLine2D::EdgeLine2D() { error_.setZero(); } -bool EdgeLine2D::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeLine2D::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeLine2D::linearizeOplus() { jacobianOplusXi_ = -Matrix2::Identity(); jacobianOplusXj_ = Matrix2::Identity(); diff --git a/g2o/types/slam2d_addons/edge_line2d.h b/g2o/types/slam2d_addons/edge_line2d.h index d90c946e2..0a6db564e 100644 --- a/g2o/types/slam2d_addons/edge_line2d.h +++ b/g2o/types/slam2d_addons/edge_line2d.h @@ -27,9 +27,6 @@ #ifndef G2O_EDGE_LINE2D_H #define G2O_EDGE_LINE2D_H -#include -#include - #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" @@ -49,8 +46,6 @@ class G2O_TYPES_SLAM2D_ADDONS_API EdgeLine2D const VertexLine2D* v2 = vertexXnRaw<1>(); error_ = (v2->estimate() - v1->estimate()) - measurement_; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void setMeasurement(const Line2D& m) override { measurement_ = m; } diff --git a/g2o/types/slam2d_addons/edge_line2d_pointxy.cpp b/g2o/types/slam2d_addons/edge_line2d_pointxy.cpp index 9574b1b94..c0db407a7 100644 --- a/g2o/types/slam2d_addons/edge_line2d_pointxy.cpp +++ b/g2o/types/slam2d_addons/edge_line2d_pointxy.cpp @@ -26,22 +26,23 @@ #include "edge_line2d_pointxy.h" -#include - -#include "g2o/config.h" - namespace g2o { -bool EdgeLine2DPointXY::read(std::istream& is) { - is >> measurement_; - is >> information()(0, 0); - return true; +void EdgeLine2DPointXY::computeError() { + const VertexLine2D* l = vertexXnRaw<0>(); + const VertexPointXY* p = vertexXnRaw<1>(); + Vector2 n(std::cos(l->theta()), std::sin(l->theta())); + double prediction = n.dot(p->estimate()) - l->rho(); + error_[0] = prediction - measurement_; } -bool EdgeLine2DPointXY::write(std::ostream& os) const { - os << measurement() << " "; - os << information()(0, 0); - return os.good(); +bool EdgeLine2DPointXY::setMeasurementFromState() { + const VertexLine2D* l = vertexXnRaw<0>(); + const VertexPointXY* p = vertexXnRaw<1>(); + Vector2 n(std::cos(l->theta()), std::sin(l->theta())); + double prediction = n.dot(p->estimate()) - l->rho(); + measurement_ = prediction; + return true; } } // namespace g2o diff --git a/g2o/types/slam2d_addons/edge_line2d_pointxy.h b/g2o/types/slam2d_addons/edge_line2d_pointxy.h index a1723371f..ba8a6b011 100644 --- a/g2o/types/slam2d_addons/edge_line2d_pointxy.h +++ b/g2o/types/slam2d_addons/edge_line2d_pointxy.h @@ -27,14 +27,7 @@ #ifndef G2O_EDGE_LINE2D_POINTXY_H #define G2O_EDGE_LINE2D_POINTXY_H -#include -#include -#include - -#include "g2o/config.h" #include "g2o/core/base_binary_edge.h" -#include "g2o/core/eigen_types.h" -#include "g2o/stuff/misc.h" #include "g2o/types/slam2d/vertex_point_xy.h" #include "g2o_types_slam2d_addons_api.h" #include "vertex_line2d.h" @@ -47,25 +40,9 @@ class EdgeLine2DPointXY // MSVC { public: - G2O_TYPES_SLAM2D_ADDONS_API void computeError() override { - const VertexLine2D* l = vertexXnRaw<0>(); - const VertexPointXY* p = vertexXnRaw<1>(); - Vector2 n(std::cos(l->theta()), std::sin(l->theta())); - double prediction = n.dot(p->estimate()) - l->rho(); - error_[0] = prediction - measurement_; - } - - G2O_TYPES_SLAM2D_ADDONS_API bool setMeasurementFromState() override { - const VertexLine2D* l = vertexXnRaw<0>(); - const VertexPointXY* p = vertexXnRaw<1>(); - Vector2 n(std::cos(l->theta()), std::sin(l->theta())); - double prediction = n.dot(p->estimate()) - l->rho(); - measurement_ = prediction; - return true; - } + G2O_TYPES_SLAM2D_ADDONS_API void computeError() override; - G2O_TYPES_SLAM2D_ADDONS_API bool read(std::istream& is) override; - G2O_TYPES_SLAM2D_ADDONS_API bool write(std::ostream& os) const override; + G2O_TYPES_SLAM2D_ADDONS_API bool setMeasurementFromState() override; }; } // namespace g2o diff --git a/g2o/types/slam2d_addons/edge_se2_line2d.cpp b/g2o/types/slam2d_addons/edge_se2_line2d.cpp index ce78c3f11..c4e56934e 100644 --- a/g2o/types/slam2d_addons/edge_se2_line2d.cpp +++ b/g2o/types/slam2d_addons/edge_se2_line2d.cpp @@ -26,23 +26,10 @@ #include "edge_se2_line2d.h" -#include - -#include "g2o/core/io_helper.h" #include "g2o/types/slam2d_addons/vertex_line2d.h" namespace g2o { -bool EdgeSE2Line2D::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeSE2Line2D::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE2Line2D::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { assert(from.size() == 1 && from.count(vertices_[0]) == 1 && diff --git a/g2o/types/slam2d_addons/edge_se2_line2d.h b/g2o/types/slam2d_addons/edge_se2_line2d.h index 4499bc6be..d2369f6f5 100644 --- a/g2o/types/slam2d_addons/edge_se2_line2d.h +++ b/g2o/types/slam2d_addons/edge_se2_line2d.h @@ -29,7 +29,6 @@ #include #include -#include #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" @@ -72,9 +71,6 @@ class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Line2D return true; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) override; double initialEstimatePossible(const OptimizableGraph::VertexSet& from, diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d.cpp b/g2o/types/slam2d_addons/edge_se2_segment2d.cpp index b56c28749..b2084bf19 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d.cpp +++ b/g2o/types/slam2d_addons/edge_se2_segment2d.cpp @@ -29,18 +29,26 @@ #include #include -#include "g2o/core/io_helper.h" - namespace g2o { -bool EdgeSE2Segment2D::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); +void EdgeSE2Segment2D::computeError() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSegment2D* l2 = vertexXnRaw<1>(); + Eigen::Map error1(&error_(0)); + Eigen::Map error2(&error_(2)); + SE2 iEst = v1->estimate().inverse(); + error1 = (iEst * l2->estimateP1()); + error2 = (iEst * l2->estimateP2()); + error_ = error_ - measurement_; } -bool EdgeSE2Segment2D::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); +bool EdgeSE2Segment2D::setMeasurementFromState() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSegment2D* l2 = vertexXnRaw<1>(); + SE2 iEst = v1->estimate().inverse(); + setMeasurementP1(iEst * l2->estimateP1()); + setMeasurementP2(iEst * l2->estimateP2()); + return true; } void EdgeSE2Segment2D::initialEstimate(const OptimizableGraph::VertexSet& from, diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d.h b/g2o/types/slam2d_addons/edge_se2_segment2d.h index a88937c4f..505e83cf4 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d.h +++ b/g2o/types/slam2d_addons/edge_se2_segment2d.h @@ -28,12 +28,10 @@ #define G2O_EDGE_SE2_SEGMENT2D_H #include -#include #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" -#include "g2o/types/slam2d/se2.h" #include "g2o/types/slam2d/vertex_se2.h" #include "g2o_types_slam2d_addons_api.h" #include "vertex_segment2d.h" @@ -61,28 +59,9 @@ class EdgeSE2Segment2D v = p2; } - G2O_TYPES_SLAM2D_ADDONS_API void computeError() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSegment2D* l2 = vertexXnRaw<1>(); - Eigen::Map error1(&error_(0)); - Eigen::Map error2(&error_(2)); - SE2 iEst = v1->estimate().inverse(); - error1 = (iEst * l2->estimateP1()); - error2 = (iEst * l2->estimateP2()); - error_ = error_ - measurement_; - } - - G2O_TYPES_SLAM2D_ADDONS_API bool setMeasurementFromState() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSegment2D* l2 = vertexXnRaw<1>(); - SE2 iEst = v1->estimate().inverse(); - setMeasurementP1(iEst * l2->estimateP1()); - setMeasurementP2(iEst * l2->estimateP2()); - return true; - } + G2O_TYPES_SLAM2D_ADDONS_API void computeError() override; - G2O_TYPES_SLAM2D_ADDONS_API bool read(std::istream& is) override; - G2O_TYPES_SLAM2D_ADDONS_API bool write(std::ostream& os) const override; + G2O_TYPES_SLAM2D_ADDONS_API bool setMeasurementFromState() override; G2O_TYPES_SLAM2D_ADDONS_API void initialEstimate( const OptimizableGraph::VertexSet& from, diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_line.cpp b/g2o/types/slam2d_addons/edge_se2_segment2d_line.cpp index f03256549..91e6161e8 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d_line.cpp +++ b/g2o/types/slam2d_addons/edge_se2_segment2d_line.cpp @@ -26,18 +26,39 @@ #include "edge_se2_segment2d_line.h" -#include "g2o/core/io_helper.h" +#include namespace g2o { -bool EdgeSE2Segment2DLine::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); +void EdgeSE2Segment2DLine::computeError() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSegment2D* l2 = vertexXnRaw<1>(); + SE2 iEst = v1->estimate().inverse(); + Vector2 predP1 = iEst * l2->estimateP1(); + Vector2 predP2 = iEst * l2->estimateP2(); + Vector2 dP = predP2 - predP1; + Vector2 normal(dP.y(), -dP.x()); + normal.normalize(); + Vector2 prediction(std::atan2(normal.y(), normal.x()), + predP1.dot(normal) * .5 + predP2.dot(normal) * .5); + + error_ = prediction - measurement_; + error_[0] = normalize_theta(error_[0]); } -bool EdgeSE2Segment2DLine::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); +bool EdgeSE2Segment2DLine::setMeasurementFromState() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSegment2D* l2 = vertexXnRaw<1>(); + SE2 iEst = v1->estimate().inverse(); + Vector2 predP1 = iEst * l2->estimateP1(); + Vector2 predP2 = iEst * l2->estimateP2(); + Vector2 dP = predP2 - predP1; + Vector2 normal(dP.y(), -dP.x()); + normal.normalize(); + Vector2 prediction(std::atan2(normal.y(), normal.x()), + predP1.dot(normal) * .5 + predP2.dot(normal) * .5); + measurement_ = prediction; + return true; } } // namespace g2o diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_line.h b/g2o/types/slam2d_addons/edge_se2_segment2d_line.h index a68f284f2..9410925db 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d_line.h +++ b/g2o/types/slam2d_addons/edge_se2_segment2d_line.h @@ -28,14 +28,9 @@ #define G2O_EDGE_SE2_SEGMENT2D_LINE_H #include -#include -#include -#include "g2o/config.h" #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" -#include "g2o/stuff/misc.h" -#include "g2o/types/slam2d/se2.h" #include "g2o/types/slam2d/vertex_se2.h" #include "g2o_types_slam2d_addons_api.h" #include "vertex_segment2d.h" @@ -58,39 +53,9 @@ class EdgeSE2Segment2DLine G2O_TYPES_SLAM2D_ADDONS_API void setTheta(double t) { measurement_[0] = t; } G2O_TYPES_SLAM2D_ADDONS_API void setRho(double r) { measurement_[1] = r; } - G2O_TYPES_SLAM2D_ADDONS_API void computeError() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSegment2D* l2 = vertexXnRaw<1>(); - SE2 iEst = v1->estimate().inverse(); - Vector2 predP1 = iEst * l2->estimateP1(); - Vector2 predP2 = iEst * l2->estimateP2(); - Vector2 dP = predP2 - predP1; - Vector2 normal(dP.y(), -dP.x()); - normal.normalize(); - Vector2 prediction(std::atan2(normal.y(), normal.x()), - predP1.dot(normal) * .5 + predP2.dot(normal) * .5); + G2O_TYPES_SLAM2D_ADDONS_API void computeError() override; - error_ = prediction - measurement_; - error_[0] = normalize_theta(error_[0]); - } - - G2O_TYPES_SLAM2D_ADDONS_API bool setMeasurementFromState() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSegment2D* l2 = vertexXnRaw<1>(); - SE2 iEst = v1->estimate().inverse(); - Vector2 predP1 = iEst * l2->estimateP1(); - Vector2 predP2 = iEst * l2->estimateP2(); - Vector2 dP = predP2 - predP1; - Vector2 normal(dP.y(), -dP.x()); - normal.normalize(); - Vector2 prediction(std::atan2(normal.y(), normal.x()), - predP1.dot(normal) * .5 + predP2.dot(normal) * .5); - measurement_ = prediction; - return true; - } - - G2O_TYPES_SLAM2D_ADDONS_API bool read(std::istream& is) override; - G2O_TYPES_SLAM2D_ADDONS_API bool write(std::ostream& os) const override; + G2O_TYPES_SLAM2D_ADDONS_API bool setMeasurementFromState() override; /* #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES */ /* virtual void linearizeOplus(); */ diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.cpp b/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.cpp index 6860f72bb..5b7a54e22 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.cpp +++ b/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.cpp @@ -26,22 +26,42 @@ #include "edge_se2_segment2d_pointLine.h" -#include - -#include "g2o/core/io_helper.h" +#include namespace g2o { -bool EdgeSE2Segment2DPointLine::read(std::istream& is) { - is >> pointNum_; - internal::readVector(is, measurement_); - return readInformationMatrix(is); +void EdgeSE2Segment2DPointLine::computeError() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSegment2D* l2 = vertexXnRaw<1>(); + SE2 iEst = v1->estimate().inverse(); + Vector2 predP1 = iEst * l2->estimateP1(); + Vector2 predP2 = iEst * l2->estimateP2(); + Vector2 dP = predP2 - predP1; + Vector2 normal(dP.y(), -dP.x()); + normal.normalize(); + Vector3 prediction; + prediction[2] = std::atan2(normal.y(), normal.x()); + Eigen::Map pt(prediction.data()); + pt = (pointNum_ == 0) ? predP1 : predP2; + error_ = prediction - measurement_; + error_[2] = normalize_theta(error_[2]); } -bool EdgeSE2Segment2DPointLine::write(std::ostream& os) const { - os << pointNum_ << " "; - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); +bool EdgeSE2Segment2DPointLine::setMeasurementFromState() { + const VertexSE2* v1 = vertexXnRaw<0>(); + const VertexSegment2D* l2 = vertexXnRaw<1>(); + SE2 iEst = v1->estimate().inverse(); + Vector2 predP1 = iEst * l2->estimateP1(); + Vector2 predP2 = iEst * l2->estimateP2(); + Vector2 dP = predP2 - predP1; + Vector2 normal(dP.y(), -dP.x()); + normal.normalize(); + Vector3 prediction; + prediction[2] = std::atan2(normal.y(), normal.x()); + Eigen::Map pt(prediction.data()); + pt = (pointNum_ == 0) ? predP1 : predP2; + setMeasurement(prediction); + return true; } } // namespace g2o diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h b/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h index 0cb3c2959..74edf3adc 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h +++ b/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h @@ -28,14 +28,9 @@ #define G2O_EDGE_SE2_SEGMENT2D_POINTLINE_H #include -#include -#include -#include "g2o/config.h" #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" -#include "g2o/stuff/misc.h" -#include "g2o/types/slam2d/se2.h" #include "g2o/types/slam2d/vertex_se2.h" #include "g2o_types_slam2d_addons_api.h" #include "vertex_segment2d.h" @@ -60,44 +55,9 @@ class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DPointLine [[nodiscard]] int pointNum() const { return pointNum_; } void setPointNum(int pn) { pointNum_ = pn; } - void computeError() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSegment2D* l2 = vertexXnRaw<1>(); - SE2 iEst = v1->estimate().inverse(); - Vector2 predP1 = iEst * l2->estimateP1(); - Vector2 predP2 = iEst * l2->estimateP2(); - Vector2 dP = predP2 - predP1; - Vector2 normal(dP.y(), -dP.x()); - normal.normalize(); - Vector3 prediction; - prediction[2] = std::atan2(normal.y(), normal.x()); - Eigen::Map pt(prediction.data()); - pt = (pointNum_ == 0) ? predP1 : predP2; - error_ = prediction - measurement_; - error_[2] = normalize_theta(error_[2]); - } - - bool setMeasurementFromState() override { - const VertexSE2* v1 = vertexXnRaw<0>(); - const VertexSegment2D* l2 = vertexXnRaw<1>(); - SE2 iEst = v1->estimate().inverse(); - Vector2 predP1 = iEst * l2->estimateP1(); - Vector2 predP2 = iEst * l2->estimateP2(); - Vector2 dP = predP2 - predP1; - Vector2 normal(dP.y(), -dP.x()); - normal.normalize(); - Vector3 prediction; - prediction[2] = std::atan2(normal.y(), normal.x()); - Eigen::Map pt(prediction.data()); - pt = (pointNum_ == 0) ? predP1 : predP2; - setMeasurement(prediction); - return true; - } + void computeError() override; - //! custom read function - bool read(std::istream& is) override; - //! custom write function - bool write(std::ostream& os) const override; + bool setMeasurementFromState() override; protected: int pointNum_ = 0; diff --git a/g2o/types/slam2d_addons/vertex_line2d.cpp b/g2o/types/slam2d_addons/vertex_line2d.cpp index 74e5c5824..d69234886 100644 --- a/g2o/types/slam2d_addons/vertex_line2d.cpp +++ b/g2o/types/slam2d_addons/vertex_line2d.cpp @@ -27,9 +27,7 @@ #include "vertex_line2d.h" #include -#include #include -#include #include "g2o/core/optimizable_graph.h" #include "g2o/types/slam2d/vertex_point_xy.h" @@ -42,16 +40,6 @@ namespace g2o { VertexLine2D::VertexLine2D() { estimate_.setZero(); } -bool VertexLine2D::read(std::istream& is) { - is >> estimate_[0] >> estimate_[1] >> p1Id >> p2Id; - return true; -} - -bool VertexLine2D::write(std::ostream& os) const { - os << estimate()(0) << " " << estimate()(1) << " " << p1Id << " " << p2Id; - return os.good(); -} - #ifdef G2O_HAVE_OPENGL VertexLine2DDrawAction::VertexLine2DDrawAction() : DrawAction(typeid(VertexLine2D).name()), pointSize_(nullptr) {} diff --git a/g2o/types/slam2d_addons/vertex_line2d.h b/g2o/types/slam2d_addons/vertex_line2d.h index 42fcbd6ef..1e7f58f10 100644 --- a/g2o/types/slam2d_addons/vertex_line2d.h +++ b/g2o/types/slam2d_addons/vertex_line2d.h @@ -27,8 +27,6 @@ #ifndef G2O_VERTEX_LINE2D_H #define G2O_VERTEX_LINE2D_H -#include -#include #include #include "g2o/config.h" @@ -58,8 +56,7 @@ class G2O_TYPES_SLAM2D_ADDONS_API VertexLine2D : public BaseVertex<2, Line2D> { estimate_(0) = normalize_theta(estimate_(0)); } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + // TODO(Rainer): below only used in visualization, currently not serialized int p1Id = -1, p2Id = -1; }; diff --git a/g2o/types/slam2d_addons/vertex_segment2d.cpp b/g2o/types/slam2d_addons/vertex_segment2d.cpp index e325f72b8..f3e4314f9 100644 --- a/g2o/types/slam2d_addons/vertex_segment2d.cpp +++ b/g2o/types/slam2d_addons/vertex_segment2d.cpp @@ -33,23 +33,9 @@ #include #include -#include "g2o/core/io_helper.h" - namespace g2o { -VertexSegment2D::VertexSegment2D() - -{ - estimate_.setZero(); -} - -bool VertexSegment2D::read(std::istream& is) { - return internal::readVector(is, estimate_); -} - -bool VertexSegment2D::write(std::ostream& os) const { - return internal::writeVector(os, estimate()); -} +VertexSegment2D::VertexSegment2D() { estimate_.setZero(); } #ifdef G2O_HAVE_OPENGL VertexSegment2DDrawAction::VertexSegment2DDrawAction() diff --git a/g2o/types/slam2d_addons/vertex_segment2d.h b/g2o/types/slam2d_addons/vertex_segment2d.h index 36bbf1907..68a4f07db 100644 --- a/g2o/types/slam2d_addons/vertex_segment2d.h +++ b/g2o/types/slam2d_addons/vertex_segment2d.h @@ -28,7 +28,6 @@ #define G2O_VERTEX_SEGMENT_2D_H #include -#include #include #include "g2o/config.h" @@ -64,9 +63,6 @@ class G2O_TYPES_SLAM2D_ADDONS_API VertexSegment2D void oplusImpl(const VectorX::MapType& update) override { estimate_ += update.head(); } - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; }; #ifdef G2O_HAVE_OPENGL diff --git a/g2o/types/slam3d/CMakeLists.txt b/g2o/types/slam3d/CMakeLists.txt index efec8abb9..70ec2253b 100644 --- a/g2o/types/slam3d/CMakeLists.txt +++ b/g2o/types/slam3d/CMakeLists.txt @@ -15,8 +15,6 @@ add_library(types_slam3d ${G2O_LIB_TYPE} vertex_pointxyz.h parameter_camera.cpp parameter_camera.h - parameter_stereo_camera.cpp - parameter_stereo_camera.h edge_se3_pointxyz.cpp edge_se3_pointxyz.h edge_se3_pointxyz_disparity.cpp @@ -35,7 +33,6 @@ add_library(types_slam3d ${G2O_LIB_TYPE} se3_ops.h edge_pointxyz.cpp edge_pointxyz.h types_slam3d.cpp - types_slam3d.h ) diff --git a/g2o/types/slam3d/edge_pointxyz.cpp b/g2o/types/slam3d/edge_pointxyz.cpp index 7d3ea4947..245f01ab6 100644 --- a/g2o/types/slam3d/edge_pointxyz.cpp +++ b/g2o/types/slam3d/edge_pointxyz.cpp @@ -26,39 +26,13 @@ #include "edge_pointxyz.h" -#include - -#include "g2o/types/slam3d/vertex_pointxyz.h" - namespace g2o { -EdgePointXYZ::EdgePointXYZ() - -{ +EdgePointXYZ::EdgePointXYZ() { information_.setIdentity(); error_.setZero(); } -bool EdgePointXYZ::read(std::istream& is) { - Vector3 p; - is >> p[0] >> p[1] >> p[2]; - setMeasurement(p); - for (int i = 0; i < 3; ++i) - for (int j = i; j < 3; ++j) { - is >> information()(i, j); - if (i != j) information()(j, i) = information()(i, j); - } - return true; -} - -bool EdgePointXYZ::write(std::ostream& os) const { - Vector3 p = measurement(); - os << p.x() << " " << p.y() << " " << p.z(); - for (int i = 0; i < 3; ++i) - for (int j = i; j < 3; ++j) os << " " << information()(i, j); - return os.good(); -} - #ifndef NUMERIC_JACOBIAN_THREE_D_TYPES void EdgePointXYZ::linearizeOplus() { jacobianOplusXi_ = -Matrix3::Identity(); diff --git a/g2o/types/slam3d/edge_pointxyz.h b/g2o/types/slam3d/edge_pointxyz.h index 83e43059b..cb2ffba72 100644 --- a/g2o/types/slam3d/edge_pointxyz.h +++ b/g2o/types/slam3d/edge_pointxyz.h @@ -28,10 +28,7 @@ #define G2O_EDGE_POINTXYZ_H #include -#include -#include -#include "g2o/config.h" #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" @@ -50,8 +47,6 @@ class G2O_TYPES_SLAM3D_API EdgePointXYZ const VertexPointXYZ* v2 = vertexXnRaw<1>(); error_ = (v2->estimate() - v1->estimate()) - measurement_; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void setMeasurement(const Vector3& m) override { measurement_ = m; } diff --git a/g2o/types/slam3d/edge_se3.cpp b/g2o/types/slam3d/edge_se3.cpp index 304577ce4..9df189c21 100644 --- a/g2o/types/slam3d/edge_se3.cpp +++ b/g2o/types/slam3d/edge_se3.cpp @@ -30,8 +30,6 @@ #include #include -#include "g2o/core/io_helper.h" -#include "g2o/stuff/property.h" #include "g2o/types/slam3d/isometry3d_mappings.h" #include "g2o/types/slam3d/vertex_se3.h" #include "isometry3d_gradients.h" @@ -45,23 +43,6 @@ namespace g2o { EdgeSE3::EdgeSE3() { information().setIdentity(); } -bool EdgeSE3::read(std::istream& is) { - Vector7 meas; - internal::readVector(is, meas); - // normalize the quaternion to recover numerical precision lost by storing as - // human readable text - Vector4::MapType(meas.data() + 3).normalize(); - setMeasurement(internal::fromVectorQT(meas)); - if (is.bad()) return false; - readInformationMatrix(is); - return is.good() || is.eof(); -} - -bool EdgeSE3::write(std::ostream& os) const { - internal::writeVector(os, internal::toVectorQT(measurement())); - return writeInformationMatrix(os); -} - void EdgeSE3::computeError() { VertexSE3* from = vertexXnRaw<0>(); VertexSE3* to = vertexXnRaw<1>(); diff --git a/g2o/types/slam3d/edge_se3.h b/g2o/types/slam3d/edge_se3.h index e2f58d6d6..cc1f83e55 100644 --- a/g2o/types/slam3d/edge_se3.h +++ b/g2o/types/slam3d/edge_se3.h @@ -28,9 +28,7 @@ #define G2O_EDGE_SE3_H_ #include -#include #include -#include #include "g2o/config.h" #include "g2o/core/base_binary_edge.h" @@ -38,7 +36,7 @@ #include "g2o/core/hyper_graph.h" #include "g2o/core/hyper_graph_action.h" #include "g2o/core/optimizable_graph.h" -#include "g2o/types/slam3d/type_traits_isometry3.h" +#include "g2o/types/slam3d/type_traits_isometry3.h" // IWYU pragma: keep #include "g2o_types_slam3d_api.h" #include "vertex_se3.h" @@ -55,8 +53,6 @@ class G2O_TYPES_SLAM3D_API EdgeSE3 : public BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3> { public: EdgeSE3(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; diff --git a/g2o/types/slam3d/edge_se3_lotsofxyz.cpp b/g2o/types/slam3d/edge_se3_lotsofxyz.cpp index a61eded49..793e10967 100644 --- a/g2o/types/slam3d/edge_se3_lotsofxyz.cpp +++ b/g2o/types/slam3d/edge_se3_lotsofxyz.cpp @@ -28,9 +28,6 @@ #include #include -#include -#include -#include #include #include "g2o/types/slam3d/vertex_pointxyz.h" @@ -45,12 +42,13 @@ bool EdgeSE3LotsOfXYZ::setMeasurementFromState() { Eigen::Transform poseinv = pose->estimate().inverse(); - for (unsigned int i = 0; i < observedPoints_; i++) { + int observed_points = measurement_.size() / 3; + for (int i = 0; i < observed_points; i++) { auto* xyz = static_cast(vertexRaw(1 + i)); // const Vector3 &pt = xyz->estimate(); Vector3 m = poseinv * xyz->estimate(); - unsigned int index = 3 * i; + const int index = 3 * i; measurement_[index] = m[0]; measurement_[index + 1] = m[1]; measurement_[index + 2] = m[2]; @@ -61,11 +59,12 @@ bool EdgeSE3LotsOfXYZ::setMeasurementFromState() { void EdgeSE3LotsOfXYZ::computeError() { auto* pose = static_cast(vertexRaw(0)); - for (unsigned int i = 0; i < observedPoints_; i++) { + int observed_points = measurement_.size() / 3; + for (int i = 0; i < observed_points; i++) { auto* xyz = static_cast(vertexRaw(1 + i)); Vector3 m = pose->estimate().inverse() * xyz->estimate(); - unsigned int index = 3 * i; + const int index = 3 * i; error_[index] = m[0] - measurement_[index]; error_[index + 1] = m[1] - measurement_[index + 1]; error_[index + 2] = m[2] - measurement_[index + 2]; @@ -115,53 +114,6 @@ void EdgeSE3LotsOfXYZ::linearizeOplus() { jacobianOplus_[0] = Ji; } -bool EdgeSE3LotsOfXYZ::read(std::istream& is) { - is >> observedPoints_; - - setSize(observedPoints_ + 1); - - // read the measurements - for (unsigned int i = 0; i < observedPoints_; i++) { - unsigned int index = 3 * i; - is >> measurement_[index] >> measurement_[index + 1] >> - measurement_[index + 2]; - } - - // read the information matrix - for (unsigned int i = 0; i < observedPoints_ * 3; i++) { - // fill the "upper triangle" part of the matrix - for (unsigned int j = i; j < observedPoints_ * 3; j++) { - is >> information()(i, j); - } - - // fill the lower triangle part - for (unsigned int j = 0; j < i; j++) { - information()(i, j) = information()(j, i); - } - } - return true; -} - -bool EdgeSE3LotsOfXYZ::write(std::ostream& os) const { - // write number of observed points - os << "|| " << observedPoints_; - - // write measurements - for (unsigned int i = 0; i < observedPoints_; i++) { - unsigned int index = 3 * i; - os << " " << measurement_[index] << " " << measurement_[index + 1] << " " - << measurement_[index + 2]; - } - - // write information matrix - for (unsigned int i = 0; i < observedPoints_ * 3; i++) { - for (unsigned int j = i; j < observedPoints_ * 3; j++) { - os << " " << information()(i, j); - } - } - return os.good(); -} - void EdgeSE3LotsOfXYZ::initialEstimate(const OptimizableGraph::VertexSet& fixed, OptimizableGraph::Vertex* toEstimate) { (void)toEstimate; @@ -171,11 +123,12 @@ void EdgeSE3LotsOfXYZ::initialEstimate(const OptimizableGraph::VertexSet& fixed, auto* pose = static_cast(vertexRaw(0)); + int observed_points = measurement_.size() / 3; #ifdef _MSC_VER - std::vector estimate_this(observedPoints_, true); + std::vector estimate_this(observed_points, true); #else - bool estimate_this[observedPoints_]; - for (unsigned int i = 0; i < observedPoints_; i++) { + bool estimate_this[observed_points]; + for (int i = 0; i < observed_points; i++) { estimate_this[i] = true; } #endif diff --git a/g2o/types/slam3d/edge_se3_lotsofxyz.h b/g2o/types/slam3d/edge_se3_lotsofxyz.h index 92216a984..9ad22639a 100644 --- a/g2o/types/slam3d/edge_se3_lotsofxyz.h +++ b/g2o/types/slam3d/edge_se3_lotsofxyz.h @@ -27,39 +27,27 @@ #ifndef G2O_SE3_LOTSOF_XYZ #define G2O_SE3_LOTSOF_XYZ -#include -#include - -#include "g2o/config.h" #include "g2o/core/base_variable_sized_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" #include "g2o_types_slam3d_api.h" -#include "vertex_pointxyz.h" -#include "vertex_se3.h" namespace g2o { class G2O_TYPES_SLAM3D_API EdgeSE3LotsOfXYZ : public BaseVariableSizedEdge<-1, VectorX> { - protected: - unsigned int observedPoints_ = 0; - public: EdgeSE3LotsOfXYZ(); - void setSize(int vertices) { - resize(vertices); - observedPoints_ = vertices - 1; - measurement_.resize(observedPoints_ * 3L, 1); - setDimension(observedPoints_ * 3); + void resize(size_t size) override { + BaseVariableSizedEdge<-1, VectorX>::resize(size); + int observed_points = size > 0 ? size - 1 : 0; + measurement_.resize(observed_points * 3L, 1); + setDimension(observed_points * 3); } void computeError() override; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - bool setMeasurementFromState() override; void initialEstimate(const OptimizableGraph::VertexSet&, diff --git a/g2o/types/slam3d/edge_se3_offset.cpp b/g2o/types/slam3d/edge_se3_offset.cpp index c155e7b8a..9af2eb9c4 100644 --- a/g2o/types/slam3d/edge_se3_offset.cpp +++ b/g2o/types/slam3d/edge_se3_offset.cpp @@ -28,11 +28,8 @@ #include #include -#include -#include "g2o/core/cache.h" #include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" #include "g2o/core/parameter.h" #include "g2o/types/slam3d/isometry3d_mappings.h" #include "g2o/types/slam3d/vertex_se3.h" @@ -59,27 +56,6 @@ bool EdgeSE3Offset::resolveCaches() { return (cacheFrom_ && cacheTo_); } -bool EdgeSE3Offset::read(std::istream& is) { - bool state = readParamIds(is); - - Vector7 meas; - state &= internal::readVector(is, meas); - // normalize the quaternion to recover numerical precision lost by storing as - // human readable text - Vector4::MapType(meas.data() + 3).normalize(); - setMeasurement(internal::fromVectorQT(meas)); - - state &= readInformationMatrix(is); - return state; -} - -bool EdgeSE3Offset::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, internal::toVectorQT(measurement_)); - writeInformationMatrix(os); - return os.good(); -} - void EdgeSE3Offset::computeError() { Isometry3 delta = inverseMeasurement_ * cacheFrom_->w2n() * cacheTo_->n2w(); error_ = internal::toVectorMQT(delta); @@ -100,8 +76,8 @@ void EdgeSE3Offset::linearizeOplus() { Isometry3 E; const Isometry3& Xi = from->estimate(); const Isometry3& Xj = to->estimate(); - const Isometry3& Pi = cacheFrom_->offsetParam()->offset(); - const Isometry3& Pj = cacheTo_->offsetParam()->offset(); + const Isometry3& Pi = cacheFrom_->offsetParam()->param(); + const Isometry3& Pj = cacheTo_->offsetParam()->param(); const Isometry3& Z = measurement_; internal::computeEdgeSE3Gradient(E, jacobianOplusXi_, jacobianOplusXj_, Z, Xi, Xj, Pi, Pj); @@ -112,9 +88,9 @@ void EdgeSE3Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, VertexSE3* from = vertexXnRaw<0>(); VertexSE3* to = vertexXnRaw<1>(); - Isometry3 virtualMeasurement = cacheFrom_->offsetParam()->offset() * + Isometry3 virtualMeasurement = cacheFrom_->offsetParam()->param() * measurement() * - cacheTo_->offsetParam()->offset().inverse(); + cacheTo_->offsetParam()->param().inverse(); if (from_.count(vertexXn<0>()) > 0) { to->setEstimate(from->estimate() * virtualMeasurement); diff --git a/g2o/types/slam3d/edge_se3_offset.h b/g2o/types/slam3d/edge_se3_offset.h index 22bbe0d42..7c5893513 100644 --- a/g2o/types/slam3d/edge_se3_offset.h +++ b/g2o/types/slam3d/edge_se3_offset.h @@ -27,7 +27,6 @@ #ifndef G2O_EDGE_SE3_OFFSET_H_ #define G2O_EDGE_SE3_OFFSET_H_ -#include #include #include "edge_se3.h" @@ -44,8 +43,6 @@ class CacheSE3Offset; class G2O_TYPES_SLAM3D_API EdgeSE3Offset : public EdgeSE3 { public: EdgeSE3Offset(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; diff --git a/g2o/types/slam3d/edge_se3_pointxyz.cpp b/g2o/types/slam3d/edge_se3_pointxyz.cpp index c69ceedc5..db98233e9 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz.cpp +++ b/g2o/types/slam3d/edge_se3_pointxyz.cpp @@ -26,11 +26,8 @@ #include "edge_se3_pointxyz.h" -#include "g2o/core/cache.h" #include "g2o/core/eigen_types.h" -#include "g2o/core/io_helper.h" #include "g2o/core/parameter.h" -#include "g2o/stuff/property.h" #include "g2o/types/slam3d/vertex_pointxyz.h" #include "g2o/types/slam3d/vertex_se3.h" #include "parameter_se3_offset.h" @@ -42,7 +39,6 @@ #include #include -#include #include #include @@ -62,22 +58,6 @@ bool EdgeSE3PointXYZ::resolveCaches() { return cache_ != nullptr; } -bool EdgeSE3PointXYZ::read(std::istream& is) { - readParamIds(is); - Vector3 meas; - internal::readVector(is, meas); - setMeasurement(meas); - readInformationMatrix(is); - return is.good() || is.eof(); -} - -bool EdgeSE3PointXYZ::write(std::ostream& os) const { - bool state = writeParamIds(os); - state &= internal::writeVector(os, measurement()); - state &= writeInformationMatrix(os); - return state; -} - void EdgeSE3PointXYZ::computeError() { // from cam to point (track) // VertexSE3 *cam = static_cast(vertices_[0]); @@ -145,7 +125,7 @@ void EdgeSE3PointXYZ::initialEstimate(const OptimizableGraph::VertexSet& from, VertexSE3* cam = vertexXnRaw<0>(); VertexPointXYZ* point = vertexXnRaw<1>(); const Vector3 p = measurement_; - point->setEstimate(cam->estimate() * (cache_->offsetParam()->offset() * p)); + point->setEstimate(cam->estimate() * (cache_->offsetParam()->param() * p)); } #ifdef G2O_HAVE_OPENGL @@ -167,7 +147,7 @@ bool EdgeSE3PointXYZDrawAction::operator()( if (!fromEdge || !toEdge) return true; ParameterSE3Offset* offsetParam = static_cast(e->parameter(0).get()); - Isometry3 fromTransform = fromEdge->estimate() * offsetParam->offset(); + Isometry3 fromTransform = fromEdge->estimate() * offsetParam->param(); glColor3f(LANDMARK_EDGE_COLOR); glPushAttrib(GL_ENABLE_BIT); glDisable(GL_LIGHTING); diff --git a/g2o/types/slam3d/edge_se3_pointxyz.h b/g2o/types/slam3d/edge_se3_pointxyz.h index c5b681454..f9d99be6d 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz.h +++ b/g2o/types/slam3d/edge_se3_pointxyz.h @@ -28,9 +28,7 @@ #define G2O_EDGE_SE3_POINT_XYZ_H_ #include -#include #include -#include #include "g2o/config.h" #include "g2o/core/base_binary_edge.h" @@ -54,8 +52,6 @@ class G2O_TYPES_SLAM3D_API EdgeSE3PointXYZ : public BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ> { public: EdgeSE3PointXYZ(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; // return the error estimate as a 3-vector void computeError() override; diff --git a/g2o/types/slam3d/edge_se3_pointxyz_depth.cpp b/g2o/types/slam3d/edge_se3_pointxyz_depth.cpp index b72cf8ca4..d161a9ecc 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz_depth.cpp +++ b/g2o/types/slam3d/edge_se3_pointxyz_depth.cpp @@ -30,8 +30,6 @@ #include #include -#include "g2o/core/cache.h" -#include "g2o/core/io_helper.h" #include "g2o/core/parameter.h" #include "g2o/types/slam3d/parameter_camera.h" #include "g2o/types/slam3d/vertex_pointxyz.h" @@ -54,18 +52,6 @@ bool EdgeSE3PointXYZDepth::resolveCaches() { return cache_ != nullptr; } -bool EdgeSE3PointXYZDepth::read(std::istream& is) { - readParamIds(is); - internal::readVector(is, measurement_); // measured keypoint - return readInformationMatrix(is); -} - -bool EdgeSE3PointXYZDepth::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE3PointXYZDepth::computeError() { // from cam to point (track) // VertexSE3 *cam = static_cast(vertices_[0]); @@ -108,7 +94,7 @@ void EdgeSE3PointXYZDepth::linearizeOplus() { Jprime.block<3, 3>(0, 6) = cache_->w2l().rotation(); - Jprime = cache_->camParams()->Kcam_inverseOffsetR() * Jprime; + Jprime = cache_->camParams()->param().KcamInverseOffsetR() * Jprime; Vector3 Zprime = cache_->w2i() * pt; JacType Jhom; @@ -145,13 +131,13 @@ void EdgeSE3PointXYZDepth::initialEstimate( VertexSE3* cam = vertexXnRaw<0>(); VertexPointXYZ* point = vertexXnRaw<1>(); - const Eigen::Matrix& invKcam = - cache_->camParams()->invKcam(); + const Matrix3& invKcam = cache_->camParams()->param().invKcam(); Vector3 p; p(2) = measurement_(2); p.head<2>() = measurement_.head<2>() * p(2); p = invKcam * p; - point->setEstimate(cam->estimate() * (cache_->camParams()->offset() * p)); + point->setEstimate(cam->estimate() * + (cache_->camParams()->param().offset() * p)); } } // namespace g2o diff --git a/g2o/types/slam3d/edge_se3_pointxyz_depth.h b/g2o/types/slam3d/edge_se3_pointxyz_depth.h index dfc81c3c0..9ddeba205 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz_depth.h +++ b/g2o/types/slam3d/edge_se3_pointxyz_depth.h @@ -27,9 +27,7 @@ #ifndef G2O_EDGE_PROJECT_DEPTH_H_ #define G2O_EDGE_PROJECT_DEPTH_H_ -#include #include -#include #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" @@ -50,8 +48,6 @@ class G2O_TYPES_SLAM3D_API EdgeSE3PointXYZDepth : public BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ> { public: EdgeSE3PointXYZDepth(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; // return the error estimate as a 3-vector void computeError() override; diff --git a/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp b/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp index fe59408a8..9a405ad6c 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp +++ b/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp @@ -32,10 +32,7 @@ #include #include -#include "g2o/core/cache.h" -#include "g2o/core/io_helper.h" #include "g2o/core/parameter.h" -#include "g2o/stuff/property.h" #include "g2o/types/slam3d/parameter_camera.h" #include "g2o/types/slam3d/vertex_pointxyz.h" #include "g2o/types/slam3d/vertex_se3.h" @@ -62,18 +59,6 @@ bool EdgeSE3PointXYZDisparity::resolveCaches() { return cache_ != nullptr; } -bool EdgeSE3PointXYZDisparity::read(std::istream& is) { - readParamIds(is); - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeSE3PointXYZDisparity::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE3PointXYZDisparity::computeError() { // VertexSE3 *cam = static_cast(vertices_[0]); VertexPointXYZ* point = vertexXnRaw<1>(); @@ -120,7 +105,7 @@ void EdgeSE3PointXYZDisparity::linearizeOplus() { // Eigen::Matrix Jprime = // vcache->params->Kcam_inverseOffsetR * J; - Jprime = cache_->camParams()->Kcam_inverseOffsetR() * Jprime; + Jprime = cache_->camParams()->param().KcamInverseOffsetR() * Jprime; JacType Jhom; Vector3 Zprime = cache_->w2i() * pt; @@ -161,14 +146,13 @@ void EdgeSE3PointXYZDisparity::initialEstimate( VertexSE3* cam = vertexXnRaw<0>(); VertexPointXYZ* point = vertexXnRaw<1>(); - const Eigen::Matrix& invKcam = - cache_->camParams()->invKcam(); + const Matrix3& invKcam = cache_->camParams()->param().invKcam(); Vector3 p; double w = 1. / measurement_(2); p.head<2>() = measurement_.head<2>() * w; p(2) = w; p = invKcam * p; - p = cam->estimate() * (cache_->camParams()->offset() * p); + p = cam->estimate() * (cache_->camParams()->param().offset() * p); point->setEstimate(p); } @@ -190,7 +174,7 @@ bool EdgeProjectDisparityDrawAction::operator()( if (!fromEdge || !toEdge) return true; ParameterCamera* camParam = static_cast(e->parameter(0).get()); - Isometry3 fromTransform = fromEdge->estimate() * camParam->offset(); + Isometry3 fromTransform = fromEdge->estimate() * camParam->param().offset(); glColor3f(LANDMARK_EDGE_COLOR); glPushAttrib(GL_ENABLE_BIT); glDisable(GL_LIGHTING); diff --git a/g2o/types/slam3d/edge_se3_pointxyz_disparity.h b/g2o/types/slam3d/edge_se3_pointxyz_disparity.h index fce729cac..b11f29b66 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz_disparity.h +++ b/g2o/types/slam3d/edge_se3_pointxyz_disparity.h @@ -27,9 +27,7 @@ #ifndef G2O_EDGE_SE3_POINTXYZ_DISPARITY_H_ #define G2O_EDGE_SE3_POINTXYZ_DISPARITY_H_ -#include #include -#include #include "g2o/config.h" #include "g2o/core/base_binary_edge.h" @@ -59,8 +57,6 @@ class G2O_TYPES_SLAM3D_API EdgeSE3PointXYZDisparity : public BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ> { public: EdgeSE3PointXYZDisparity(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; // return the error estimate as a 3-vector void computeError() override; diff --git a/g2o/types/slam3d/edge_se3_prior.cpp b/g2o/types/slam3d/edge_se3_prior.cpp index 9ee166d46..bdc126758 100644 --- a/g2o/types/slam3d/edge_se3_prior.cpp +++ b/g2o/types/slam3d/edge_se3_prior.cpp @@ -28,10 +28,7 @@ #include #include -#include -#include "g2o/core/cache.h" -#include "g2o/core/io_helper.h" #include "g2o/core/parameter.h" #include "g2o/types/slam3d/isometry3d_mappings.h" #include "g2o/types/slam3d/parameter_se3_offset.h" @@ -55,22 +52,6 @@ bool EdgeSE3Prior::resolveCaches() { return cache_ != nullptr; } -bool EdgeSE3Prior::read(std::istream& is) { - bool state = readParamIds(is); - Vector7 meas; - state &= internal::readVector(is, meas); - setMeasurement(internal::fromVectorQT(meas)); - state &= readInformationMatrix(is); - return state; -} - -bool EdgeSE3Prior::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, internal::toVectorQT(measurement())); - writeInformationMatrix(os); - return os.good(); -} - void EdgeSE3Prior::computeError() { Isometry3 delta = inverseMeasurement_ * cache_->n2w(); error_ = internal::toVectorMQT(delta); @@ -83,7 +64,7 @@ void EdgeSE3Prior::linearizeOplus() { Isometry3 X; Isometry3 P; X = from->estimate(); - P = cache_->offsetParam()->offset(); + P = cache_->offsetParam()->param(); Z = measurement_; internal::computeEdgeSE3PriorGradient(E, jacobianOplusXi_, Z, X, P); } @@ -99,7 +80,7 @@ void EdgeSE3Prior::initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, assert(v && "Vertex for the Prior edge is not set"); Isometry3 newEstimate = - cache_->offsetParam()->offset().inverse() * measurement(); + cache_->offsetParam()->param().inverse() * measurement(); // do not set translation, as that part of the information is all zero if (information_.block<3, 3>(0, 0).array().abs().sum() == 0) { newEstimate.translation() = v->estimate().translation(); diff --git a/g2o/types/slam3d/edge_se3_prior.h b/g2o/types/slam3d/edge_se3_prior.h index f2606cc46..24b847ed2 100644 --- a/g2o/types/slam3d/edge_se3_prior.h +++ b/g2o/types/slam3d/edge_se3_prior.h @@ -28,14 +28,12 @@ #define G2O_EDGE_SE3_PRIOR_H_ #include -#include #include -#include #include "g2o/core/base_unary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" -#include "g2o/types/slam3d/type_traits_isometry3.h" +#include "g2o/types/slam3d/type_traits_isometry3.h" // IWYU pragma: keep #include "g2o_types_slam3d_api.h" #include "parameter_se3_offset.h" #include "vertex_se3.h" @@ -53,8 +51,6 @@ class G2O_TYPES_SLAM3D_API EdgeSE3Prior : public BaseUnaryEdge<6, Isometry3, VertexSE3> { public: EdgeSE3Prior(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; // return the error estimate as a 3-vector void computeError() override; diff --git a/g2o/types/slam3d/edge_se3_xyzprior.cpp b/g2o/types/slam3d/edge_se3_xyzprior.cpp index dde886d46..8d7a9c081 100644 --- a/g2o/types/slam3d/edge_se3_xyzprior.cpp +++ b/g2o/types/slam3d/edge_se3_xyzprior.cpp @@ -30,8 +30,6 @@ #include #include -#include "g2o/core/cache.h" -#include "g2o/core/io_helper.h" #include "g2o/core/parameter.h" #include "g2o/types/slam3d/parameter_se3_offset.h" #include "g2o/types/slam3d/vertex_se3.h" @@ -52,18 +50,6 @@ bool EdgeSE3XYZPrior::resolveCaches() { return cache_ != nullptr; } -bool EdgeSE3XYZPrior::read(std::istream& is) { - readParamIds(is); - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeSE3XYZPrior::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE3XYZPrior::computeError() { const VertexSE3* v = vertexXnRaw<0>(); error_ = v->estimate().translation() - measurement_; @@ -87,7 +73,7 @@ void EdgeSE3XYZPrior::initialEstimate( VertexSE3* v = vertexXnRaw<0>(); assert(v && "Vertex for the Prior edge is not set"); - Isometry3 newEstimate = cache_->offsetParam()->offset().inverse() * + Isometry3 newEstimate = cache_->offsetParam()->param().inverse() * Eigen::Translation3d(measurement()); if (information_.block<3, 3>(0, 0).array().abs().sum() == 0) { // do not set translation, as that part of the information is all diff --git a/g2o/types/slam3d/edge_se3_xyzprior.h b/g2o/types/slam3d/edge_se3_xyzprior.h index de7dc5707..12a596245 100644 --- a/g2o/types/slam3d/edge_se3_xyzprior.h +++ b/g2o/types/slam3d/edge_se3_xyzprior.h @@ -27,9 +27,7 @@ #ifndef G2O_EDGE_SE3_PRIOR_XYZ_H #define G2O_EDGE_SE3_PRIOR_XYZ_H -#include #include -#include #include "g2o/core/base_unary_edge.h" #include "g2o/core/eigen_types.h" @@ -49,8 +47,6 @@ class G2O_TYPES_SLAM3D_API EdgeSE3XYZPrior public: EdgeSE3XYZPrior(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; void linearizeOplus() override; bool setMeasurementFromState() override; diff --git a/g2o/types/slam3d/edge_xyz_prior.cpp b/g2o/types/slam3d/edge_xyz_prior.cpp index d01d3b7ab..87e3ac400 100644 --- a/g2o/types/slam3d/edge_xyz_prior.cpp +++ b/g2o/types/slam3d/edge_xyz_prior.cpp @@ -27,25 +27,13 @@ #include "edge_xyz_prior.h" #include -#include -#include "g2o/core/io_helper.h" #include "g2o/types/slam3d/vertex_pointxyz.h" namespace g2o { EdgeXYZPrior::EdgeXYZPrior() { information().setIdentity(); } -bool EdgeXYZPrior::read(std::istream& is) { - internal::readVector(is, measurement_); - return readInformationMatrix(is); -} - -bool EdgeXYZPrior::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeXYZPrior::computeError() { const VertexPointXYZ* v = vertexXnRaw<0>(); error_ = v->estimate() - measurement_; diff --git a/g2o/types/slam3d/edge_xyz_prior.h b/g2o/types/slam3d/edge_xyz_prior.h index 754898ed1..c02d31936 100644 --- a/g2o/types/slam3d/edge_xyz_prior.h +++ b/g2o/types/slam3d/edge_xyz_prior.h @@ -27,9 +27,6 @@ #ifndef G2O_EDGE_XYZ_PRIOR_H_ #define G2O_EDGE_XYZ_PRIOR_H_ -#include -#include - #include "g2o/core/base_unary_edge.h" #include "g2o/core/eigen_types.h" #include "g2o/core/optimizable_graph.h" @@ -48,8 +45,6 @@ class G2O_TYPES_SLAM3D_API EdgeXYZPrior : public BaseUnaryEdge<3, Vector3, VertexPointXYZ> { public: EdgeXYZPrior(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void computeError() override; diff --git a/g2o/types/slam3d/parameter_camera.cpp b/g2o/types/slam3d/parameter_camera.cpp index e0a988210..fd5b57458 100644 --- a/g2o/types/slam3d/parameter_camera.cpp +++ b/g2o/types/slam3d/parameter_camera.cpp @@ -26,16 +26,15 @@ #include "parameter_camera.h" +#include + #include #include #include -#include #include #include -#include "g2o/core/io_helper.h" -#include "g2o/types/slam3d/parameter_se3_offset.h" -#include "isometry3d_mappings.h" +#include "g2o/types/slam3d/vertex_se3.h" #ifdef G2O_HAVE_OPENGL #include "g2o/stuff/opengl_primitives.h" @@ -44,61 +43,25 @@ namespace g2o { -ParameterCamera::ParameterCamera() { - setId(-1); - setKcam(1, 1, 0.5, 0.5); - setOffset(); -} - -void ParameterCamera::setOffset(const Isometry3& offset_) { - ParameterSE3Offset::setOffset(offset_); - Kcam_inverseOffsetR_ = Kcam_ * inverseOffset().rotation(); -} - -void ParameterCamera::setKcam(double fx, double fy, double cx, double cy) { - Kcam_.setZero(); - Kcam_(0, 0) = fx; - Kcam_(1, 1) = fy; - Kcam_(0, 2) = cx; - Kcam_(1, 2) = cy; - Kcam_(2, 2) = 1.0; - invKcam_ = Kcam_.inverse(); - Kcam_inverseOffsetR_ = Kcam_ * inverseOffset().rotation(); -} +void ParameterCamera::update() {} -bool ParameterCamera::read(std::istream& is) { - Vector7 off; - internal::readVector(is, off); - // normalize the quaternion to recover numerical precision lost by storing as - // human readable text - Vector4::MapType(off.data() + 3).normalize(); - setOffset(internal::fromVectorQT(off)); - double fx; - double fy; - double cx; - double cy; - is >> fx >> fy >> cx >> cy; - setKcam(fx, fy, cx, cy); - return is.good(); -} +void CacheCamera::updateImpl() { +#ifndef NDEBUG + auto* offsetParam = dynamic_cast(parameters_[0].get()); +#else + auto* offsetParam = static_cast(parameters_[0].get()); +#endif -bool ParameterCamera::write(std::ostream& os) const { - internal::writeVector(os, internal::toVectorQT(offset_)); - os << Kcam_(0, 0) << " "; - os << Kcam_(1, 1) << " "; - os << Kcam_(0, 2) << " "; - os << Kcam_(1, 2) << " "; - return os.good(); -} + const auto& v = static_cast(vertex()); + n2w_ = v.estimate() * offsetParam->param().offset(); + w2n_ = n2w_.inverse(); + w2l_ = v.estimate().inverse(); -void CacheCamera::updateImpl() { - CacheSE3Offset::updateImpl(); w2i_.matrix().topLeftCorner<3, 4>() = - camParams()->Kcam() * w2n().matrix().topLeftCorner<3, 4>(); + offsetParam->param().Kcam() * w2n().matrix().topLeftCorner<3, 4>(); } #ifdef G2O_HAVE_OPENGL - CacheCameraDrawAction::CacheCameraDrawAction() : DrawAction(typeid(CacheCamera).name()) {} @@ -128,10 +91,12 @@ bool CacheCameraDrawAction::operator()( if (show_ && !show_->value()) return true; + auto* offsetParam = + static_cast(that->parameters()[0].get()); glPushAttrib(GL_COLOR); glColor3f(POSE_PARAMETER_COLOR); glPushMatrix(); - glMultMatrixd(that->camParams()->offset().cast().data()); + glMultMatrixd(offsetParam->param().offset().data()); glRotatef(180.0F, 0.0F, 1.0F, 0.0F); opengl::drawPyramid(cameraSide_->value(), cameraZ_->value()); glPopMatrix(); diff --git a/g2o/types/slam3d/parameter_camera.h b/g2o/types/slam3d/parameter_camera.h index 2ec870b55..9735b6735 100644 --- a/g2o/types/slam3d/parameter_camera.h +++ b/g2o/types/slam3d/parameter_camera.h @@ -27,54 +27,142 @@ #ifndef G2O_CAMERA_PARAMETERS_H_ #define G2O_CAMERA_PARAMETERS_H_ -#include #include +#include #include "g2o/config.h" +#include "g2o/core/cache.h" #include "g2o/core/eigen_types.h" #include "g2o/core/hyper_graph.h" #include "g2o/core/hyper_graph_action.h" +#include "g2o/core/parameter.h" +#include "g2o/core/type_traits.h" #include "g2o/stuff/property.h" #include "g2o_types_slam3d_api.h" -#include "parameter_se3_offset.h" +#include "type_traits_isometry3.h" namespace g2o { -/** - * \brief parameters for a camera - */ -class G2O_TYPES_SLAM3D_API ParameterCamera : public ParameterSE3Offset { + +class CameraWithOffset { public: - ParameterCamera(); - void setKcam(double fx, double fy, double cx, double cy); - void setOffset(const Isometry3& offset_ = Isometry3::Identity()); + CameraWithOffset() : offset_(Isometry3::Identity()) { + setKcam(1, 1, 0.5, 0.5); + } + + CameraWithOffset(Isometry3 offset, double fx, double fy, double cx, double cy) + : offset_(std::move(offset)) { + setKcam(fx, fy, cx, cy); + } + + [[nodiscard]] const Matrix3& Kcam() const { return Kcam_; } + void setKcam(double fx, double fy, double cx, double cy) { + Kcam_.setZero(); + Kcam_(0, 0) = fx; + Kcam_(1, 1) = fy; + Kcam_(0, 2) = cx; + Kcam_(1, 2) = cy; + Kcam_(2, 2) = 1.0; + update(); + } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; + [[nodiscard]] const Isometry3& offset() const { return offset_; } + [[nodiscard]] Isometry3& offset() { return offset_; } + void setOffset(const Isometry3& offset) { + offset_ = offset; + update(); + } - const Matrix3& Kcam() const { return Kcam_; } - const Matrix3& invKcam() const { return invKcam_; } - const Matrix3& Kcam_inverseOffsetR() const { return Kcam_inverseOffsetR_; } + [[nodiscard]] const Matrix3& invKcam() const { return invKcam_; } + [[nodiscard]] const Matrix3& KcamInverseOffsetR() const { + return Kcam_inverseOffsetR_; + } protected: + virtual void update() { + invKcam_ = Kcam_.inverse(); + Kcam_inverseOffsetR_ = Kcam_ * offset_.inverse().rotation(); + } + + Isometry3 offset_; Matrix3 Kcam_; Matrix3 invKcam_; Matrix3 Kcam_inverseOffsetR_; }; -class G2O_TYPES_SLAM3D_API CacheCamera : public CacheSE3Offset { +template <> +struct TypeTraits { + enum { + kVectorDimension = 11, + kMinimalVectorDimension = 11, + kIsVector = 0, + kIsScalar = 0, + }; + using Type = CameraWithOffset; + using VectorType = VectorN; + using MinimalVectorType = VectorN; + + static VectorType toVector(const Type& t) { + VectorType result; + result << TypeTraits::toVector(t.offset()), t.Kcam()(0, 0), + t.Kcam()(1, 1), t.Kcam()(0, 2), t.Kcam()(1, 2); + return result; + } + static void toData(const Type& t, double* data) { + typename VectorType::MapType v(data, kVectorDimension); + v = toVector(t); + } + + static MinimalVectorType toMinimalVector(const Type& t) { + return toVector(t); + } + static void toMinimalData(const Type& t, double* data) { toData(t, data); } + + template + static Type fromVector(const Eigen::DenseBase& v) { + Isometry3 offset = TypeTraits::fromVector(v.template head<7>()); + Vector4 cam_params = v.template tail<4>(); + return Type(offset, cam_params(0), cam_params(1), cam_params(2), + cam_params(3)); + } + + template + static Type fromMinimalVector(const Eigen::DenseBase& v) { + return fromVector(v); + } +}; + +/** + * \brief parameters for a camera + */ +class G2O_TYPES_SLAM3D_API ParameterCamera + : public BaseParameter { + public: + ParameterCamera() = default; + + void update() override; +}; + +class G2O_TYPES_SLAM3D_API CacheCamera : public Cache { public: using ParameterType = ParameterCamera; //! parameters of the camera - std::shared_ptr camParams() const { - return std::static_pointer_cast(parameters_[0]); + [[nodiscard]] const ParameterType* camParams() const { + return static_cast(parameters_[0].get()); } + [[nodiscard]] const Isometry3& w2n() const { return w2n_; } + [[nodiscard]] const Isometry3& n2w() const { return n2w_; } + [[nodiscard]] const Isometry3& w2l() const { return w2l_; } + //! return the world to image transform - const Affine3& w2i() const { return w2i_; } + [[nodiscard]] const Affine3& w2i() const { return w2i_; } protected: void updateImpl() override; + Isometry3 w2n_; + Isometry3 n2w_; + Isometry3 w2l_; Affine3 w2i_; ///< world to image transform }; diff --git a/g2o/types/slam3d/parameter_se3_offset.cpp b/g2o/types/slam3d/parameter_se3_offset.cpp index 911d93492..a7b156a93 100644 --- a/g2o/types/slam3d/parameter_se3_offset.cpp +++ b/g2o/types/slam3d/parameter_se3_offset.cpp @@ -30,8 +30,7 @@ #include #include -#include "g2o/core/io_helper.h" -#include "g2o/types/slam3d/isometry3d_mappings.h" +#include "g2o/core/eigen_types.h" #include "vertex_se3.h" #ifdef G2O_HAVE_OPENGL @@ -41,26 +40,9 @@ namespace g2o { -ParameterSE3Offset::ParameterSE3Offset() { setOffset(); } +ParameterSE3Offset::ParameterSE3Offset() { setParam(Isometry3::Identity()); } -void ParameterSE3Offset::setOffset(const Isometry3& offset) { - offset_ = offset; - inverseOffset_ = offset_.inverse(); -} - -bool ParameterSE3Offset::read(std::istream& is) { - Vector7 off; - bool state = internal::readVector(is, off); - // normalize the quaternion to recover numerical precision lost by storing as - // human readable text - Vector4::MapType(off.data() + 3).normalize(); - setOffset(internal::fromVectorQT(off)); - return state; -} - -bool ParameterSE3Offset::write(std::ostream& os) const { - return internal::writeVector(os, internal::toVectorQT(offset_)); -} +void ParameterSE3Offset::update() { inverseOffset_ = parameter_.inverse(); } void CacheSE3Offset::updateImpl() { #ifndef NDEBUG @@ -70,7 +52,7 @@ void CacheSE3Offset::updateImpl() { #endif const auto& v = static_cast(vertex()); - n2w_ = v.estimate() * offsetParam->offset(); + n2w_ = v.estimate() * offsetParam->param(); w2n_ = n2w_.inverse(); w2l_ = v.estimate().inverse(); } @@ -104,7 +86,7 @@ bool CacheSE3OffsetDrawAction::operator()( glPushAttrib(GL_COLOR); glColor3f(POSE_PARAMETER_COLOR); glPushMatrix(); - glMultMatrixd(that->offsetParam()->offset().cast().data()); + glMultMatrixd(that->offsetParam()->param().cast().data()); opengl::drawBox(cs, cs, cs); glPopMatrix(); glPopAttrib(); diff --git a/g2o/types/slam3d/parameter_se3_offset.h b/g2o/types/slam3d/parameter_se3_offset.h index 37918f51d..f569d4087 100644 --- a/g2o/types/slam3d/parameter_se3_offset.h +++ b/g2o/types/slam3d/parameter_se3_offset.h @@ -27,7 +27,6 @@ #ifndef G2O_VERTEX_SE3_OFFSET_PARAMETERS_H_ #define G2O_VERTEX_SE3_OFFSET_PARAMETERS_H_ -#include #include #include "g2o/config.h" @@ -38,34 +37,31 @@ #include "g2o/core/parameter.h" #include "g2o/stuff/property.h" #include "g2o_types_slam3d_api.h" +#include "type_traits_isometry3.h" // IWYU pragma: keep namespace g2o { /** * \brief offset for an SE3 */ -class G2O_TYPES_SLAM3D_API ParameterSE3Offset : public Parameter { +class G2O_TYPES_SLAM3D_API ParameterSE3Offset + : public BaseParameter { public: ParameterSE3Offset(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - /** * update the offset to a new value. * re-calculates the different representations, e.g., the rotation matrix */ - void setOffset(const Isometry3& offset_ = Isometry3::Identity()); - - //! rotation of the offset as 3x3 rotation matrix - const Isometry3& offset() const { return offset_; } + void update() override; //! rotation of the inverse offset as 3x3 rotation matrix - const Isometry3& inverseOffset() const { return inverseOffset_; } + [[nodiscard]] const Isometry3& inverseOffset() const { + return inverseOffset_; + } protected: - Isometry3 offset_; - Isometry3 inverseOffset_; + ParameterType inverseOffset_; }; /** @@ -77,14 +73,14 @@ class G2O_TYPES_SLAM3D_API CacheSE3Offset : public Cache { void updateImpl() override; - std::shared_ptr offsetParam() const { + [[nodiscard]] std::shared_ptr offsetParam() const { return std::static_pointer_cast(parameters_[0]); } void setOffsetParam(ParameterSE3Offset* offsetParam); - const Isometry3& w2n() const { return w2n_; } - const Isometry3& n2w() const { return n2w_; } - const Isometry3& w2l() const { return w2l_; } + [[nodiscard]] const Isometry3& w2n() const { return w2n_; } + [[nodiscard]] const Isometry3& n2w() const { return n2w_; } + [[nodiscard]] const Isometry3& w2l() const { return w2l_; } protected: Isometry3 w2n_; diff --git a/g2o/types/slam3d/se3quat.h b/g2o/types/slam3d/se3quat.h index 1fc34a908..688d56541 100644 --- a/g2o/types/slam3d/se3quat.h +++ b/g2o/types/slam3d/se3quat.h @@ -269,7 +269,7 @@ class G2O_TYPES_SLAM3D_API SE3Quat { }; inline std::ostream& operator<<(std::ostream& out_str, const SE3Quat& se3) { - out_str << se3.to_homogeneous_matrix() << std::endl; + out_str << se3.to_homogeneous_matrix() << '\n'; return out_str; } @@ -315,8 +315,6 @@ struct TypeTraits { res.fromMinimalVector(v); return res; } - - static Type Identity() { return Type(); } }; } // namespace g2o diff --git a/g2o/types/slam3d/type_traits_isometry3.h b/g2o/types/slam3d/type_traits_isometry3.h index ba8ae05a2..abbd680ea 100644 --- a/g2o/types/slam3d/type_traits_isometry3.h +++ b/g2o/types/slam3d/type_traits_isometry3.h @@ -71,8 +71,6 @@ struct TypeTraits { static Type fromMinimalVector(const Eigen::DenseBase& v) { return internal::fromVectorMQT(v); } - - static Type Identity() { return Type::Identity(); } }; } // namespace g2o diff --git a/g2o/types/slam3d/types_slam3d.cpp b/g2o/types/slam3d/types_slam3d.cpp index 1b7a272d9..41b7ecb65 100644 --- a/g2o/types/slam3d/types_slam3d.cpp +++ b/g2o/types/slam3d/types_slam3d.cpp @@ -24,10 +24,6 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include "types_slam3d.h" - -#include - #include "g2o/config.h" #include "g2o/core/factory.h" #include "g2o/core/hyper_graph_action.h" @@ -43,7 +39,6 @@ #include "g2o/types/slam3d/edge_xyz_prior.h" #include "g2o/types/slam3d/parameter_camera.h" #include "g2o/types/slam3d/parameter_se3_offset.h" -#include "g2o/types/slam3d/parameter_stereo_camera.h" #include "g2o/types/slam3d/vertex_pointxyz.h" #include "g2o/types/slam3d/vertex_se3.h" @@ -66,7 +61,6 @@ G2O_REGISTER_TYPE(CACHE_SE3_OFFSET, CacheSE3Offset); G2O_REGISTER_TYPE(EDGE_SE3_OFFSET, EdgeSE3Offset); G2O_REGISTER_TYPE(PARAMS_CAMERACALIB, ParameterCamera); -G2O_REGISTER_TYPE(PARAMS_STEREOCAMERACALIB, ParameterStereoCamera); G2O_REGISTER_TYPE(CACHE_CAMERA, CacheCamera); G2O_REGISTER_TYPE(EDGE_PROJECT_DISPARITY, EdgeSE3PointXYZDisparity); G2O_REGISTER_TYPE(EDGE_PROJECT_DEPTH, EdgeSE3PointXYZDepth); diff --git a/g2o/types/slam3d/vertex_pointxyz.cpp b/g2o/types/slam3d/vertex_pointxyz.cpp index 4dd2c3889..b99f162f4 100644 --- a/g2o/types/slam3d/vertex_pointxyz.cpp +++ b/g2o/types/slam3d/vertex_pointxyz.cpp @@ -32,20 +32,9 @@ #endif #include -#include - -#include "g2o/core/io_helper.h" namespace g2o { -bool VertexPointXYZ::read(std::istream& is) { - return internal::readVector(is, estimate_); -} - -bool VertexPointXYZ::write(std::ostream& os) const { - return internal::writeVector(os, estimate()); -} - #ifdef G2O_HAVE_OPENGL VertexPointXYZDrawAction::VertexPointXYZDrawAction() : DrawAction(typeid(VertexPointXYZ).name()), pointSize_(nullptr) {} diff --git a/g2o/types/slam3d/vertex_pointxyz.h b/g2o/types/slam3d/vertex_pointxyz.h index f8718223b..a5a8738f6 100644 --- a/g2o/types/slam3d/vertex_pointxyz.h +++ b/g2o/types/slam3d/vertex_pointxyz.h @@ -27,8 +27,6 @@ #ifndef G2O_VERTEX_TRACKXYZ_H_ #define G2O_VERTEX_TRACKXYZ_H_ -#include -#include #include #include "g2o/config.h" @@ -46,8 +44,6 @@ namespace g2o { class G2O_TYPES_SLAM3D_API VertexPointXYZ : public BaseVertex<3, Vector3> { public: VertexPointXYZ() = default; - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void oplusImpl(const VectorX::MapType& update) override { estimate_ += update.head(); diff --git a/g2o/types/slam3d/vertex_se3.cpp b/g2o/types/slam3d/vertex_se3.cpp index 9546cd1d5..343a278ca 100644 --- a/g2o/types/slam3d/vertex_se3.cpp +++ b/g2o/types/slam3d/vertex_se3.cpp @@ -29,9 +29,7 @@ #include #include #include -#include -#include "g2o/core/io_helper.h" #include "g2o/types/slam3d/isometry3d_mappings.h" #ifdef G2O_HAVE_OPENGL #include "g2o/stuff/opengl_primitives.h" @@ -46,21 +44,10 @@ constexpr int kOrthogonalizeAfter = namespace g2o { VertexSE3::VertexSE3() { - setToOriginImpl(); + setEstimate(Eigen::Isometry3d::Identity()); updateCache(); } -bool VertexSE3::read(std::istream& is) { - Vector7 est; - bool state = internal::readVector(is, est); - setEstimate(internal::fromVectorQT(est)); - return state; -} - -bool VertexSE3::write(std::ostream& os) const { - return internal::writeVector(os, internal::toVectorQT(estimate())); -} - void VertexSE3::oplusImpl(const VectorX::MapType& update) { const Isometry3 increment = internal::fromVectorMQT(update); estimate_ = estimate_ * increment; diff --git a/g2o/types/slam3d/vertex_se3.h b/g2o/types/slam3d/vertex_se3.h index cad0a5162..c9230ecfd 100644 --- a/g2o/types/slam3d/vertex_se3.h +++ b/g2o/types/slam3d/vertex_se3.h @@ -27,7 +27,6 @@ #ifndef G2O_VERTEX_SE3_ #define G2O_VERTEX_SE3_ -#include #include #include "g2o/config.h" @@ -37,8 +36,7 @@ #include "g2o/core/hyper_graph_action.h" #include "g2o/stuff/property.h" #include "g2o_types_slam3d_api.h" -#include "isometry3d_mappings.h" -#include "type_traits_isometry3.h" +#include "type_traits_isometry3.h" // IWYU pragma: keep namespace g2o { @@ -58,9 +56,6 @@ class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3> { public: VertexSE3(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - /** * update the position of this vertex. The update is in the form * (x,y,z,qx,qy,qz) whereas (x,y,z) represents the translational update diff --git a/g2o/types/slam3d_addons/CMakeLists.txt b/g2o/types/slam3d_addons/CMakeLists.txt index 9893d72a0..2abacb210 100644 --- a/g2o/types/slam3d_addons/CMakeLists.txt +++ b/g2o/types/slam3d_addons/CMakeLists.txt @@ -1,8 +1,4 @@ add_library(types_slam3d_addons ${G2O_LIB_TYPE} - vertex_se3_euler.cpp - vertex_se3_euler.h - edge_se3_euler.cpp - edge_se3_euler.h vertex_plane.cpp vertex_plane.h edge_se3_plane_calib.cpp @@ -13,7 +9,6 @@ add_library(types_slam3d_addons ${G2O_LIB_TYPE} edge_plane.cpp edge_plane.h edge_se3_calib.cpp edge_se3_calib.h types_slam3d_addons.cpp - types_slam3d_addons.h ) diff --git a/g2o/types/slam3d_addons/edge_plane.cpp b/g2o/types/slam3d_addons/edge_plane.cpp index fd5b625e0..835c314bb 100644 --- a/g2o/types/slam3d_addons/edge_plane.cpp +++ b/g2o/types/slam3d_addons/edge_plane.cpp @@ -26,9 +26,6 @@ #include "edge_plane.h" -#include "g2o/core/io_helper.h" -#include "g2o/types/slam3d_addons/vertex_plane.h" - namespace g2o { EdgePlane::EdgePlane() { @@ -36,18 +33,6 @@ EdgePlane::EdgePlane() { error_.setZero(); } -bool EdgePlane::read(std::istream& is) { - Vector4 v; - internal::readVector(is, v); - setMeasurement(v); - return readInformationMatrix(is); -} - -bool EdgePlane::write(std::ostream& os) const { - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - #if 0 void EdgePlane::linearizeOplus() { diff --git a/g2o/types/slam3d_addons/edge_plane.h b/g2o/types/slam3d_addons/edge_plane.h index 0c67ca344..180ceefd4 100644 --- a/g2o/types/slam3d_addons/edge_plane.h +++ b/g2o/types/slam3d_addons/edge_plane.h @@ -28,13 +28,9 @@ #define G2O_EDGE_PLANE3D_H #include -#include -#include -#include "g2o/config.h" #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" -#include "g2o/types/slam3d_addons/plane3d.h" #include "g2o_types_slam3d_addons_api.h" #include "vertex_plane.h" @@ -51,8 +47,6 @@ class G2O_TYPES_SLAM3D_ADDONS_API EdgePlane error_ = (v2->estimate().toVector() - v1->estimate().toVector()) - measurement_; } - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void setMeasurement(const Vector4& m) override { measurement_ = m; } diff --git a/g2o/types/slam3d_addons/edge_se3_calib.cpp b/g2o/types/slam3d_addons/edge_se3_calib.cpp index ce420a1f4..f18202817 100644 --- a/g2o/types/slam3d_addons/edge_se3_calib.cpp +++ b/g2o/types/slam3d_addons/edge_se3_calib.cpp @@ -28,7 +28,6 @@ #include -#include "g2o/core/io_helper.h" #include "g2o/types/slam3d/isometry3d_mappings.h" #include "g2o/types/slam3d/vertex_se3.h" @@ -45,19 +44,4 @@ void EdgeSE3Calib::computeError() { v1->estimate().inverse() * v2->estimate() * calib->estimate()); } -bool EdgeSE3Calib::write(std::ostream& os) const { - internal::writeVector(os, internal::toVectorQT(measurement_)); - return writeInformationMatrix(os); -} - -bool EdgeSE3Calib::read(std::istream& is) { - Vector7 meas; - internal::readVector(is, meas); - // normalize the quaternion to recover numerical precision lost by storing as - // human readable text - Vector4::MapType(meas.data() + 3).normalize(); - setMeasurement(g2o::internal::fromVectorQT(meas)); - return readInformationMatrix(is); -} - } // namespace g2o diff --git a/g2o/types/slam3d_addons/edge_se3_calib.h b/g2o/types/slam3d_addons/edge_se3_calib.h index 2edafd90b..fbabbd849 100644 --- a/g2o/types/slam3d_addons/edge_se3_calib.h +++ b/g2o/types/slam3d_addons/edge_se3_calib.h @@ -27,13 +27,9 @@ #ifndef G2O_EDGE_SE3_CALIB_H #define G2O_EDGE_SE3_CALIB_H -#include - #include "g2o/core/base_variable_sized_edge.h" #include "g2o/core/eigen_types.h" -#include "g2o/types/slam3d/isometry3d_mappings.h" -#include "g2o/types/slam3d/type_traits_isometry3.h" -#include "g2o/types/slam3d/vertex_se3.h" +#include "g2o/types/slam3d/type_traits_isometry3.h" // IWYU pragma: keep #include "g2o_types_slam3d_addons_api.h" namespace g2o { @@ -50,8 +46,6 @@ class EdgeSE3Calib EdgeSE3Calib(); G2O_TYPES_SLAM3D_ADDONS_API void computeError() override; - G2O_TYPES_SLAM3D_ADDONS_API bool read(std::istream& is) override; - G2O_TYPES_SLAM3D_ADDONS_API bool write(std::ostream& os) const override; }; } // namespace g2o diff --git a/g2o/types/slam3d_addons/edge_se3_euler.cpp b/g2o/types/slam3d_addons/edge_se3_euler.cpp deleted file mode 100644 index 801edb9a4..000000000 --- a/g2o/types/slam3d_addons/edge_se3_euler.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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 "edge_se3_euler.h" - -#include -#include -#include - -#include "g2o/core/eigen_types.h" -#include "g2o/stuff/misc.h" -#include "g2o/types/slam3d/isometry3d_mappings.h" - -namespace g2o { - -/** conversion code from Euler angles */ -static void jac_quat3_euler3(Eigen::Matrix& J, - const Isometry3& t) { - Vector7 t0 = g2o::internal::toVectorQT(t); - - double delta = cst(1e-6); - double idelta = 1 / (2 * delta); - - Vector7 ta; - Vector7 tb; - for (int i = 0; i < 6; i++) { - ta = tb = t0; - ta[i] -= delta; - tb[i] += delta; - Vector6 ea = g2o::internal::toVectorET(g2o::internal::fromVectorQT(ta)); - Vector6 eb = g2o::internal::toVectorET(g2o::internal::fromVectorQT(tb)); - J.col(i) = (eb - ea) * idelta; - } -} - -bool EdgeSE3Euler::read(std::istream& is) { - Vector6 meas; - for (int i = 0; i < 6; i++) is >> meas[i]; - Isometry3 transf = g2o::internal::fromVectorET(meas); - Eigen::Matrix infMatEuler; - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) { - is >> infMatEuler(i, j); - if (i != j) infMatEuler(j, i) = infMatEuler(i, j); - } - Eigen::Matrix J; - jac_quat3_euler3(J, transf); - Eigen::Matrix infMat = - J.transpose() * infMatEuler * J; - setMeasurement(transf); - setInformation(infMat); - return true; -} - -bool EdgeSE3Euler::write(std::ostream& os) const { - Vector6 meas = g2o::internal::toVectorET(measurement_); - for (int i = 0; i < 6; i++) os << meas[i] << " "; - - Eigen::Matrix J; - jac_quat3_euler3(J, measurement()); - // HACK: invert the jacobian to simulate the inverse derivative - J = J.inverse(); - Eigen::Matrix infMatEuler = - J.transpose() * information() * J; - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) { - os << " " << infMatEuler(i, j); - } - return os.good(); -} - -} // namespace g2o diff --git a/g2o/types/slam3d_addons/edge_se3_line.cpp b/g2o/types/slam3d_addons/edge_se3_line.cpp index 28d29c04a..275f74303 100644 --- a/g2o/types/slam3d_addons/edge_se3_line.cpp +++ b/g2o/types/slam3d_addons/edge_se3_line.cpp @@ -31,8 +31,6 @@ #include #include -#include "g2o/core/cache.h" -#include "g2o/core/io_helper.h" #include "g2o/core/parameter.h" #include "g2o/stuff/opengl_wrapper.h" #include "g2o/types/slam3d/parameter_se3_offset.h" @@ -48,19 +46,6 @@ EdgeSE3Line3D::EdgeSE3Line3D() { color << 0.0, 0.5, 1.0; } -bool EdgeSE3Line3D::read(std::istream& is) { - bool state = readParamIds(is); - state &= internal::readVector(is, measurement_); - state &= readInformationMatrix(is); - return state; -} - -bool EdgeSE3Line3D::write(std::ostream& os) const { - writeParamIds(os); - internal::writeVector(os, measurement()); - return writeInformationMatrix(os); -} - void EdgeSE3Line3D::computeError() { const VertexSE3* se3Vertex = vertexXnRaw<0>(); const VertexLine3D* lineVertex = vertexXnRaw<1>(); diff --git a/g2o/types/slam3d_addons/edge_se3_line.h b/g2o/types/slam3d_addons/edge_se3_line.h index 53ddd08f7..66a0fbc2f 100644 --- a/g2o/types/slam3d_addons/edge_se3_line.h +++ b/g2o/types/slam3d_addons/edge_se3_line.h @@ -27,9 +27,7 @@ #ifndef G2O_EDGE_SE3_LINE_H_ #define G2O_EDGE_SE3_LINE_H_ -#include #include -#include #include "g2o/config.h" #include "g2o/core/base_binary_edge.h" @@ -51,9 +49,6 @@ class G2O_TYPES_SLAM3D_ADDONS_API EdgeSE3Line3D public: EdgeSE3Line3D(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void computeError() override; virtual void setMeasurement(const Vector6& m) { measurement_ = Line3D(m); } diff --git a/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp b/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp index 4fa388ed9..9cc7589bf 100644 --- a/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp +++ b/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp @@ -26,12 +26,8 @@ #include "edge_se3_plane_calib.h" -#include -#include #include -#include -#include "g2o/core/io_helper.h" #include "g2o/stuff/macros.h" #include "g2o/stuff/misc.h" #include "g2o/stuff/opengl_wrapper.h" @@ -43,21 +39,6 @@ EdgeSE3PlaneSensorCalib::EdgeSE3PlaneSensorCalib() resize(3); } -bool EdgeSE3PlaneSensorCalib::read(std::istream& is) { - Vector4 v; - bool state = internal::readVector(is, v); - setMeasurement(Plane3D(v)); - state &= internal::readVector(is, color); - state &= readInformationMatrix(is); - return state; -} - -bool EdgeSE3PlaneSensorCalib::write(std::ostream& os) const { - internal::writeVector(os, measurement().toVector()); - internal::writeVector(os, color); - return writeInformationMatrix(os); -} - #ifdef G2O_HAVE_OPENGL EdgeSE3PlaneSensorCalibDrawAction::EdgeSE3PlaneSensorCalibDrawAction() : DrawAction(typeid(EdgeSE3PlaneSensorCalib).name()), diff --git a/g2o/types/slam3d_addons/edge_se3_plane_calib.h b/g2o/types/slam3d_addons/edge_se3_plane_calib.h index 76a11a944..faa8cc8c9 100644 --- a/g2o/types/slam3d_addons/edge_se3_plane_calib.h +++ b/g2o/types/slam3d_addons/edge_se3_plane_calib.h @@ -27,8 +27,6 @@ #ifndef G2O_EDGE_SE3_PLANE_CALIB_H #define G2O_EDGE_SE3_PLANE_CALIB_H -#include -#include #include #include "g2o/config.h" @@ -64,9 +62,6 @@ class G2O_TYPES_SLAM3D_ADDONS_API EdgeSE3PlaneSensorCalib } void setMeasurement(const Plane3D& m) override { measurement_ = m; } - - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; }; #ifdef G2O_HAVE_OPENGL diff --git a/g2o/types/slam3d_addons/line3d.cpp b/g2o/types/slam3d_addons/line3d.cpp index 091225aaf..7c7960210 100644 --- a/g2o/types/slam3d_addons/line3d.cpp +++ b/g2o/types/slam3d_addons/line3d.cpp @@ -32,11 +32,13 @@ namespace g2o { -static inline Matrix3 skew(const Vector3& t) { +namespace { +inline Matrix3 skew(const Vector3& t) { Matrix3 S; S << 0, -t.z(), t.y(), t.z(), 0, -t.x(), -t.y(), t.x(), 0; return S; } +} // namespace Vector6 Line3D::toCartesian() const { Vector6 cartesian; diff --git a/g2o/types/slam3d_addons/plane3d.h b/g2o/types/slam3d_addons/plane3d.h index 6168d5b2b..1163566ef 100644 --- a/g2o/types/slam3d_addons/plane3d.h +++ b/g2o/types/slam3d_addons/plane3d.h @@ -32,7 +32,6 @@ #include "g2o/core/eigen_types.h" #include "g2o/core/type_traits.h" -#include "g2o/stuff/misc.h" #include "g2o_types_slam3d_addons_api.h" namespace g2o { diff --git a/g2o/types/slam3d_addons/types_slam3d_addons.cpp b/g2o/types/slam3d_addons/types_slam3d_addons.cpp index 880e88880..ff66c9f31 100644 --- a/g2o/types/slam3d_addons/types_slam3d_addons.cpp +++ b/g2o/types/slam3d_addons/types_slam3d_addons.cpp @@ -24,33 +24,21 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include "types_slam3d_addons.h" - -#include -#include - #include "g2o/config.h" #include "g2o/core/factory.h" #include "g2o/core/hyper_graph_action.h" -#include "g2o/stuff/macros.h" -#include "g2o/types/slam3d/edge_se3.h" #include "g2o/types/slam3d/parameter_camera.h" -#include "g2o/types/slam3d/vertex_se3.h" #include "g2o/types/slam3d_addons/edge_plane.h" #include "g2o/types/slam3d_addons/edge_se3_calib.h" -#include "g2o/types/slam3d_addons/edge_se3_euler.h" #include "g2o/types/slam3d_addons/edge_se3_line.h" #include "g2o/types/slam3d_addons/edge_se3_plane_calib.h" #include "g2o/types/slam3d_addons/vertex_line3d.h" #include "g2o/types/slam3d_addons/vertex_plane.h" -#include "g2o/types/slam3d_addons/vertex_se3_euler.h" namespace g2o { G2O_REGISTER_TYPE_GROUP(slam3d_addons); -G2O_REGISTER_TYPE(VERTEX3, VertexSE3Euler); -G2O_REGISTER_TYPE(EDGE3, EdgeSE3Euler); G2O_REGISTER_TYPE(VERTEX_PLANE, VertexPlane); G2O_REGISTER_TYPE(EDGE_SE3_PLANE_CALIB, EdgeSE3PlaneSensorCalib); @@ -67,23 +55,4 @@ G2O_REGISTER_ACTION(VertexLine3DDrawAction); G2O_REGISTER_ACTION(EdgeSE3Line3DDrawAction); #endif -G2O_ATTRIBUTE_CONSTRUCTOR(init_slam3d_addons_types) { - static bool initialized = false; - if (initialized) return; - initialized = true; - -#ifdef G2O_HAVE_OPENGL - HyperGraphActionLibrary* actionLib = HyperGraphActionLibrary::instance(); - HyperGraphElementAction::HyperGraphElementActionPtr vertexse3eulerdraw( - new g2o::VertexSE3DrawAction); - vertexse3eulerdraw->setTypeName(typeid(VertexSE3Euler).name()); - actionLib->registerAction(vertexse3eulerdraw); - - HyperGraphElementAction::HyperGraphElementActionPtr edgese3eulerdraw( - new g2o::EdgeSE3DrawAction); - edgese3eulerdraw->setTypeName(typeid(EdgeSE3Euler).name()); - actionLib->registerAction(edgese3eulerdraw); -#endif -} - } // namespace g2o diff --git a/g2o/types/slam3d_addons/types_slam3d_addons.h b/g2o/types/slam3d_addons/types_slam3d_addons.h deleted file mode 100644 index d75cda248..000000000 --- a/g2o/types/slam3d_addons/types_slam3d_addons.h +++ /dev/null @@ -1,44 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_TYPES_SLAM3D_ADDONS_ -#define G2O_TYPES_SLAM3D_ADDONS_ - -#include "edge_plane.h" -#include "edge_se3_calib.h" -#include "edge_se3_euler.h" -#include "edge_se3_line.h" -#include "edge_se3_plane_calib.h" -#include "g2o/config.h" -#include "g2o/core/base_binary_edge.h" -#include "g2o/core/base_vertex.h" -#include "g2o/core/hyper_graph_action.h" -#include "g2o/types//slam3d/types_slam3d.h" -#include "vertex_line3d.h" -#include "vertex_plane.h" -#include "vertex_se3_euler.h" - -#endif diff --git a/g2o/types/slam3d_addons/vertex_line3d.cpp b/g2o/types/slam3d_addons/vertex_line3d.cpp index 48f77029f..baf54af6f 100644 --- a/g2o/types/slam3d_addons/vertex_line3d.cpp +++ b/g2o/types/slam3d_addons/vertex_line3d.cpp @@ -26,11 +26,8 @@ #include "vertex_line3d.h" -#include #include -#include -#include "g2o/core/io_helper.h" #include "g2o/stuff/opengl_wrapper.h" #include "g2o/types/slam3d_addons/line3d.h" @@ -38,17 +35,6 @@ namespace g2o { VertexLine3D::VertexLine3D() : color(1., 0.5, 0.) {} -bool VertexLine3D::read(std::istream& is) { - Vector6 lv; - bool state = internal::readVector(is, lv); - setEstimate(Line3D(lv)); - return state; -} - -bool VertexLine3D::write(std::ostream& os) const { - return internal::writeVector(os, estimate_); -} - #ifdef G2O_HAVE_OPENGL VertexLine3DDrawAction::VertexLine3DDrawAction() : DrawAction(typeid(VertexLine3D).name()), diff --git a/g2o/types/slam3d_addons/vertex_line3d.h b/g2o/types/slam3d_addons/vertex_line3d.h index 713efc126..ad29495e5 100644 --- a/g2o/types/slam3d_addons/vertex_line3d.h +++ b/g2o/types/slam3d_addons/vertex_line3d.h @@ -28,7 +28,6 @@ #define G2O_VERTEX_LINE3D_H_ #include -#include #include #include "g2o/config.h" @@ -45,8 +44,6 @@ namespace g2o { class G2O_TYPES_SLAM3D_ADDONS_API VertexLine3D : public BaseVertex<4, Line3D> { public: VertexLine3D(); - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; void oplusImpl(const VectorX::MapType& update) override { estimate_.oplus(update); diff --git a/g2o/types/slam3d_addons/vertex_plane.cpp b/g2o/types/slam3d_addons/vertex_plane.cpp index df7ed41f2..44dfcc847 100644 --- a/g2o/types/slam3d_addons/vertex_plane.cpp +++ b/g2o/types/slam3d_addons/vertex_plane.cpp @@ -29,7 +29,6 @@ #include #include -#include "g2o/core/io_helper.h" #include "g2o/stuff/macros.h" #include "g2o/stuff/misc.h" #include "g2o/stuff/opengl_wrapper.h" @@ -39,20 +38,6 @@ namespace g2o { VertexPlane::VertexPlane() { color << cst(.2), cst(.2), cst(.2); } -bool VertexPlane::read(std::istream& is) { - Vector4 lv; - bool state = internal::readVector(is, lv); - setEstimate(Plane3D(lv)); - state &= internal::readVector(is, color); - return state; -} - -bool VertexPlane::write(std::ostream& os) const { - bool state = internal::writeVector(os, estimate_.toVector()); - state &= internal::writeVector(os, color); - return state; -} - #ifdef G2O_HAVE_OPENGL VertexPlaneDrawAction::VertexPlaneDrawAction() diff --git a/g2o/types/slam3d_addons/vertex_plane.h b/g2o/types/slam3d_addons/vertex_plane.h index 8bd332b5f..2bdbf190c 100644 --- a/g2o/types/slam3d_addons/vertex_plane.h +++ b/g2o/types/slam3d_addons/vertex_plane.h @@ -27,8 +27,6 @@ #ifndef G2O_VERTEX_PLANE_H_ #define G2O_VERTEX_PLANE_H_ -#include -#include #include #include "g2o/config.h" @@ -46,10 +44,6 @@ class G2O_TYPES_SLAM3D_ADDONS_API VertexPlane : public BaseVertex<3, Plane3D> { public: VertexPlane(); - //! Custom IO - bool read(std::istream& is) override; - bool write(std::ostream& os) const override; - void oplusImpl(const VectorX::MapType& update) override { estimate_.oplus(update); } diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 3cf1c30ae..16a0f79e2 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -24,6 +24,7 @@ pybind11_add_module(g2opy core/py_factory.cpp core/py_hyper_dijkstra.cpp core/py_hyper_graph.cpp + core/py_io_format.cpp core/py_jacobian_workspace.cpp core/py_optimizable_graph.cpp core/py_optimization_algorithm.cpp @@ -64,4 +65,4 @@ target_link_libraries(g2opy PRIVATE types_slam3d_addons ) target_include_directories(g2opy PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) -install(TARGETS g2opy LIBRARY DESTINATION g2opy) +install(TARGETS g2opy LIBRARY DESTINATION g2o) diff --git a/python/README.md b/python/README.md index 3f6851170..6b6d986fd 100644 --- a/python/README.md +++ b/python/README.md @@ -1,5 +1,4 @@ # g2opy - This is a python binding of [g2o](https://github.com/RainerKuemmerle/g2o). The code here is based on https://github.com/uoip/g2opy.git by qihang@outlook.com @@ -18,125 +17,5 @@ Isometry3d, Isometry2d, AngleAxis) are packed into this library. ## Requirements * ([pybind11](https://github.com/pybind/pybind11) - -## Installation -``` -git clone https://github.com/uoip/g2opy.git -cd g2opy -mkdir build -cd build -cmake .. -make -j8 -cd .. -python setup.py install -``` -Tested under Ubuntu 16.04, Python 3.6+. - - -## Get Started -The code snippets below show the core parts of BA and Pose Graph Optimization in a SLAM system. -#### Bundle Adjustment -```python -import numpy -import g2o - -class BundleAdjustment(g2o.SparseOptimizer): - def __init__(self, ): - super().__init__() - solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3()) - solver = g2o.OptimizationAlgorithmLevenberg(solver) - super().set_algorithm(solver) - - def optimize(self, max_iterations=10): - super().initialize_optimization() - super().optimize(max_iterations) - - def add_pose(self, pose_id, pose, cam, fixed=False): - sbacam = g2o.SBACam(pose.orientation(), pose.position()) - sbacam.set_cam(cam.fx, cam.fy, cam.cx, cam.cy, cam.baseline) - - v_se3 = g2o.VertexCam() - v_se3.set_id(pose_id * 2) # internal id - v_se3.set_estimate(sbacam) - v_se3.set_fixed(fixed) - super().add_vertex(v_se3) - - def add_point(self, point_id, point, fixed=False, marginalized=True): - v_p = g2o.VertexSBAPointXYZ() - v_p.set_id(point_id * 2 + 1) - v_p.set_estimate(point) - v_p.set_marginalized(marginalized) - v_p.set_fixed(fixed) - super().add_vertex(v_p) - - def add_edge(self, point_id, pose_id, - measurement, - information=np.identity(2), - robust_kernel=g2o.RobustKernelHuber(np.sqrt(5.991))): # 95% CI - - edge = g2o.EdgeProjectP2MC() - edge.set_vertex(0, self.vertex(point_id * 2 + 1)) - edge.set_vertex(1, self.vertex(pose_id * 2)) - edge.set_measurement(measurement) # projection - edge.set_information(information) - - if robust_kernel is not None: - edge.set_robust_kernel(robust_kernel) - super().add_edge(edge) - - def get_pose(self, pose_id): - return self.vertex(pose_id * 2).estimate() - - def get_point(self, point_id): - return self.vertex(point_id * 2 + 1).estimate() -``` - -#### Pose Graph Optimization -```python -import numpy -import g2o - -class PoseGraphOptimization(g2o.SparseOptimizer): - def __init__(self): - super().__init__() - solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) - solver = g2o.OptimizationAlgorithmLevenberg(solver) - super().set_algorithm(solver) - - def optimize(self, max_iterations=20): - super().initialize_optimization() - super().optimize(max_iterations) - - def add_vertex(self, id, pose, fixed=False): - v_se3 = g2o.VertexSE3() - v_se3.set_id(id) - v_se3.set_estimate(pose) - v_se3.set_fixed(fixed) - super().add_vertex(v_se3) - - def add_edge(self, vertices, measurement, - information=np.identity(6), - robust_kernel=None): - - edge = g2o.EdgeSE3() - for i, v in enumerate(vertices): - if isinstance(v, int): - v = self.vertex(v) - edge.set_vertex(i, v) - - edge.set_measurement(measurement) # relative pose - edge.set_information(information) - if robust_kernel is not None: - edge.set_robust_kernel(robust_kernel) - super().add_edge(edge) - - def get_pose(self, id): - return self.vertex(id).estimate() -``` - -For more details, checkout [python examples](python/examples). Thanks to -[pybind11](https://github.com/pybind/pybind11), g2opy works seamlessly between -numpy and underlying Eigen. - ## License * The binding code and python example code is licensed under BSD License. diff --git a/python/core/py_base_edge.h b/python/core/py_base_edge.h index 9b43d7c0c..5af9e9f09 100644 --- a/python/core/py_base_edge.h +++ b/python/core/py_base_edge.h @@ -25,8 +25,10 @@ void templatedBaseEdge(py::module& m, const std::string& suffix) { //-> data* .def("information", static_cast( &CLS::information)) // -> InformationType - .def("set_information", &CLS::setInformation, "information"_a, - py::keep_alive<1, 2>()) // InformationType -> + .def( + "set_information", + [](CLS& edge, const MatrixX& info) { edge.setInformation(info); }, + "information"_a, py::keep_alive<1, 2>()) // InformationType -> .def("measurement", &CLS::measurement) // -> E .def("set_measurement", &CLS::setMeasurement, "m"_a, @@ -57,8 +59,12 @@ void templatedDynamicBaseEdge(py::module& m, const std::string& suffix) { //-> data* .def("information", static_cast( &CLS::information)) // -> InformationType - .def("set_information", &CLS::setInformation, "information"_a, - py::keep_alive<1, 2>()) // InformationType -> + .def( + "set_information", + [](CLS& edge, const InformationType& info) { + edge.setInformation(info); + }, + "information"_a, py::keep_alive<1, 2>()) // InformationType -> .def("measurement", &CLS::measurement) // -> E .def("set_measurement", &CLS::setMeasurement, "m"_a, diff --git a/python/core/py_core.cpp b/python/core/py_core.cpp index 8ea21bcc0..ec54a9afc 100644 --- a/python/core/py_core.cpp +++ b/python/core/py_core.cpp @@ -1,5 +1,6 @@ #include "py_core.h" +#include "core/py_io_format.h" #include "py_base_binary_edge.h" #include "py_base_edge.h" #include "py_base_unary_edge.h" @@ -13,6 +14,7 @@ #include "py_hyper_dijkstra.h" #include "py_hyper_graph.h" #include "py_hyper_graph_action.h" +#include "py_io_format.h" #include "py_jacobian_workspace.h" #include "py_linear_solver.h" #include "py_optimizable_graph.h" @@ -27,6 +29,7 @@ namespace g2o { void declareCore(py::module& m) { + declareIOFormat(m); declareHyperGraph(m); declareOptimizableGraph(m); declareSparseOptimizer(m); diff --git a/python/core/py_io_format.cpp b/python/core/py_io_format.cpp new file mode 100644 index 000000000..b314e14f3 --- /dev/null +++ b/python/core/py_io_format.cpp @@ -0,0 +1,15 @@ +#include "py_io_format.h" + +#include "g2o/core/io/io_format.h" + +namespace g2o { + +void declareIOFormat(py::module& m) { + py::enum_(m, "IoFormat") + .value("G2O", io::Format::kG2O, "G2O's custom ASCII IO format") + .value("JSON", io::Format::kJson, "JSON IO format") + .value("BINARY", io::Format::kBinary, "G2O's custom binary format") + .value("XML", io::Format::kXML, "XML IO format"); +} + +} // end namespace g2o diff --git a/python/core/py_io_format.h b/python/core/py_io_format.h new file mode 100644 index 000000000..a823cef72 --- /dev/null +++ b/python/core/py_io_format.h @@ -0,0 +1,9 @@ +#pragma once + +#include "g2opy.h" + +namespace g2o { + +void declareIOFormat(py::module& m); + +} // end namespace g2o diff --git a/python/core/py_optimizable_graph.cpp b/python/core/py_optimizable_graph.cpp index c14a8e8fa..8748a7540 100644 --- a/python/core/py_optimizable_graph.cpp +++ b/python/core/py_optimizable_graph.cpp @@ -7,6 +7,7 @@ #include #include "g2o/core/eigen_types.h" +#include "g2o/core/io/io_format.h" namespace g2o { @@ -40,7 +41,6 @@ void declareOptimizableGraph(py::module& m) { py::class_>(cls, "OptimizableGraph_Vertex") //.def(py::init<>()) // invalid new-expression of abstract class - .def("set_to_origin", &CLS::Vertex::setToOrigin) // -> void .def( "set_estimate_data", [](CLS::Vertex& v, const VectorX& data) { @@ -208,11 +208,13 @@ void declareOptimizableGraph(py::module& m) { cls.def("discard_top", static_cast(&CLS::discardTop)); - cls.def("load", static_cast(&CLS::load), - "filename"_a); + cls.def("load", + static_cast(&CLS::load), + "filename"_a, "format"_a = io::Format::kG2O); cls.def("save", - static_cast(&CLS::save), - "filename"_a, "level"_a = 0); + static_cast( + &CLS::save), + "filename"_a, "format"_a = io::Format::kG2O, "level"_a = 0); cls.def("set_fixed", &CLS::setFixed, "vset"_a, "fixes"_a, py::keep_alive<1, 2>()); // (HyperGraph::VertexSet&, bool) -> void diff --git a/python/core/py_sparse_optimizer.cpp b/python/core/py_sparse_optimizer.cpp index 893fb3d3c..10849ca8a 100644 --- a/python/core/py_sparse_optimizer.cpp +++ b/python/core/py_sparse_optimizer.cpp @@ -42,8 +42,7 @@ void declareSparseOptimizer(py::module& m) { .def("compute_initial_guess", static_cast( &CLS::computeInitialGuess), - "propagator"_a) // virtual - .def("set_to_origin", &CLS::setToOrigin) // virtual + "propagator"_a) // virtual .def("optimize", &CLS::optimize, "iterations"_a, "online"_a = false) // -> int diff --git a/python/examples/simple_optimize.py b/python/examples/simple_optimize.py index bd136d36f..b67267600 100644 --- a/python/examples/simple_optimize.py +++ b/python/examples/simple_optimize.py @@ -1,43 +1,59 @@ import argparse -import os -import g2opy -import numpy as np +import g2opy as g2o -parser = argparse.ArgumentParser() -parser.add_argument( - "-n", "--max_iterations", type=int, default=10, help="perform n iterations" -) -parser.add_argument( - "-i", "--input", type=str, default="sphere2500.g2o", help="input file" -) -parser.add_argument( - "-o", "--output", type=str, default="", help="save resulting graph as file" -) -args = parser.parse_args() +def to_io_format(value: str) -> g2o.IoFormat: + return g2o.IoFormat.__members__[value.upper()] -def main(): - solver = g2opy.BlockSolverX(g2opy.LinearSolverEigenX()) - # solver = g2opy.BlockSolverSE3(g2opy.LinearSolverEigenSE3()) - solver = g2opy.OptimizationAlgorithmLevenberg(solver) - - optimizer = g2opy.SparseOptimizer() - optimizer.set_verbose(True) - optimizer.set_algorithm(solver) +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("input", type=str, help="input filename") + parser.add_argument( + "-i", "--max_iterations", type=int, default=10, help="perform n iterations" + ) + parser.add_argument( + "-o", "--output", type=str, default="", help="save resulting graph as file" + ) + parser.add_argument( + "--output-format", + type=str, + choices=["g2o", "json", "xml", "binary"], + default=g2o.IoFormat.G2O.name, + help="define the output format", + ) + parser.add_argument( + "-v", + "--verbose", + action="store_true", + help="activate verbose output", + ) + args = parser.parse_args() + + optimizer = g2o.SparseOptimizer() + optimizer.set_verbose(args.verbose) + optimizer.set_algorithm( + g2o.OptimizationAlgorithmLevenberg(g2o.BlockSolverX(g2o.LinearSolverEigenX())) + ) + + print(f"Loading {args.input}") optimizer.load(args.input) - print("num vertices:", len(optimizer.vertices())) - print("num edges:", len(optimizer.edges()), end="\n\n") - + print("Num vertices:", len(optimizer.vertices())) + print("Num edges:", len(optimizer.edges())) optimizer.initialize_optimization() - optimizer.optimize(args.max_iterations) + optimizer.compute_active_errors() + print(f"Initial chi2: {optimizer.chi2()}") + + if args.max_iterations > 0: + optimizer.optimize(args.max_iterations) + optimizer.compute_active_errors() + print(f"Final chi2: {optimizer.chi2()}") if len(args.output) > 0: - optimizer.save(args.output) + print(f"Saving to {args.output}") + optimizer.save(args.output, to_io_format(args.output_format)) if __name__ == "__main__": - assert os.path.isfile(args.input), "Please provide a existing .g2o file" - main() diff --git a/python/g2opy.h b/python/g2opy.h index 43e9955c3..3c8c917d0 100644 --- a/python/g2opy.h +++ b/python/g2opy.h @@ -1,6 +1,7 @@ #ifndef G2O_PY_H #define G2O_PY_H +// IWYU pragma: begin_exports #include #include #include @@ -10,5 +11,6 @@ namespace py = pybind11; // NOLINT using namespace pybind11::literals; // NOLINT +// IWYU pragma: end_exports #endif diff --git a/python/types/pure/py_types_pure.cpp b/python/types/pure/py_types_pure.cpp index 7feccc2f0..aa8e21cd3 100644 --- a/python/types/pure/py_types_pure.cpp +++ b/python/types/pure/py_types_pure.cpp @@ -3,10 +3,6 @@ #include #include -#include -#include - -#include "g2o/config.h" #include "g2o/core/base_dynamic_vertex.h" #include "g2o/core/base_variable_sized_edge.h" #include "g2o/core/eigen_types.h" @@ -17,21 +13,6 @@ class VectorXVertex : public BaseDynamicVertex { public: VectorXVertex() = default; - bool read(std::istream& is) override { - int dimension; - is >> dimension; - if (!is.good()) { - return false; - } - setDimension(dimension); - return g2o::internal::readVector(is, estimate_); - } - - bool write(std::ostream& os) const override { - os << estimate_.size() << " "; - return g2o::internal::writeVector(os, estimate_); - } - bool setDimensionImpl(int newDimension) override { const int oldDimension = dimension(); if (oldDimension == Eigen::Dynamic) { @@ -75,9 +56,6 @@ class VariableVectorXEdge void computeError() override { error_ = compute_error(); } - bool read(std::istream&) override { return false; } - bool write(std::ostream&) const override { return false; } - // python trampoline virtual VectorX compute_error() = 0; }; diff --git a/python/types/sba/py_types_six_dof_expmap.cpp b/python/types/sba/py_types_six_dof_expmap.cpp index 73c6bccf2..97ae2f260 100644 --- a/python/types/sba/py_types_six_dof_expmap.cpp +++ b/python/types/sba/py_types_six_dof_expmap.cpp @@ -2,36 +2,35 @@ #include "g2o/core/factory.h" #include "g2o/types/sba/edge_project_xyz.h" -#include "g2o/types/sba/types_six_dof_expmap.h" +#include "g2o/types/sba/parameter_cameraparameters.h" +#include "g2o/types/sba/types_six_dof_expmap.h" // IWYU pragma: keep #include "g2o/types/slam3d/se3quat.h" #include "g2opy.h" #include "python/core/py_base_binary_edge.h" #include "python/core/py_base_fixed_sized_edge.h" #include "python/core/py_base_unary_edge.h" -#include "python/core/py_base_variable_sized_edge.h" G2O_USE_TYPE_GROUP(expmap) namespace g2o { void declareTypesSixDofExpmap(py::module& m) { + py::class_(m, "StereoCameraParameters") + .def(py::init<>()) + .def_readwrite("focal_length", &StereoCameraParameters::focal_length) + .def_readwrite("principle_point", + &StereoCameraParameters::principle_point) + .def_readwrite("baseline", &StereoCameraParameters::baseline); + py::class_>( m, "CameraParameters") .def(py::init<>()) .def(py::init([](double f, const Eigen::Ref& p, double b) { return CameraParameters(f, p, b); })) - .def("cam_map", &CameraParameters::cam_map, "trans_xyz"_a) .def("stereocam_uvu_map", &CameraParameters::stereocam_uvu_map, - "trans_xyz"_a) - .def_readwrite("focal_length", &CameraParameters::focal_length) - .def_readwrite("principal_point", &CameraParameters::principle_point) - .def_readwrite("principle_point", &CameraParameters::principle_point) - .def_readwrite("baseline", &CameraParameters::baseline) - // read - // write - ; + "trans_xyz"_a); py::class_, std::shared_ptr>(m, "VertexSE3Expmap") @@ -43,8 +42,7 @@ void declareTypesSixDofExpmap(py::module& m) { BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>, std::shared_ptr>(m, "EdgeSE3Expmap") .def(py::init<>()) - .def("compute_error", &EdgeSE3Expmap::computeError) - .def("linearize_oplus", &EdgeSE3Expmap::linearizeOplus); + .def("compute_error", &EdgeSE3Expmap::computeError); templatedBaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>( m, "_2_Vector2_VertexPointXYZ_VertexSE3Expmap"); diff --git a/python/types/slam2d/py_edge_se2.h b/python/types/slam2d/py_edge_se2.h index e579bcb13..e301cc6cf 100644 --- a/python/types/slam2d/py_edge_se2.h +++ b/python/types/slam2d/py_edge_se2.h @@ -32,7 +32,7 @@ inline void declareEdgeSE2(py::module& m) { std::shared_ptr>(m, "EdgeSE2LotsOfXY") .def(py::init<>()) .def("set_dimension", &EdgeSE2LotsOfXY::setDimension<-1>) - .def("set_size", &EdgeSE2LotsOfXY::setSize) + .def("resize", &EdgeSE2LotsOfXY::resize) .def("compute_error", &EdgeSE2LotsOfXY::computeError) .def("set_measurement_from_state", @@ -67,8 +67,13 @@ inline void declareEdgeSE2(py::module& m) { .def("initial_estimate_possible", &EdgeSE2Prior::initialEstimatePossible) .def("initial_estimate", &EdgeSE2Prior::initialEstimate); - py::class_, - std::shared_ptr>(m, "EdgeSE2TwoPointsXY") + templatedBaseFixedSizedEdge<4, Vector4, VertexSE2, VertexPointXY, + VertexPointXY>( + m, "_4_Vector4_VertexSE2_VertexPointXY_VertexPointXY"); + py::class_< + EdgeSE2TwoPointsXY, + BaseFixedSizedEdge<4, Vector4, VertexSE2, VertexPointXY, VertexPointXY>, + std::shared_ptr>(m, "EdgeSE2TwoPointsXY") .def(py::init<>()) .def("compute_error", &EdgeSE2TwoPointsXY::computeError) .def("set_measurement_from_state", diff --git a/python/types/slam2d/py_edge_se2_pointxy.h b/python/types/slam2d/py_edge_se2_pointxy.h index 948cb3d00..99b65f78a 100644 --- a/python/types/slam2d/py_edge_se2_pointxy.h +++ b/python/types/slam2d/py_edge_se2_pointxy.h @@ -54,9 +54,12 @@ inline void declareEdgeSE2PointXY(py::module& m) { // class G2O_TYPES_SLAM2D_API EdgeSE2PointXYBearingWriteGnuplotAction: public // WriteGnuplotAction class G2O_TYPES_SLAM2D_API // EdgeSE2PointXYBearingDrawAction: public DrawAction - - py::class_, - std::shared_ptr>(m, "EdgeSE2PointXYCalib") + templatedBaseFixedSizedEdge<2, Vector2, VertexSE2, VertexPointXY, VertexSE2>( + m, "_2_Vector2_VertexSE2_VertexPointXY_VertexSE2"); + py::class_< + EdgeSE2PointXYCalib, + BaseFixedSizedEdge<2, Vector2, VertexSE2, VertexPointXY, VertexSE2>, + std::shared_ptr>(m, "EdgeSE2PointXYCalib") .def(py::init<>()) .def("compute_error", &EdgeSE2PointXYCalib::computeError) .def("initial_estimate_possible", diff --git a/python/types/slam2d/py_parameter_se2_offset.h b/python/types/slam2d/py_parameter_se2_offset.h index 2b01d8326..218f42d60 100644 --- a/python/types/slam2d/py_parameter_se2_offset.h +++ b/python/types/slam2d/py_parameter_se2_offset.h @@ -9,8 +9,9 @@ inline void declareParameterSE2Offset(py::module& m) { py::class_>(m, "ParameterSE2Offset") .def(py::init<>()) - .def("set_offset", &ParameterSE2Offset::setOffset) - .def("offset", &ParameterSE2Offset::offset) + .def("set_param", &ParameterSE2Offset::setParam) + .def("set_param_data", &ParameterSE2Offset::setParameterData) + .def("param", &ParameterSE2Offset::param) .def("offset_matrix", &ParameterSE2Offset::offsetMatrix) .def("inverse_offset_matrix", &ParameterSE2Offset::inverseOffsetMatrix); diff --git a/python/types/slam3d/py_edge_se3.h b/python/types/slam3d/py_edge_se3.h index 108a4eab0..465b6b89f 100644 --- a/python/types/slam3d/py_edge_se3.h +++ b/python/types/slam3d/py_edge_se3.h @@ -36,7 +36,7 @@ inline void declareEdgeSE3(py::module& m) { .def(py::init<>()) .def("set_dimension", &EdgeSE3LotsOfXYZ::setDimension<-1>) - .def("set_size", &EdgeSE3LotsOfXYZ::setSize) + .def("resize", &EdgeSE3LotsOfXYZ::resize) .def("compute_error", &EdgeSE3LotsOfXYZ::computeError) .def("set_measurement_from_state", &EdgeSE3LotsOfXYZ::setMeasurementFromState) diff --git a/python/types/slam3d/py_parameter.h b/python/types/slam3d/py_parameter.h index 67bd35773..7d8fcfcc2 100644 --- a/python/types/slam3d/py_parameter.h +++ b/python/types/slam3d/py_parameter.h @@ -2,7 +2,6 @@ #include "g2o/types/slam3d/parameter_camera.h" #include "g2o/types/slam3d/parameter_se3_offset.h" -#include "g2o/types/slam3d/parameter_stereo_camera.h" #include "g2opy.h" namespace g2o { @@ -12,30 +11,22 @@ inline void declareSalm3dParameter(py::module& m) { std::shared_ptr>(m, "ParameterSE3Offset") .def(py::init<>()) - .def("set_offset", &ParameterSE3Offset::setOffset) - .def("offset", &ParameterSE3Offset::offset) + .def("set_param", &ParameterSE3Offset::setParam) + .def("set_param_data", &ParameterSE3Offset::setParameterData) + .def("param", &ParameterSE3Offset::param) .def("inverse_offset", &ParameterSE3Offset::inverseOffset); // class G2O_TYPES_SLAM3D_API CacheSE3Offset: public Cache // class G2O_TYPES_SLAM3D_API CacheSE3OffsetDrawAction: public DrawAction - py::class_>(m, "ParameterCamera") + py::class_>( + m, "ParameterCamera") .def(py::init<>()) - .def("setKcam", &ParameterCamera::setKcam) - .def("setOffset", &ParameterCamera::setOffset) - .def("Kcam", &ParameterCamera::Kcam) - .def("invKcam", &ParameterCamera::invKcam) - .def("Kcam_inverseOffsetR", &ParameterCamera::Kcam_inverseOffsetR); + .def("set_param", &ParameterCamera::setParam) + .def("param", &ParameterCamera::param); // class G2O_TYPES_SLAM3D_API CacheCamera: public CacheSE3Offset // class G2O_TYPES_SLAM3D_API CacheCameraDrawAction: public DrawAction - - py::class_>(m, "ParameterStereoCamera") - .def(py::init<>()) - .def("set_baseline", &ParameterStereoCamera::setBaseline) - .def("baseline", &ParameterStereoCamera::baseline); } } // namespace g2o diff --git a/todo.md b/todo.md new file mode 100644 index 000000000..684947fe9 --- /dev/null +++ b/todo.md @@ -0,0 +1,24 @@ +[x] Create parameter type for ParameterCamera to support load/save +[ ] Add typing for parameters to template? +[ ] Re-work python wrapping for parameters, templated base param +[ ] Test load/save for VertexCam (camera and baseline seem missing from traits) +[ ] Refactor Data container. Can it be a container instead of linked list? +[ ] Use FakeDependency in Traits +[x] EdgeSE2TwoPointsXY, EdgeSE2PointXYCalib can be fixed size edges -> IO test +[x] IO test fixture including parameter +[x] Type Based Tests for Jacobian +[x] support saving VectorX and MatrixX in G2O format +[x] update python optimizable graph wrapper (save/load) with format +[x] add version number for file format +[x] fix core API with exports for MSVC +[x] test save/load of dynamic vertex +[x] test save/load of dynamic edge +[x] binary format for save/load +[x] XML format for save/load +[ ] wrap abstract graph to python and update save wrapper +[x] ParameterContainer remove read/write +[x] remove read/write methods for vertex / edge + +[ ] use cmakedefine01 for compile flags in config +[ ] add test for setData / getData on the graph +[ ] update py_base_vertex/edge to map now common methods diff --git a/todo.txt b/todo.txt deleted file mode 100644 index 4963aa207..000000000 --- a/todo.txt +++ /dev/null @@ -1,10 +0,0 @@ -- use cmakedefine01 for compile flags in config - -- add test for setData / getData on the graph - -- update py_base_vertex/edge to map now common methods - -- read write methods based on type traits conversion to vector - - how to handle params? - -- investigate binary format for save/load diff --git a/unit_test/CMakeLists.txt b/unit_test/CMakeLists.txt index b3b2db700..e333d179f 100644 --- a/unit_test/CMakeLists.txt +++ b/unit_test/CMakeLists.txt @@ -8,10 +8,12 @@ FetchContent_Declare( set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) -macro(create_test target) - target_link_libraries(${target} gtest gmock gtest_main) +set(UNITTEST_BASE_DIR "${CMAKE_CURRENT_LIST_DIR}") +function(create_test target) + target_sources(${target} PRIVATE ${UNITTEST_BASE_DIR}/test_helper/test_main.cpp) + target_link_libraries(${target} gtest gmock) add_test (NAME ${target} COMMAND $) -endmacro(create_test) +endfunction() add_subdirectory(test_helper) add_subdirectory(general) @@ -19,9 +21,7 @@ add_subdirectory(data) add_subdirectory(stuff) add_subdirectory(sclam2d) add_subdirectory(slam2d) -add_subdirectory(slam2d_addons) add_subdirectory(slam3d) -add_subdirectory(slam3d_addons) -add_subdirectory(sim3) add_subdirectory(sba) +add_subdirectory(sim3) add_subdirectory(solver) diff --git a/unit_test/general/CMakeLists.txt b/unit_test/general/CMakeLists.txt index 8ffda6892..f9f128466 100644 --- a/unit_test/general/CMakeLists.txt +++ b/unit_test/general/CMakeLists.txt @@ -1,6 +1,8 @@ add_executable(unittest_general auto_diff.cpp batch_statistics.cpp + graph_io.cpp + graph_io_dynamic.cpp graph_operations.cpp clear_and_redo.cpp base_fixed_sized_edge.cpp @@ -8,5 +10,6 @@ add_executable(unittest_general sparse_block_matrix.cpp type_traits_tests.cpp ) -target_link_libraries(unittest_general unittest_helper types_slam3d types_slam2d) +target_link_libraries(unittest_general unittest_helper) +target_link_libraries(unittest_general types_slam3d types_slam2d types_data) create_test(unittest_general) diff --git a/unit_test/general/auto_diff.cpp b/unit_test/general/auto_diff.cpp index 0ea573325..9690498eb 100644 --- a/unit_test/general/auto_diff.cpp +++ b/unit_test/general/auto_diff.cpp @@ -38,9 +38,6 @@ class VertexFlatSE2 : public g2o::BaseVertex<3, g2o::Vector3> { estimate_ += update.head(); estimate_(2) = g2o::normalize_theta(estimate_(2)); } - - bool read(std::istream&) override { return false; } - bool write(std::ostream&) const override { return false; } }; /** @@ -78,10 +75,6 @@ class Edge3ADTester // add the AD interface G2O_MAKE_AUTO_AD_FUNCTIONS - - // NOOPs - bool read(std::istream&) override { return false; }; - bool write(std::ostream&) const override { return false; }; }; /** @@ -165,12 +158,6 @@ class AutoDifferentiation : public ::testing::Test { TEST_F(AutoDifferentiation, ComputesSomething) { testEdge_.linearizeOplus(jacobianWorkspace_); -#if 0 - std::cerr << PVAR(testEdge.jacobianOplusXn<0>()) << std::endl; - std::cerr << PVAR(testEdge.jacobianOplusXn<1>()) << std::endl; - std::cerr << PVAR(testEdge.jacobianOplusXn<2>()) << std::endl; -#endif - ASSERT_FALSE(testEdge_.jacobianOplusXn<0>().array().isNaN().any()) << "Jacobian contains NaN"; ASSERT_FALSE(testEdge_.jacobianOplusXn<1>().array().isNaN().any()) @@ -242,11 +229,6 @@ class AutoDifferentiationEdgeSE2 : public ::testing::Test { TEST_F(AutoDifferentiationEdgeSE2, AdComputesSomething) { testEdgeAd_.linearizeOplus(jacobianWorkspaceAd_); -#if 0 - std::cerr << PVAR(testEdgeAd.jacobianOplusXn<0>()) << std::endl; - std::cerr << PVAR(testEdgeAd.jacobianOplusXn<1>()) << std::endl; -#endif - ASSERT_FALSE(testEdgeAd_.jacobianOplusXn<0>().array().isNaN().any()) << "Jacobian contains NaN"; ASSERT_FALSE(testEdgeAd_.jacobianOplusXn<1>().array().isNaN().any()) diff --git a/unit_test/general/base_fixed_sized_edge.cpp b/unit_test/general/base_fixed_sized_edge.cpp index bd624bcad..d40a19d72 100644 --- a/unit_test/general/base_fixed_sized_edge.cpp +++ b/unit_test/general/base_fixed_sized_edge.cpp @@ -46,8 +46,6 @@ class Edge3Constant const auto c = vertexXnRaw<2>()->estimate(); error_ = (a * b * c - measurement_).eval(); } - bool read(std::istream&) override { return false; }; - bool write(std::ostream&) const override { return false; }; }; class Edge3Dynamic : public g2o::BaseVariableSizedEdge<2, g2o::Vector2> { @@ -60,8 +58,6 @@ class Edge3Dynamic : public g2o::BaseVariableSizedEdge<2, g2o::Vector2> { static_cast(vertexRaw(2))->estimate(); error_ = (a * b * c - measurement_).eval(); } - bool read(std::istream&) override { return false; }; - bool write(std::ostream&) const override { return false; }; }; class VertexNotDefaultCtor : public g2o::BaseVertex<2, g2o::Vector2> { @@ -71,9 +67,6 @@ class VertexNotDefaultCtor : public g2o::BaseVertex<2, g2o::Vector2> { void oplusImpl(const g2o::VectorX::MapType& update) override { estimate_ += update.head<2>(); } - - bool read(std::istream& /*is*/) override { return false; }; - bool write(std::ostream& /*os*/) const override { return false; }; }; class EdgeUnaryCreateVertexTester @@ -85,8 +78,6 @@ class EdgeUnaryCreateVertexTester const VertexNotDefaultCtor* v = vertexXnRaw<0>(); error_ = v->estimate() - measurement_; } - bool read(std::istream& /*is*/) override { return false; }; - bool write(std::ostream& /*os*/) const override { return false; }; void setMeasurement(const g2o::Vector2& m) override { measurement_ = m; } }; diff --git a/unit_test/general/clear_and_redo.cpp b/unit_test/general/clear_and_redo.cpp index 0a989db0b..0b39ebbb9 100644 --- a/unit_test/general/clear_and_redo.cpp +++ b/unit_test/general/clear_and_redo.cpp @@ -28,7 +28,8 @@ #include "g2o/core/sparse_optimizer.h" #include "g2o/core/sparse_optimizer_terminate_action.h" -#include "g2o/types/slam3d/types_slam3d.h" +#include "g2o/types/slam3d/edge_se3.h" +#include "g2o/types/slam3d/vertex_se3.h" #include "unit_test/test_helper/allocate_optimizer.h" TEST(General, ClearAndRedo) { diff --git a/unit_test/general/graph_io.cpp b/unit_test/general/graph_io.cpp new file mode 100644 index 000000000..6917dd742 --- /dev/null +++ b/unit_test/general/graph_io.cpp @@ -0,0 +1,318 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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 +#include +#include +#include +#include +#include +#include + +#include "g2o/config.h" // IWYU pragma: keep +#include "g2o/core/abstract_graph.h" +#include "g2o/core/io/io_format.h" +#include "g2o/core/optimizable_graph.h" +#include "g2o/core/sparse_optimizer.h" +#include "g2o/types/data/vertex_tag.h" +#include "g2o/types/slam2d/edge_se2.h" +#include "g2o/types/slam2d/vertex_se2.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "unit_test/test_helper/allocate_optimizer.h" + +using namespace testing; // NOLINT + +MATCHER(ParamEqual, "") { + const auto& that = std::get<0>(arg); + const auto& expected = std::get<1>(arg); + return ExplainMatchResult( + AllOf(Field("tag", &g2o::AbstractGraph::AbstractParameter::tag, + Eq(expected.tag)), + Field("id", &g2o::AbstractGraph::AbstractParameter::id, + Eq(expected.id)), + Field("value", &g2o::AbstractGraph::AbstractParameter::value, + ElementsAreArray(expected.value))), + that, result_listener); +} + +MATCHER(VertexEqual, "") { + const auto& that = std::get<0>(arg); + const auto& expected = std::get<1>(arg); + // TODO(Rainer): compare data vector + return ExplainMatchResult( + AllOf( + Field("tag", &g2o::AbstractGraph::AbstractVertex::tag, + Eq(expected.tag)), + Field("id", &g2o::AbstractGraph::AbstractVertex::id, Eq(expected.id)), + Field("estimate", &g2o::AbstractGraph::AbstractVertex::estimate, + ElementsAreArray(expected.estimate))), + that, result_listener); +} + +MATCHER(EdgeEqual, "") { + const auto& that = std::get<0>(arg); + const auto& expected = std::get<1>(arg); + // TODO(Rainer): compare data vector + return ExplainMatchResult( + AllOf(Field("tag", &g2o::AbstractGraph::AbstractEdge::tag, + Eq(expected.tag)), + Field("ids", &g2o::AbstractGraph::AbstractEdge::ids, + ElementsAreArray(expected.ids)), + Field("param_ids", &g2o::AbstractGraph::AbstractEdge::param_ids, + ElementsAreArray(expected.param_ids)), + Field("measurement", &g2o::AbstractGraph::AbstractEdge::measurement, + ElementsAreArray(expected.measurement)), + Field("information", &g2o::AbstractGraph::AbstractEdge::information, + ElementsAreArray(expected.information))), + that, result_listener); +} + +namespace { +auto KeyMatch(const std::vector& keys) { + std::vector matchers; + matchers.reserve(keys.size()); + for (const auto& val : keys) matchers.push_back(Key(val)); + return UnorderedElementsAreArray(matchers); +}; +} // namespace + +/** + * @brief Test fixture for IO with the abstract graph. + */ +class AbstractGraphIO : public TestWithParam { + protected: + void SetUp() override { + abstract_graph_.fixed() = {1}; + + abstract_graph_.parameters().emplace_back("PARAMS_SE2OFFSET", 42, + std::vector{12., 13.}); + + abstract_graph_.vertices().emplace_back("VERTEX_SE2", 1, + std::vector{1., 2., 3.}); + abstract_graph_.vertices().emplace_back("VERTEX_SE2", 2, + std::vector{2., 3., 4.}); + abstract_graph_.vertices().emplace_back( + "VERTEX_SE2", 3, std::vector{5., 6., 7.}, + std::vector{ + {"VERTEX_TAG", "fancy data"}}); + + abstract_graph_.edges().emplace_back( + "EDGE_SE2", std::vector{1, 2}, std::vector{0.1, 0.2, 0.3}, + std::vector{1., 2., 3., 4., 5., 6.}, std::vector(), + std::vector{ + {"VERTEX_TAG", "more fancy data"}}); + abstract_graph_.edges().emplace_back( + "EDGE_SE2", std::vector{2, 3}, std::vector{0.4, 0.5, 0.6}, + std::vector{1.1, 2.1, 3.1, 4.1, 5.1, 6.1}); + } + g2o::AbstractGraph abstract_graph_; +}; + +TEST_F(AbstractGraphIO, RenameTags) { + EXPECT_THAT(abstract_graph_.vertices(), + Contains(testing::Field(&g2o::AbstractGraph::AbstractVertex::tag, + Eq("VERTEX_SE2"))) + .Times(Ge(1))); + + auto v3_it = std::find_if( + abstract_graph_.vertices().begin(), abstract_graph_.vertices().end(), + [](const g2o::AbstractGraph::AbstractVertex& v) { return v.id == 3; }); + EXPECT_THAT(v3_it->data, + Contains(testing::Field(&g2o::AbstractGraph::AbstractData::tag, + Eq("VERTEX_TAG"))) + .Times(Ge(1))); + + std::unordered_map tag_map = { + {"VERTEX_SE2", "VERTEX_BAR"}, {"VERTEX_TAG", "TAG_FOO"}}; + abstract_graph_.renameTags(tag_map); + + EXPECT_THAT(abstract_graph_.vertices(), + Contains(testing::Field(&g2o::AbstractGraph::AbstractVertex::tag, + Eq("VERTEX_SE2"))) + .Times(0)); + EXPECT_THAT(abstract_graph_.vertices(), + Contains(testing::Field(&g2o::AbstractGraph::AbstractVertex::tag, + Eq("VERTEX_BAR"))) + .Times(Ge(1))); + EXPECT_THAT(v3_it->data, + Contains(testing::Field(&g2o::AbstractGraph::AbstractData::tag, + Eq("VERTEX_TAG"))) + .Times(0)); + EXPECT_THAT(v3_it->data, + Contains(testing::Field(&g2o::AbstractGraph::AbstractData::tag, + Eq("TAG_FOO"))) + .Times(Ge(1))); +} + +TEST_P(AbstractGraphIO, SaveAndLoad) { + g2o::io::Format format = GetParam(); + g2o::AbstractGraph load_save_graph(abstract_graph_); + + std::stringstream buffer(format == g2o::io::Format::kBinary + ? std::ios_base::binary | std::ios_base::in | + std::ios_base::out + : std::ios_base::in | std::ios_base::out); + bool save_result = load_save_graph.save(buffer, format); + ASSERT_THAT(save_result, IsTrue()); + EXPECT_THAT(buffer.str(), Not(IsEmpty())); + + load_save_graph.clear(); + EXPECT_THAT(load_save_graph.fixed(), IsEmpty()); + EXPECT_THAT(load_save_graph.parameters(), IsEmpty()); + EXPECT_THAT(load_save_graph.vertices(), IsEmpty()); + EXPECT_THAT(load_save_graph.edges(), IsEmpty()); + + bool load_status = load_save_graph.load(buffer, format); + ASSERT_THAT(load_status, IsTrue()); + + EXPECT_THAT(load_save_graph.fixed(), + ElementsAreArray(abstract_graph_.fixed())); + EXPECT_THAT(load_save_graph.parameters(), + Pointwise(ParamEqual(), abstract_graph_.parameters())); + EXPECT_THAT(load_save_graph.vertices(), + Pointwise(VertexEqual(), abstract_graph_.vertices())); + EXPECT_THAT(load_save_graph.edges(), + Pointwise(EdgeEqual(), abstract_graph_.edges())); +} + +/** + * @brief Test fixture for IO with the abstract graph. + */ +class OptimizableGraphIO : public TestWithParam { + protected: + void SetUp() override { + optimizer_ptr_ = g2o::internal::createOptimizerForTests(); + + // Add vertices + auto v0 = std::make_shared(); + v0->setFixed(true); + v0->setId(0); + optimizer_ptr_->addVertex(v0); + + auto data = std::make_shared(); + data->setName("vertex_foobar"); + data->setHostname("my_robot"); + v0->setUserData(data); + + auto v1 = std::make_shared(); + v1->setId(1); + optimizer_ptr_->addVertex(v1); + + auto v2 = std::make_shared(); + v2->setId(2); + optimizer_ptr_->addVertex(v2); + + // Add edges + auto e1 = std::make_shared(); + e1->vertices()[0] = optimizer_ptr_->vertex(0); + e1->vertices()[1] = optimizer_ptr_->vertex(1); + e1->setMeasurement(g2o::SE2(1, 0, 0)); + e1->setInformation(g2o::MatrixN<3>::Identity()); + optimizer_ptr_->addEdge(e1); + + auto edge_data = std::make_shared(); + edge_data->setName("foobar"); + edge_data->setHostname("my_robot"); + e1->setUserData(edge_data); + + auto e2 = std::make_shared(); + e2->vertices()[0] = optimizer_ptr_->vertex(1); + e2->vertices()[1] = optimizer_ptr_->vertex(2); + e2->setMeasurement(g2o::SE2(0, 1, 0)); + e2->setInformation(g2o::MatrixN<3>::Identity()); + optimizer_ptr_->addEdge(e2); + } + std::unique_ptr optimizer_ptr_; +}; + +TEST_P(OptimizableGraphIO, SaveAndLoad) { + g2o::io::Format format = GetParam(); + + std::stringstream buffer(format == g2o::io::Format::kBinary + ? std::ios_base::binary | std::ios_base::in | + std::ios_base::out + : std::ios_base::in | std::ios_base::out); + bool save_result = optimizer_ptr_->save(buffer, format); + ASSERT_THAT(save_result, IsTrue()); + EXPECT_THAT(buffer.str(), Not(IsEmpty())); + + auto loaded_optimizer = g2o::internal::createOptimizerForTests(); + loaded_optimizer->load(buffer, format); + + EXPECT_THAT(loaded_optimizer->vertices(), + SizeIs(optimizer_ptr_->vertices().size())); + + const std::vector expected_keys = [this]() { + std::vector result; + for (const auto& p : optimizer_ptr_->vertices()) result.push_back(p.first); + return result; + }(); + EXPECT_THAT(loaded_optimizer->vertices(), KeyMatch(expected_keys)); + + const std::vector loaded_fixed = [&loaded_optimizer]() { + std::vector result; + for (const auto& p : loaded_optimizer->vertices()) + if (static_cast(p.second.get())->fixed()) + result.push_back(p.first); + return result; + }(); + const std::vector expected_fixed = [this]() { + std::vector result; + for (const auto& p : optimizer_ptr_->vertices()) + if (static_cast(p.second.get())->fixed()) + result.push_back(p.first); + return result; + }(); + EXPECT_THAT(loaded_fixed, UnorderedElementsAreArray(expected_fixed)); + + EXPECT_THAT(loaded_optimizer->edges(), + SizeIs(optimizer_ptr_->edges().size())); + // TODO(Rainer): Compare estimates of vertices + // TODO(Rainer): Compare measurement of edges + // TODO(Rainer): Compare information of edges + + // Brutally check that serialization result is the same + std::stringstream buffer_after_loading; + save_result = loaded_optimizer->save(buffer_after_loading, format); + ASSERT_THAT(save_result, IsTrue()); + EXPECT_THAT(buffer.str(), Eq(buffer_after_loading.str())); +} + +namespace { +// We can always test G2O format, others depend on libraries +const auto kFileformatsToTest = Values( + g2o::io::Format::kG2O +#ifdef G2O_HAVE_CEREAL + , + g2o::io::Format::kJson, g2o::io::Format::kXML, g2o::io::Format::kBinary +#endif +); +} // namespace + +INSTANTIATE_TEST_SUITE_P(AbstractGraph, AbstractGraphIO, kFileformatsToTest); +INSTANTIATE_TEST_SUITE_P(OptimizableGraphGraph, OptimizableGraphIO, + kFileformatsToTest); diff --git a/unit_test/general/graph_io_dynamic.cpp b/unit_test/general/graph_io_dynamic.cpp new file mode 100644 index 000000000..f233bec63 --- /dev/null +++ b/unit_test/general/graph_io_dynamic.cpp @@ -0,0 +1,190 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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 +#include +#include +#include + +#include "g2o/config.h" // IWYU pragma: keep +#include "g2o/core/base_dynamic_vertex.h" +#include "g2o/core/base_unary_edge.h" +#include "g2o/core/eigen_types.h" +#include "g2o/core/factory.h" +#include "g2o/core/io/io_format.h" +#include "g2o/core/sparse_optimizer.h" +#include "g2o/types/slam2d/edge_se2_lotsofxy.h" +#include "g2o/types/slam2d/vertex_point_xy.h" +#include "g2o/types/slam2d/vertex_se2.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "unit_test/test_helper/allocate_optimizer.h" + +using namespace testing; // NOLINT + +class DynamicVertex : public g2o::BaseDynamicVertex { + public: + DynamicVertex() = default; + + void oplusImpl(const g2o::VectorX::MapType& update) override { + estimate_ += update; + } + + protected: + bool setDimensionImpl(int newDimension) override { + estimate_.resize(newDimension); + return true; + } +}; + +class EdgeForDynamicVertex + : public g2o::BaseUnaryEdge<2, g2o::Vector2, DynamicVertex> { + public: + void computeError() override { error_.setZero(); } +}; + +/** + * @brief Test fixture for IO with the abstract graph. + */ +class OptimizableGraphDynamicEdgeIO + : public TestWithParam> { + protected: + void SetUp() override { + // Register the specific types of the test once + g2o::Factory* factory = g2o::Factory::instance(); + if (!factory->knowsTag("TEST_DYN_VERTEX")) { + factory->registerType( + "TEST_DYN_VERTEX", + std::make_unique>()); + } + if (!factory->knowsTag("TEST_EDGE_FOR_DYN")) { + factory->registerType( + "TEST_EDGE_FOR_DYN", + std::make_unique< + g2o::HyperGraphElementCreator>()); + } + + optimizer_ptr_ = g2o::internal::createOptimizerForTests(); + + // Add vertices + auto pose = std::make_shared(); + pose->setFixed(true); + pose->setId(0); + optimizer_ptr_->addVertex(pose); + + auto point0 = std::make_shared(); + point0->setId(1); + point0->setEstimate(g2o::Vector2::Random()); + optimizer_ptr_->addVertex(point0); + + auto point1 = std::make_shared(); + point1->setId(2); + point1->setEstimate(g2o::Vector2::Random()); + optimizer_ptr_->addVertex(point1); + + auto point2 = std::make_shared(); + point2->setId(3); + point2->setEstimate(g2o::Vector2::Random()); + optimizer_ptr_->addVertex(point2); + + // Add dynamically sized vertex + constexpr int kDynVertexSize = 5; + auto dyn_vertex = std::make_shared(); + dyn_vertex->setId(4); + dyn_vertex->setDimension(kDynVertexSize); + dyn_vertex->setEstimate(g2o::VectorX::Random(kDynVertexSize)); + optimizer_ptr_->addVertex(dyn_vertex); + + auto edge = std::make_shared(); + edge->resize(4); + edge->vertices()[0] = pose; + edge->vertices()[1] = point0; + edge->vertices()[2] = point1; + edge->vertices()[3] = point2; + edge->setInformation( + g2o::VectorX::Ones(edge->information().cols()).asDiagonal()); + edge->setMeasurementFromState(); + optimizer_ptr_->addEdge(edge); + + auto edge_for_dyn = std::make_shared(); + edge_for_dyn->setInformation( + EdgeForDynamicVertex::InformationType::Identity() * 5.); + edge_for_dyn->setVertex(0, dyn_vertex); + optimizer_ptr_->addEdge(edge_for_dyn); + } + std::unique_ptr optimizer_ptr_; +}; + +TEST_P(OptimizableGraphDynamicEdgeIO, SaveAndLoad) { + const g2o::io::Format format = std::get<0>(GetParam()); + const bool supports_dynamic_edge = std::get<1>(GetParam()); + + std::stringstream buffer(format == g2o::io::Format::kBinary + ? std::ios_base::binary | std::ios_base::in | + std::ios_base::out + : std::ios_base::in | std::ios_base::out); + bool save_result = optimizer_ptr_->save(buffer, format); + ASSERT_THAT(save_result, IsTrue()); + EXPECT_THAT(buffer.str(), Not(IsEmpty())); + if (!supports_dynamic_edge) { + SUCCEED(); + return; + } + + auto loaded_optimizer = g2o::internal::createOptimizerForTests(); + loaded_optimizer->load(buffer, format); + + EXPECT_THAT(loaded_optimizer->vertices(), + SizeIs(optimizer_ptr_->vertices().size())); + + EXPECT_THAT(loaded_optimizer->vertices(), + UnorderedElementsAre(Key(0), Key(1), Key(2), Key(3), Key(4))); + + EXPECT_THAT(loaded_optimizer->edges(), + SizeIs(optimizer_ptr_->edges().size())); + + // Brutally check that serialization result is the same + std::stringstream buffer_after_loading; + save_result = loaded_optimizer->save(buffer_after_loading, format); + ASSERT_THAT(save_result, IsTrue()); + EXPECT_THAT(buffer.str(), Eq(buffer_after_loading.str())); +} + +namespace { +// We can always test G2O format, others depend on libraries +const auto kFileformatsToTest = + Values(std::make_tuple(g2o::io::Format::kG2O, true) +#ifdef G2O_HAVE_CEREAL + , + std::make_tuple(g2o::io::Format::kJson, true), + std::make_tuple(g2o::io::Format::kXML, true), + std::make_tuple(g2o::io::Format::kBinary, true) +#endif + ); +} // namespace + +INSTANTIATE_TEST_SUITE_P(OptimizableGraphDynamic, OptimizableGraphDynamicEdgeIO, + kFileformatsToTest); diff --git a/unit_test/general/graph_operations.cpp b/unit_test/general/graph_operations.cpp index d838d0cea..c8cbfef06 100644 --- a/unit_test/general/graph_operations.cpp +++ b/unit_test/general/graph_operations.cpp @@ -32,6 +32,7 @@ #include #include #include +#include #include "g2o/core/eigen_types.h" #include "g2o/core/factory.h" @@ -42,8 +43,14 @@ #include "g2o/core/optimization_algorithm_property.h" #include "g2o/core/sparse_optimizer.h" #include "g2o/stuff/string_tools.h" -#include "g2o/types/slam2d/types_slam2d.h" +#include "g2o/types/slam2d/edge_se2.h" +#include "g2o/types/slam2d/vertex_point_xy.h" +#include "g2o/types/slam2d/vertex_se2.h" #include "unit_test/test_helper/allocate_optimizer.h" +#include "unit_test/test_helper/eigen_matcher.h" + +using g2o::internal::print_wrap; +using namespace testing; // NOLINT G2O_USE_TYPE_GROUP(slam2d); // NOLINT @@ -176,6 +183,81 @@ TEST(General, GraphChangeId) { EXPECT_EQ(v2->id(), 1); } +class PublicRenamedTypesGraph : public g2o::OptimizableGraph { + public: + const std::unordered_map& renamedTypesLookup() + const { + return renamedTypesLookup_; + } +}; + +TEST(General, RenamedTypesFromString) { + using namespace testing; // NOLINT + PublicRenamedTypesGraph optimizer; + + EXPECT_THAT(optimizer.renamedTypesLookup(), IsEmpty()); + + optimizer.setRenamedTypesFromString("VERTEX_SE2:foobar"); + ASSERT_THAT(optimizer.renamedTypesLookup(), IsEmpty()); + + optimizer.setRenamedTypesFromString("VERTEX_SE2:foobar=DoesNotExist"); + ASSERT_THAT(optimizer.renamedTypesLookup(), IsEmpty()); + + optimizer.setRenamedTypesFromString("VERTEX_SE2:foobar=VERTEX_SE2"); + EXPECT_THAT(optimizer.renamedTypesLookup(), SizeIs(1)); + EXPECT_THAT(optimizer.renamedTypesLookup(), + Contains(Key("VERTEX_SE2:foobar"))); + EXPECT_THAT(optimizer.renamedTypesLookup(), + Contains(Pair("VERTEX_SE2:foobar", "VERTEX_SE2"))); +} + +TEST(General, SetEstimateData) { + g2o::VertexSE2 vertex; + + std::vector data_vector = {1., 2., 3.}; + vertex.setEstimateData(data_vector.data()); + EXPECT_THAT( + print_wrap(vertex.estimate().toVector()), + g2o::internal::EigenApproxEqual(print_wrap(g2o::Vector3(1, 2, 3)), 1e-3)); + + vertex.setEstimateData(g2o::Vector3(3, 2, 1)); + EXPECT_THAT( + print_wrap(vertex.estimate().toVector()), + g2o::internal::EigenApproxEqual(print_wrap(g2o::Vector3(3, 2, 1)), 1e-3)); + + vertex.setEstimateData(std::vector({0.1, 0.2, 0.3})); + EXPECT_THAT(print_wrap(vertex.estimate().toVector()), + g2o::internal::EigenApproxEqual( + print_wrap(g2o::Vector3(0.1, 0.2, 0.3)), 1e-3)); + + g2o::VectorX estimate_data; + ASSERT_THAT(vertex.getEstimateData(estimate_data), IsTrue()); + EXPECT_THAT(print_wrap(estimate_data), + g2o::internal::EigenApproxEqual( + print_wrap(g2o::Vector3(0.1, 0.2, 0.3)), 1e-3)); + + vertex.setMinimalEstimateData(data_vector.data()); + EXPECT_THAT( + print_wrap(vertex.estimate().toVector()), + g2o::internal::EigenApproxEqual(print_wrap(g2o::Vector3(1, 2, 3)), 1e-3)); + + vertex.setMinimalEstimateData(g2o::Vector3(3, 2, 1)); + EXPECT_THAT( + print_wrap(vertex.estimate().toVector()), + g2o::internal::EigenApproxEqual(print_wrap(g2o::Vector3(3, 2, 1)), 1e-3)); + + vertex.setMinimalEstimateData(std::vector({0.1, 0.2, 0.3})); + EXPECT_THAT(print_wrap(vertex.estimate().toVector()), + g2o::internal::EigenApproxEqual( + print_wrap(g2o::Vector3(0.1, 0.2, 0.3)), 1e-3)); + + estimate_data.setZero(); + ASSERT_THAT(vertex.getMinimalEstimateData(estimate_data), IsTrue()); + EXPECT_THAT(print_wrap(estimate_data), + g2o::internal::EigenApproxEqual( + print_wrap(g2o::Vector3(0.1, 0.2, 0.3)), 1e-3)); +} + /** * Fixture to test saving and loading of a graph. * Here, we will have a simple graph with N nodes and N edges. @@ -320,14 +402,14 @@ TEST_F(GeneralGraphOperations, SavingGraph) { EXPECT_THAT(edgeIds, testing::ElementsAreArray(expectedEdgeIds())); } -namespace internal { +namespace { using KeyIntVector = std::vector; static KeyIntVector VectorIntToKeys(const std::vector& keys) { // NOLINT KeyIntVector matchers; for (const auto& val : keys) matchers.push_back(testing::Key(val)); return matchers; } -} // namespace internal +} // namespace TEST_F(GeneralGraphOperations, LoadingGraph) { std::stringstream graphData; @@ -344,9 +426,8 @@ TEST_F(GeneralGraphOperations, LoadingGraph) { ASSERT_THAT(optimizer_->edges(), testing::SizeIs(kNumVertices)); ASSERT_THAT(optimizer_->dimensions(), testing::ElementsAre(3)); - ASSERT_THAT(optimizer_->vertices(), - testing::UnorderedElementsAreArray( - internal::VectorIntToKeys(expectedIds()))); + ASSERT_THAT(optimizer_->vertices(), testing::UnorderedElementsAreArray( + VectorIntToKeys(expectedIds()))); ASSERT_THAT( optimizer_->edges(), testing::Each(testing::Pointee(testing::Property( @@ -378,8 +459,8 @@ TEST_F(GeneralGraphOperations, SaveSubsetVertices) { optimizer_->load(graphData); EXPECT_THAT(optimizer_->vertices(), testing::SizeIs(2)); EXPECT_THAT(optimizer_->edges(), testing::SizeIs(1)); - EXPECT_THAT(optimizer_->vertices(), testing::UnorderedElementsAreArray( - internal::VectorIntToKeys({0, 1}))); + EXPECT_THAT(optimizer_->vertices(), + testing::UnorderedElementsAreArray(VectorIntToKeys({0, 1}))); } TEST_F(GeneralGraphOperations, SaveSubsetEdges) { @@ -400,8 +481,8 @@ TEST_F(GeneralGraphOperations, SaveSubsetEdges) { optimizer_->load(graphData); EXPECT_THAT(optimizer_->vertices(), testing::SizeIs(2)); EXPECT_THAT(optimizer_->edges(), testing::SizeIs(1)); - EXPECT_THAT(optimizer_->vertices(), testing::UnorderedElementsAreArray( - internal::VectorIntToKeys({1, 2}))); + EXPECT_THAT(optimizer_->vertices(), + testing::UnorderedElementsAreArray(VectorIntToKeys({1, 2}))); } TEST_F(GeneralGraphOperations, PushPopActiveVertices) { diff --git a/unit_test/general/type_traits_tests.cpp b/unit_test/general/type_traits_tests.cpp index 92c79341a..31151d1a1 100644 --- a/unit_test/general/type_traits_tests.cpp +++ b/unit_test/general/type_traits_tests.cpp @@ -24,7 +24,7 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include +#include #include "g2o/core/eigen_types.h" #include "g2o/core/type_traits.h" @@ -59,8 +59,36 @@ TEST(TypeTraits, VectorFixed) { EXPECT_THAT(g2o::internal::print_wrap( Traits::fromMinimalVector(g2o::Vector3(0, 1, 2))), EigenEqual(g2o::internal::print_wrap(g2o::Vector3(0, 1, 2)))); - EXPECT_THAT(g2o::internal::print_wrap(Traits::Identity()), - EigenEqual(g2o::internal::print_wrap(g2o::Vector3::Zero()))); +} + +TEST(TypeTraits, VectorDynamic) { + using Traits = g2o::TypeTraits; + EXPECT_THAT(Traits::kVectorDimension, Eq(Eigen::Dynamic)); + EXPECT_THAT(Traits::kMinimalVectorDimension, Eq(Eigen::Dynamic)); + EXPECT_THAT(Traits::kIsVector, Eq(1)); + EXPECT_THAT(Traits::kIsScalar, Eq(0)); + EXPECT_THAT( + g2o::internal::print_wrap(Traits::toVector(g2o::VectorX::Ones(4))), + EigenEqual(g2o::internal::print_wrap(g2o::VectorX::Ones(4)))); + g2o::VectorX data(3); + g2o::VectorX input(3); + input << 1, 2, 3; + Traits::toData(input, data.data()); + EXPECT_THAT(g2o::internal::print_wrap(input), + EigenEqual(g2o::internal::print_wrap(data))); + EXPECT_THAT( + g2o::internal::print_wrap(Traits::toMinimalVector(g2o::VectorX::Ones(4))), + EigenEqual(g2o::internal::print_wrap(g2o::VectorX::Ones(4)))); + input << 0, 1, 2; + Traits::toMinimalData(input, data.data()); + EXPECT_THAT(g2o::internal::print_wrap(Traits::toVector(input)), + EigenEqual(g2o::internal::print_wrap(data))); + EXPECT_THAT( + g2o::internal::print_wrap(Traits::fromVector(g2o::Vector3(0, 1, 2))), + EigenEqual(g2o::internal::print_wrap(input))); + EXPECT_THAT(g2o::internal::print_wrap( + Traits::fromMinimalVector(g2o::Vector3(0, 1, 2))), + EigenEqual(g2o::internal::print_wrap(input))); } TEST(TypeTraits, Double) { @@ -83,7 +111,6 @@ TEST(TypeTraits, Double) { EigenEqual(g2o::internal::print_wrap(data))); EXPECT_THAT(Traits::fromVector(Vec1(2.)), Eq(2.)); EXPECT_THAT(Traits::fromMinimalVector(Vec1(3.)), Eq(3.)); - EXPECT_THAT(Traits::Identity(), Eq(0.)); } TEST(TypeTraits, Dimension) { diff --git a/unit_test/sba/CMakeLists.txt b/unit_test/sba/CMakeLists.txt index 24136cc7a..ec2211726 100644 --- a/unit_test/sba/CMakeLists.txt +++ b/unit_test/sba/CMakeLists.txt @@ -1,6 +1,5 @@ add_executable(unittest_sba - io_sba.cpp - io_six_dof_expmap.cpp +sba_basic_tests.cpp ) -target_link_libraries(unittest_sba types_sba) +target_link_libraries(unittest_sba unittest_helper types_sba) create_test(unittest_sba) diff --git a/unit_test/sba/io_sba.cpp b/unit_test/sba/io_sba.cpp deleted file mode 100644 index bde0b3bfc..000000000 --- a/unit_test/sba/io_sba.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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 - -#include "g2o/core/eigen_types.h" -#include "g2o/types/sba/types_sba.h" -#include "gtest/gtest.h" -#include "unit_test/test_helper/io.h" -#include "unit_test/test_helper/random_state.h" - -using namespace g2o; - -namespace { - -struct RandomIntrinsics { - static VertexIntrinsicsEstimate create() { - VertexIntrinsicsEstimate res; - res.values = VectorN<5>::Random(); - return res; - } - static bool isApprox(const VertexIntrinsicsEstimate& a, - const VertexIntrinsicsEstimate& b) { - return a.values.isApprox(b.values, 1e-5); - } -}; - -struct RandomSBACam { - static SBACam create() { return SBACam(); } // TODO(goki): Randomize - static bool isApprox(const SBACam& a, const SBACam& b) { - return a.toVector().isApprox(b.toVector(), 1e-5) && - a.Kcam.isApprox(b.Kcam, 1e-5); - } -}; - -struct RandomSE3Quat { - static SE3Quat create() { return SE3Quat(); } // TODO(goki): Randomize - static bool isApprox(const SE3Quat& a, const SE3Quat& b) { - return a.toVector().isApprox(b.toVector(), 1e-5); - } -}; -} // namespace - -/* - * VERTEX Tests - */ -TEST(IoSba, ReadWriteVertexIntrinsics) { - readWriteVectorBasedVertex(); -} - -TEST(IoSba, ReadWriteVertexCam) { - readWriteVectorBasedVertex(); -} - -/* - * EDGE Tests - */ -TEST(IoSba, ReadWriteEdgeProjectP2MC) { - readWriteVectorBasedEdge(); -} - -TEST(IoSba, ReadWriteEdgeProjectP2SC) { - readWriteVectorBasedEdge(); -} - -TEST(IoSba, ReadWriteEdgeSBACam) { - readWriteVectorBasedEdge(); -} - -TEST(IoSba, ReadWriteEdgeSBAScale) { - readWriteVectorBasedEdge(); -} diff --git a/unit_test/sba/io_six_dof_expmap.cpp b/unit_test/sba/io_six_dof_expmap.cpp deleted file mode 100644 index 3f9cf5834..000000000 --- a/unit_test/sba/io_six_dof_expmap.cpp +++ /dev/null @@ -1,121 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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 - -#include "g2o/types/sba/types_six_dof_expmap.h" -#include "gtest/gtest.h" -#include "unit_test/test_helper/io.h" -#include "unit_test/test_helper/random_state.h" - -using namespace std; -using namespace g2o; - -/* - * VERTEX Tests - */ -TEST(IoSixDofExpmap, ReadWriteVertexSE3Expmap) { - readWriteVectorBasedVertex(); -} - -/* - * EDGE Tests - */ -TEST(IoSixDofExpmap, ReadWriteEdgeSE3Expmap) { - readWriteVectorBasedEdge(); -} - -TEST(IoSixDofExpmap, ReadWriteEdgeSE3ProjectXYZ) { - readWriteVectorBasedEdge(); -} - -TEST(IoSixDofExpmap, ReadWriteEdgeStereoSE3ProjectXYZ) { - readWriteVectorBasedEdge(); -} - -TEST(IoSixDofExpmap, ReadWriteEdgeSE3ProjectXYZOnlyPose) { - readWriteVectorBasedEdge(); -} - -TEST(IoSixDofExpmap, ReadWriteEdgeStereoSE3ProjectXYZOnlyPose) { - readWriteVectorBasedEdge(); -} - -/* - * EDGE Tests including a Camera Parameter - */ -class IoSixDofExpmapParam : public ::testing::Test { - protected: - void SetUp() override { - graph.reset(new g2o::OptimizableGraph); - - auto paramCam = std::make_shared(); - paramCam->setId(42); - graph->addParameter(paramCam); - - // setting up some vertices - point = std::make_shared(); - point->setId(0); - graph->addVertex(point); - pose = std::make_shared(); - pose->setId(1); - graph->addVertex(pose); - } - - template - void prepareEdge(typename std::shared_ptr& e) { - e->setParameterId(0, 42); - e->setVertex(0, point); - e->setVertex(1, pose); - graph->addEdge(e); - } - - std::shared_ptr graph; - std::shared_ptr point; - std::shared_ptr pose; -}; - -TEST_F(IoSixDofExpmapParam, ReadWriteEdgeProjectXYZ2UV) { - auto outputEdge = std::make_shared(); - prepareEdge(outputEdge); - readWriteVectorBasedEdge(outputEdge); -} - -TEST_F(IoSixDofExpmapParam, ReadWriteEdgeProjectPSI2UV) { - auto p2 = std::make_shared(); - p2->setId(2); - graph->addVertex(p2); - auto outputEdge = std::make_shared(); - outputEdge->setVertex(2, p2); - prepareEdge(outputEdge); - readWriteVectorBasedEdge(outputEdge); -} - -TEST_F(IoSixDofExpmapParam, ReadWriteEdgeProjectXYZ2UVU) { - auto outputEdge = std::make_shared(); - prepareEdge(outputEdge); - readWriteVectorBasedEdge(outputEdge); -} diff --git a/unit_test/sba/sba_basic_tests.cpp b/unit_test/sba/sba_basic_tests.cpp new file mode 100644 index 000000000..57919f3ac --- /dev/null +++ b/unit_test/sba/sba_basic_tests.cpp @@ -0,0 +1,60 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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 + +#include "g2o/core/factory.h" +#include "g2o/types/sba/edge_project_p2mc.h" +#include "g2o/types/sba/edge_project_p2sc.h" +#include "g2o/types/sba/edge_project_stereo_xyz.h" +#include "g2o/types/sba/edge_project_stereo_xyz_onlypose.h" +#include "g2o/types/sba/edge_project_xyz.h" +#include "g2o/types/sba/edge_project_xyz_onlypose.h" +#include "g2o/types/sba/edge_sba_cam.h" +#include "g2o/types/sba/edge_sba_scale.h" +#include "g2o/types/sba/edge_se3_expmap.h" +#include "unit_test/test_helper/typed_basic_tests.h" + +G2O_USE_TYPE_GROUP(slam3d) + +template <> +struct g2o::internal::RandomValue { + using Type = g2o::SBACam; + static Type create() { + g2o::SBACam result(g2o::Quaternion::UnitRandom(), g2o::Vector3::Random()); + return result; + } +}; + +using SBAIoTypes = ::testing::Types< + std::tuple, std::tuple, + std::tuple, std::tuple, + std::tuple, + std::tuple, + std::tuple, + std::tuple, std::tuple>; +INSTANTIATE_TYPED_TEST_SUITE_P(SBA, FixedSizeEdgeBasicTests, SBAIoTypes, + g2o::internal::DefaultTypeNames); diff --git a/unit_test/sclam2d/CMakeLists.txt b/unit_test/sclam2d/CMakeLists.txt index 2ecd1ba12..a189eb47b 100644 --- a/unit_test/sclam2d/CMakeLists.txt +++ b/unit_test/sclam2d/CMakeLists.txt @@ -1,5 +1,4 @@ add_executable(unittest_sclam2d - io_sclam2d.cpp odom_convert_sclam2d.cpp sensor_offset.cpp ) diff --git a/unit_test/sclam2d/io_sclam2d.cpp b/unit_test/sclam2d/io_sclam2d.cpp deleted file mode 100644 index 278275252..000000000 --- a/unit_test/sclam2d/io_sclam2d.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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 - -#include "g2o/types/sclam2d/edge_se2_odom_differential_calib.h" -#include "g2o/types/sclam2d/edge_se2_sensor_calib.h" -#include "g2o/types/sclam2d/vertex_odom_differential_params.h" -#include "gtest/gtest.h" -#include "unit_test/test_helper/io.h" - -using namespace std; -using namespace g2o; - -namespace { -struct RandomSE2 { - static SE2 create() { - auto randomPosition = Vector2::Random(); - auto randomOrientation = Vector2::Random(); - return SE2(randomPosition.x(), randomPosition.y(), - std::atan2(randomOrientation.y(), randomOrientation.x())); - } - static bool isApprox(const SE2& a, const SE2& b) { - return a.toVector().isApprox(b.toVector(), 1e-5); - } -}; - -struct RandomVelocityMeasurement { - static VelocityMeasurement create() { - auto randomValues = Vector3::Random(); - return VelocityMeasurement(randomValues(0), randomValues(1), - randomValues(2)); - } - static bool isApprox(const VelocityMeasurement& a, - const VelocityMeasurement& b) { - return a.measurement().isApprox(b.measurement(), 1e-5) && - fabs(a.dt() - b.dt()) < 1e-5; - } -}; -} // namespace - -TEST(IoSclam2d, ReadWriteVertexOdomDifferentialParams) { - readWriteVectorBasedVertex(); -} - -TEST(IoSclam2d, ReadWriteEdgeSE2SensorCalib) { - readWriteVectorBasedEdge(); -} - -TEST(IoSclam2d, ReadWriteEdgeSE2OdomDifferentialCalib) { - readWriteVectorBasedEdge(); -} diff --git a/unit_test/sim3/CMakeLists.txt b/unit_test/sim3/CMakeLists.txt index 4487faeff..a0f6c7411 100644 --- a/unit_test/sim3/CMakeLists.txt +++ b/unit_test/sim3/CMakeLists.txt @@ -1,6 +1,5 @@ add_executable(unittest_sim3 allocate_sim3.cpp - io_sim3.cpp jacobians_sim3.cpp ) target_link_libraries(unittest_sim3 types_sim3) diff --git a/unit_test/sim3/allocate_sim3.cpp b/unit_test/sim3/allocate_sim3.cpp index a7741ca06..58b3927cb 100644 --- a/unit_test/sim3/allocate_sim3.cpp +++ b/unit_test/sim3/allocate_sim3.cpp @@ -27,8 +27,8 @@ #include "g2o/types/sim3/types_seven_dof_expmap.h" #include "gtest/gtest.h" -TEST(Sim3, VertexSE3ExpmapCtor) { - auto* v = new g2o::VertexSE3Expmap(); +TEST(Sim3, VertexSim3ExpmapCtor) { + auto* v = new g2o::VertexSim3Expmap(); ASSERT_NE(nullptr, v); delete v; SUCCEED(); diff --git a/unit_test/sim3/io_sim3.cpp b/unit_test/sim3/io_sim3.cpp deleted file mode 100644 index a12a48324..000000000 --- a/unit_test/sim3/io_sim3.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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 - -#include "g2o/types/sim3/types_seven_dof_expmap.h" -#include "gtest/gtest.h" -#include "unit_test/test_helper/io.h" - -using namespace std; -using namespace g2o; - -struct RandomSim3 { - static Sim3 create() { - Vector3 randomPosition = Vector3::Random(); - Quaternion randomOrientation(Vector4::Random().normalized()); - return Sim3(randomOrientation, randomPosition, 1.0); - } - static bool isApprox(const Sim3& a, const Sim3& b) { - return a.translation().isApprox(b.translation(), 1e-5) && - a.rotation().isApprox(b.rotation(), 1e-5) && - fabs(a.scale() - b.scale()) < 1e-5; - } -}; - -TEST(IoSim3, ReadWriteVertexSim3Expmap) { - readWriteVectorBasedVertex(); -} - -TEST(IoSim3, ReadWriteEdgeSim3) { - readWriteVectorBasedEdge(); -} - -TEST(IoSim3, ReadWriteEdgeSim3ProjectXYZ) { - readWriteVectorBasedEdge(); -} - -TEST(IoSim3, ReadWriteEdgeInverseSim3ProjectXYZ) { - readWriteVectorBasedEdge(); -} diff --git a/unit_test/slam2d/CMakeLists.txt b/unit_test/slam2d/CMakeLists.txt index 66ed8a3fe..d6ddeb104 100644 --- a/unit_test/slam2d/CMakeLists.txt +++ b/unit_test/slam2d/CMakeLists.txt @@ -1,7 +1,8 @@ add_executable(unittest_slam2d mappings_se2.cpp jacobians_slam2d.cpp - io_slam2d.cpp + slam2d_basic_tests.cpp + solve_direct_slam2d.cpp ) -target_link_libraries(unittest_slam2d types_slam2d) +target_link_libraries(unittest_slam2d unittest_helper types_slam2d) create_test(unittest_slam2d) diff --git a/unit_test/slam2d/io_slam2d.cpp b/unit_test/slam2d/io_slam2d.cpp deleted file mode 100644 index 3c0caefb0..000000000 --- a/unit_test/slam2d/io_slam2d.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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 - -#include "g2o/types/slam2d/edge_se2.h" -#include "g2o/types/slam2d/edge_se2_pointxy.h" -#include "g2o/types/slam2d/edge_se2_prior.h" -#include "g2o/types/slam2d/edge_xy_prior.h" -#include "g2o/types/slam2d/parameter_se2_offset.h" -#include "g2o/types/slam2d/vertex_point_xy.h" -#include "g2o/types/slam2d/vertex_se2.h" -#include "gtest/gtest.h" -#include "unit_test/test_helper/io.h" - -namespace { -struct RandomSE2 { - static g2o::SE2 create() { - g2o::Vector2 randomPosition = g2o::Vector2::Random(); - g2o::Vector2 randomOrientation = g2o::Vector2::Random(); - return g2o::SE2(randomPosition.x(), randomPosition.y(), - std::atan2(randomOrientation.y(), randomOrientation.x())); - } - static bool isApprox(const g2o::SE2& a, const g2o::SE2& b) { - return a.toVector().isApprox(b.toVector(), 1e-5); - } -}; -} // namespace - -TEST(IoSlam2d, ReadWriteVertexSE2) { - g2o::readWriteVectorBasedVertex(); -} - -TEST(IoSlam2d, ReadWriteVertexPointXY) { - g2o::readWriteVectorBasedVertex(); -} - -TEST(IoSlam2d, ReadWriteParameterSE2Offset) { - g2o::ParameterSE2Offset outputParam; - outputParam.setOffset(g2o::SE2(1., 2., 0.3)); - g2o::ParameterSE2Offset inputParam; - readWriteGraphElement(outputParam, &inputParam); - ASSERT_TRUE( - outputParam.offset().toVector().isApprox(inputParam.offset().toVector())); -} - -TEST(IoSlam2d, ReadWriteEdgeSE2) { - g2o::readWriteVectorBasedEdge(); -} - -TEST(IoSlam2d, ReadWriteEdgeSE2Prior) { - g2o::readWriteVectorBasedEdge(); -} - -TEST(IoSlam2d, ReadWriteEdgeSE2PointXY) { - g2o::readWriteVectorBasedEdge(); -} - -TEST(IoSlam2d, ReadWriteEdgeXYPrior) { - g2o::readWriteVectorBasedEdge(); -} diff --git a/unit_test/slam2d/mappings_se2.cpp b/unit_test/slam2d/mappings_se2.cpp index 0609ff560..b5a3791c2 100644 --- a/unit_test/slam2d/mappings_se2.cpp +++ b/unit_test/slam2d/mappings_se2.cpp @@ -80,6 +80,4 @@ TEST(MappingsSlam2D, TraitsSE2) { EXPECT_THAT( print_wrap(Traits::fromMinimalVector(Vector3(0, 1, 2)).translation()), EigenEqual(print_wrap(Vector2(0, 1)))); - EXPECT_THAT(print_wrap(Traits::Identity().toVector()), - EigenEqual(print_wrap(Vector3::Zero()))); } diff --git a/g2o/types/slam3d/parameter_stereo_camera.cpp b/unit_test/slam2d/slam2d_basic_tests.cpp similarity index 53% rename from g2o/types/slam3d/parameter_stereo_camera.cpp rename to unit_test/slam2d/slam2d_basic_tests.cpp index b21a23875..abe695679 100644 --- a/g2o/types/slam3d/parameter_stereo_camera.cpp +++ b/unit_test/slam2d/slam2d_basic_tests.cpp @@ -24,27 +24,31 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include "parameter_stereo_camera.h" - -#include - -#include "g2o/stuff/misc.h" -#include "g2o/types/slam3d/parameter_camera.h" - -namespace g2o { - -ParameterStereoCamera::ParameterStereoCamera() : baseline_(cst(0.075)) {} - -bool ParameterStereoCamera::read(std::istream& is) { - bool state = ParameterCamera::read(is); - is >> baseline_; - return is.good() && state; -} - -bool ParameterStereoCamera::write(std::ostream& os) const { - bool state = ParameterCamera::write(os); - os << baseline() << " "; - return state && os.good(); -} - -} // namespace g2o +#include + +#include + +#include "g2o/types/slam2d/edge_se2.h" +#include "g2o/types/slam2d/edge_se2_offset.h" +#include "g2o/types/slam2d/edge_se2_pointxy.h" +#include "g2o/types/slam2d/edge_se2_pointxy_bearing.h" +#include "g2o/types/slam2d/edge_se2_pointxy_calib.h" +#include "g2o/types/slam2d/edge_se2_pointxy_offset.h" +#include "g2o/types/slam2d/edge_se2_prior.h" +#include "g2o/types/slam2d/edge_se2_twopointsxy.h" +#include "g2o/types/slam2d/edge_xy_prior.h" +#include "g2o/types/slam2d/parameter_se2_offset.h" +#include "unit_test/test_helper/typed_basic_tests.h" + +using Slam2DIoTypes = ::testing::Types< + // without parameters + std::tuple, std::tuple, + std::tuple, std::tuple, + std::tuple, std::tuple, + std::tuple, + // with parameters + std::tuple, + std::tuple >; +INSTANTIATE_TYPED_TEST_SUITE_P(Slam2D, FixedSizeEdgeBasicTests, Slam2DIoTypes, + g2o::internal::DefaultTypeNames); diff --git a/unit_test/slam2d/solve_direct_slam2d.cpp b/unit_test/slam2d/solve_direct_slam2d.cpp new file mode 100644 index 000000000..0b6095df5 --- /dev/null +++ b/unit_test/slam2d/solve_direct_slam2d.cpp @@ -0,0 +1,80 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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 "g2o/core/eigen_types.h" +#include "g2o/core/optimization_algorithm_with_hessian.h" +#include "g2o/core/sparse_optimizer.h" +#include "g2o/types/slam2d/edge_se2_pointxy.h" +#include "g2o/types/slam2d/vertex_point_xy.h" +#include "g2o/types/slam2d/vertex_se2.h" +#include "gmock/gmock.h" +#include "unit_test/test_helper/allocate_optimizer.h" +#include "unit_test/test_helper/eigen_matcher.h" + +using namespace g2o; // NOLINT +using namespace testing; // NOLINT +using g2o::internal::print_wrap; + +TEST(Slam2D, SolveDirect) { + std::unique_ptr optimizer = + g2o::internal::createOptimizerForTests(); + + OptimizationAlgorithmWithHessian* solverWithHessian = + dynamic_cast( + optimizer->solver().get()); + ASSERT_THAT(solverWithHessian, Ne(nullptr)); + + auto v1 = std::make_shared(); + v1->setId(0); + ASSERT_THAT(optimizer->addVertex(v1), IsTrue()); + + auto v2 = std::make_shared(); + v2->setEstimate(Vector2(1, 0.5)); + v2->setId(1); + ASSERT_THAT(optimizer->addVertex(v2), IsTrue()); + + auto e = std::make_shared(); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(EdgeSE2PointXY::Measurement(2, 1)); + e->setInformation(EdgeSE2PointXY::InformationType::Identity()); + ASSERT_THAT(optimizer->addEdge(e), IsTrue()); + + HyperGraph::EdgeSet edges_to_optimize = {e}; + + optimizer->initializeOptimization(edges_to_optimize); + optimizer->computeInitialGuess(); + optimizer->solver()->init(); + ASSERT_THAT(solverWithHessian->buildLinearStructure(), IsTrue()) + << "FATAL: failure while building linear structure"; + optimizer->computeActiveErrors(); + solverWithHessian->updateLinearSystem(); + + const bool solve_status = v2->solveDirect(); + EXPECT_THAT(solve_status, IsTrue()); + EXPECT_THAT(print_wrap(v2->estimate()), + EigenApproxEqual(print_wrap(Vector2(2, 1)), 1e-3)); +} diff --git a/unit_test/slam2d_addons/CMakeLists.txt b/unit_test/slam2d_addons/CMakeLists.txt deleted file mode 100644 index 96f6b7d0b..000000000 --- a/unit_test/slam2d_addons/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -add_executable(unittest_slam2d_addons - io_slam2d_addons.cpp -) -target_link_libraries(unittest_slam2d_addons types_slam2d_addons) -create_test(unittest_slam2d_addons) diff --git a/unit_test/slam2d_addons/io_slam2d_addons.cpp b/unit_test/slam2d_addons/io_slam2d_addons.cpp deleted file mode 100644 index 0ab57b4ec..000000000 --- a/unit_test/slam2d_addons/io_slam2d_addons.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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 - -#include "g2o/types/slam2d_addons/edge_line2d.cpp" -#include "g2o/types/slam2d_addons/edge_se2_line2d.h" -#include "g2o/types/slam2d_addons/edge_se2_segment2d.h" -#include "g2o/types/slam2d_addons/edge_se2_segment2d_line.h" -#include "g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h" -#include "g2o/types/slam2d_addons/vertex_segment2d.h" -#include "gtest/gtest.h" -#include "unit_test/test_helper/io.h" - -using namespace g2o; // NOLINT - -struct RandomLine2D { - static Line2D create() { return Line2D(g2o::Vector2::Random()); } - static bool isApprox(const Line2D& a, const Line2D& b) { - return a.isApprox(b, 1e-5); - } -}; - -TEST(IoSlam2dAddOns, ReadWriteVertexSegment2D) { - readWriteVectorBasedVertex(); -} - -TEST(IoSlam2dAddOns, ReadWriteEdgeLine2D) { - readWriteVectorBasedEdge(); -} - -TEST(IoSlam2dAddOns, ReadWriteEdgeSE2Line2D) { - readWriteVectorBasedEdge(); -} - -TEST(IoSlam2dAddOns, ReadWriteEdgeSE2Segment2D) { - readWriteVectorBasedEdge(); -} - -TEST(IoSlam2dAddOns, ReadWriteEdgeSE2Segment2DLine) { - readWriteVectorBasedEdge(); -} - -TEST(IoSlam2dAddOns, ReadWriteEdgeSE2Segment2DPointLine) { - readWriteVectorBasedEdge(); -} diff --git a/unit_test/slam3d/CMakeLists.txt b/unit_test/slam3d/CMakeLists.txt index 44003cba6..2d77ff3f4 100644 --- a/unit_test/slam3d/CMakeLists.txt +++ b/unit_test/slam3d/CMakeLists.txt @@ -1,9 +1,9 @@ add_executable(unittest_slam3d - io_slam3d.cpp jacobians_slam3d.cpp mappings_slam3d.cpp orthogonal_matrix.cpp optimization_slam3d.cpp + slam3d_basic_tests.cpp ) -target_link_libraries(unittest_slam3d types_slam3d) +target_link_libraries(unittest_slam3d unittest_helper types_slam3d) create_test(unittest_slam3d) diff --git a/unit_test/slam3d/io_slam3d.cpp b/unit_test/slam3d/io_slam3d.cpp deleted file mode 100644 index fcbb50bae..000000000 --- a/unit_test/slam3d/io_slam3d.cpp +++ /dev/null @@ -1,211 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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 -#include - -#include "g2o/types/slam3d/edge_pointxyz.h" -#include "g2o/types/slam3d/edge_se3.h" -#include "g2o/types/slam3d/edge_se3_offset.h" -#include "g2o/types/slam3d/edge_se3_pointxyz.h" -#include "g2o/types/slam3d/edge_se3_pointxyz_depth.h" -#include "g2o/types/slam3d/edge_se3_pointxyz_disparity.h" -#include "g2o/types/slam3d/edge_se3_prior.h" -#include "g2o/types/slam3d/edge_se3_xyzprior.h" -#include "g2o/types/slam3d/edge_xyz_prior.h" -#include "g2o/types/slam3d/parameter_stereo_camera.h" -#include "g2o/types/slam3d/vertex_pointxyz.h" -#include "g2o/types/slam3d/vertex_se3.h" -#include "gtest/gtest.h" -#include "unit_test/test_helper/io.h" -#include "unit_test/test_helper/random_state.h" - -using namespace g2o; // NOLINT - -class IoSlam3dParam : public ::testing::Test { - protected: - void SetUp() override { - graph_ = std::make_unique(); - - // setting up parameters for tests - paramOffset_.reset(new ParameterSE3Offset()); - paramOffset_->setId(paramId_++); - graph_->addParameter(paramOffset_); - - paramCamera_.reset(new ParameterCamera); - paramCamera_->setId(paramId_++); - graph_->addParameter(paramCamera_); - - // setting up some vertices - for (int i = 0; i < 2; ++i) { - auto p = std::make_shared(); - p->setId(numVertices_++); - graph_->addVertex(p); - poses_.at(i) = p; - } - point_.reset(new VertexPointXYZ); - point_->setId(numVertices_++); - graph_->addVertex(point_); - } - - template - bool preparePosePoseEdge(T& e) { - e->setParameterId(0, paramOffset_->id()); - for (size_t i = 0; i < e->vertices().size(); ++i) - e->setVertex(i, poses_.at(i)); - return graph_->addEdge(e); - } - - template - bool preparePosePointEdge(T& e) { - e->setParameterId(0, paramOffset_->id()); - e->setVertex(0, poses_.at(0)); - e->setVertex(1, point_); - return graph_->addEdge(e); - } - - template - bool prepareCamPointEdge(T& e) { - e->setParameterId(0, paramCamera_->id()); - e->setVertex(0, poses_.at(0)); - e->setVertex(1, point_); - return graph_->addEdge(e); - } - - std::unique_ptr graph_; - std::shared_ptr point_; - std::shared_ptr paramOffset_; - std::shared_ptr paramCamera_; - std::vector> poses_ = {nullptr, nullptr}; - int numVertices_ = 0; - int paramId_ = 42; -}; - -/* - * VERTEX Tests - */ -TEST(IoSlam3d, ReadWriteVertexSE3) { - readWriteVectorBasedVertex(); -} - -TEST(IoSlam3d, ReadWriteVertexPointXYZ) { - readWriteVectorBasedVertex(); -} - -/* - * EDGE Tests - */ -TEST(IoSlam3d, ReadWriteEdgeSE3) { - readWriteVectorBasedEdge(); -} - -TEST(IoSlam3d, ReadWriteEdgePointXYZ) { - readWriteVectorBasedEdge(); -} - -TEST(IoSlam3d, ReadWriteEdgeXYZPrior) { - readWriteVectorBasedEdge(); -} - -TEST_F(IoSlam3dParam, ReadWriteEdgeSE3Offset) { - // additional graph structures - auto paramOffset2 = std::make_shared(); - paramOffset2->setId(paramId_++); - graph_->addParameter(paramOffset2); - - // setting up the edge for output - auto outputEdge = std::make_shared(); - outputEdge->setParameterId(1, paramOffset2->id()); - ASSERT_TRUE(preparePosePoseEdge(outputEdge)); - - // Test IO - readWriteVectorBasedEdge( - outputEdge); -} - -TEST_F(IoSlam3dParam, ReadWriteEdgeSE3PointXYZ) { - auto outputEdge = std::make_shared(); - ASSERT_TRUE(preparePosePointEdge(outputEdge)); - readWriteVectorBasedEdge(outputEdge); -} - -TEST_F(IoSlam3dParam, ReadWriteEdgeSE3Prior) { - auto outputEdge = std::make_shared(); - ASSERT_TRUE(preparePosePoseEdge(outputEdge)); - readWriteVectorBasedEdge(outputEdge); -} - -TEST_F(IoSlam3dParam, ReadWriteEdgeSE3PointXYZDepth) { - auto outputEdge = std::make_shared(); - ASSERT_TRUE(prepareCamPointEdge(outputEdge)); - readWriteVectorBasedEdge(outputEdge); -} - -TEST_F(IoSlam3dParam, ReadWriteEdgeSE3PointXYZDisparity) { - auto outputEdge = std::make_shared(); - ASSERT_TRUE(prepareCamPointEdge(outputEdge)); - readWriteVectorBasedEdge(outputEdge); -} - -TEST_F(IoSlam3dParam, ReadWriteEdgeSE3XYZPrior) { - auto outputEdge = std::make_shared(); - ASSERT_TRUE(preparePosePoseEdge(outputEdge)); - readWriteVectorBasedEdge(outputEdge); -} - -// Parameter tests -TEST(IoSlam3d, ReadWriteParameterCamera) { - ParameterCamera outputParam; - outputParam.setOffset(internal::RandomIsometry3::create()); - outputParam.setKcam(1, 2, 3, 4); // just some test values for read/write - ParameterCamera inputParam; - readWriteGraphElement(outputParam, &inputParam); - ASSERT_TRUE(internal::RandomIsometry3::isApprox(outputParam.offset(), - inputParam.offset())); - ASSERT_TRUE(outputParam.Kcam().isApprox(inputParam.Kcam(), 1e-5)); -} - -TEST(IoSlam3d, ReadWriteParameterStereoCamera) { - ParameterStereoCamera outputParam; - outputParam.setOffset(internal::RandomIsometry3::create()); - outputParam.setBaseline(0.1); - outputParam.setKcam(1, 2, 3, 4); // just some test values for read/write - ParameterStereoCamera inputParam; - readWriteGraphElement(outputParam, &inputParam); - ASSERT_DOUBLE_EQ(outputParam.baseline(), inputParam.baseline()); - ASSERT_TRUE(internal::RandomIsometry3::isApprox(outputParam.offset(), - inputParam.offset())); - ASSERT_TRUE(outputParam.Kcam().isApprox(inputParam.Kcam(), 1e-5)); -} - -TEST(IoSlam3d, ReadWriteParameterSE3Offset) { - ParameterSE3Offset outputParam; - outputParam.setOffset(internal::RandomIsometry3::create()); - ParameterSE3Offset inputParam; - readWriteGraphElement(outputParam, &inputParam); - ASSERT_TRUE(internal::RandomIsometry3::isApprox(outputParam.offset(), - inputParam.offset())); -} diff --git a/unit_test/slam3d/jacobians_slam3d.cpp b/unit_test/slam3d/jacobians_slam3d.cpp index 5188e6d07..5b6676916 100644 --- a/unit_test/slam3d/jacobians_slam3d.cpp +++ b/unit_test/slam3d/jacobians_slam3d.cpp @@ -39,6 +39,7 @@ #include "g2o/types/slam3d/edge_se3_pointxyz_disparity.h" #include "g2o/types/slam3d/edge_se3_prior.h" #include "g2o/types/slam3d/edge_se3_xyzprior.h" +#include "g2o/types/slam3d/parameter_camera.h" #include "gtest/gtest.h" #include "unit_test/test_helper/evaluate_jacobian.h" #include "unit_test/test_helper/random_state.h" @@ -133,11 +134,11 @@ TEST(Slam3D, EdgeSE3OffsetJacobian) { auto paramOffset1 = std::make_shared(); paramOffset1->setId(0); - paramOffset1->setOffset(internal::randomIsometry3()); + paramOffset1->setParam(internal::randomIsometry3()); graph.addParameter(paramOffset1); auto paramOffset2 = std::make_shared(); paramOffset2->setId(1); - paramOffset2->setOffset(internal::randomIsometry3()); + paramOffset2->setParam(internal::randomIsometry3()); graph.addParameter(paramOffset2); auto e = std::make_shared(); @@ -220,7 +221,7 @@ TEST(Slam3D, EdgeSE3PointXYZJacobian) { auto my_epsilon = [](const double, const double) { return 1e-3; }; for (int k = 0; k < 10000; ++k) { - paramOffset->setOffset(internal::randomIsometry3()); + paramOffset->setParam(internal::randomIsometry3()); v1->setEstimate(internal::randomIsometry3()); v2->setEstimate(Eigen::Vector3d::Random()); e->setMeasurement(Eigen::Vector3d::Random()); @@ -359,7 +360,9 @@ TEST(Slam3D, EdgeSE3PointXYZDepthJacobian) { numericJacobianWorkspace.allocate(); for (int k = 0; k < 10000; ++k) { - paramOffset->setOffset(internal::randomIsometry3()); + g2o::CameraWithOffset cam_param(internal::randomIsometry3(), 1, 1, 0.5, + 0.5); + paramOffset->setParam(cam_param); v1->setEstimate(internal::randomIsometry3()); v2->setEstimate(Eigen::Vector3d::Random()); e->setMeasurement(Eigen::Vector3d::Random()); @@ -397,7 +400,9 @@ TEST(Slam3D, EdgeSE3PointXYZDisparityJacobian) { numericJacobianWorkspace.allocate(); for (int k = 0; k < 10000; ++k) { - paramOffset->setOffset(internal::randomIsometry3()); + g2o::CameraWithOffset cam_param(internal::randomIsometry3(), 1, 1, 0.5, + 0.5); + paramOffset->setParam(cam_param); v1->setEstimate(internal::randomIsometry3()); v2->setEstimate(Eigen::Vector3d::Random()); e->setMeasurement(Eigen::Vector3d::Random()); diff --git a/unit_test/slam3d/slam3d_basic_tests.cpp b/unit_test/slam3d/slam3d_basic_tests.cpp new file mode 100644 index 000000000..5b97f1307 --- /dev/null +++ b/unit_test/slam3d/slam3d_basic_tests.cpp @@ -0,0 +1,57 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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 + +#include + +#include "g2o/types/slam3d/edge_pointxyz.h" +#include "g2o/types/slam3d/edge_se3.h" +#include "g2o/types/slam3d/edge_se3_offset.h" +#include "g2o/types/slam3d/edge_se3_pointxyz.h" +#include "g2o/types/slam3d/edge_se3_pointxyz_depth.h" +#include "g2o/types/slam3d/edge_se3_pointxyz_disparity.h" +#include "g2o/types/slam3d/edge_se3_prior.h" +#include "g2o/types/slam3d/edge_se3_xyzprior.h" +#include "g2o/types/slam3d/edge_xyz_prior.h" +#include "g2o/types/slam3d/parameter_camera.h" +#include "g2o/types/slam3d/parameter_se3_offset.h" +#include "unit_test/test_helper/typed_basic_tests.h" + +using Slam3DIoTypes = ::testing::Types< + // without parameters + std::tuple, std::tuple, + std::tuple, + // with parameters + std::tuple, + std::tuple, + std::tuple, + std::tuple, + std::tuple, + std::tuple >; +INSTANTIATE_TYPED_TEST_SUITE_P(Slam3D, FixedSizeEdgeBasicTests, Slam3DIoTypes, + g2o::internal::DefaultTypeNames); diff --git a/unit_test/slam3d_addons/CMakeLists.txt b/unit_test/slam3d_addons/CMakeLists.txt deleted file mode 100644 index f5de2eecf..000000000 --- a/unit_test/slam3d_addons/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -add_executable(unittest_slam3d_addons - io_slam3d_addons.cpp -) -target_link_libraries(unittest_slam3d_addons types_slam3d_addons) -create_test(unittest_slam3d_addons) diff --git a/unit_test/slam3d_addons/io_slam3d_addons.cpp b/unit_test/slam3d_addons/io_slam3d_addons.cpp deleted file mode 100644 index 3a7f50d5a..000000000 --- a/unit_test/slam3d_addons/io_slam3d_addons.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2020 R. Kuemmerle, G. Grisetti, 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 - -#include "g2o/types/slam3d_addons/edge_plane.h" -#include "g2o/types/slam3d_addons/edge_se3_calib.h" -#include "g2o/types/slam3d_addons/edge_se3_line.h" -#include "g2o/types/slam3d_addons/edge_se3_plane_calib.h" -#include "g2o/types/slam3d_addons/vertex_line3d.h" -#include "g2o/types/slam3d_addons/vertex_plane.h" -#include "g2o/types/slam3d_addons/vertex_se3_euler.h" -#include "gtest/gtest.h" -#include "unit_test/test_helper/io.h" -#include "unit_test/test_helper/random_state.h" - -using namespace std; -using namespace g2o; - -struct RandomPlane3D { - static Plane3D create() { return Plane3D(Vector4::Random()); } - static bool isApprox(const Plane3D& a, const Plane3D& b) { - return a.coeffs().isApprox(b.coeffs(), 1e-5); - } -}; - -struct RandomLine3D { - static Line3D create() { return Line3D(Vector6::Random()); } - static bool isApprox(const Line3D& a, const Line3D& b) { - return a.isApprox(b, 1e-5); - } -}; - -class IoSlam3dAddonsParam : public ::testing::Test { - protected: - void SetUp() override { - graph.reset(new g2o::OptimizableGraph); - - // setting up parameters for tests - paramOffset.reset(new ParameterSE3Offset()); - paramOffset->setId(63); - graph->addParameter(paramOffset); - - // setting up some vertices - int numVertices = 0; - pose.reset(new VertexSE3); - pose->setId(numVertices++); - graph->addVertex(pose); - line.reset(new VertexLine3D); - line->setId(numVertices++); - graph->addVertex(line); - } - - std::shared_ptr graph; - std::shared_ptr paramOffset; - std::shared_ptr pose; - std::shared_ptr line; -}; - -TEST(IoSlam3dAddOns, ReadWriteVertexSE3Euler) { - readWriteVectorBasedVertex(); -} - -TEST(IoSlam3dAddOns, ReadWriteVertexPlane) { - // setting up the vertex for output - auto outputVertex = std::make_shared(); - outputVertex->color = Vector3::Random(); - // IO Test - readWriteVectorBasedVertex(outputVertex); -} - -TEST(IoSlam3dAddOns, ReadWriteVertexLine3D) { - readWriteVectorBasedVertex(); -} - -TEST(IoSlam3dAddOns, ReadWriteEdgeSE3Calib) { - readWriteVectorBasedEdge(); -} - -TEST_F(IoSlam3dAddonsParam, ReadWriteEdgeSE3Line3D) { - // prepare edge - auto outputEdge = std::make_shared(); - outputEdge->setParameterId(0, paramOffset->id()); - outputEdge->setVertex(0, pose); - outputEdge->setVertex(1, line); - ASSERT_TRUE(graph->addEdge(outputEdge)); - // test IO - readWriteVectorBasedEdge(outputEdge); -} - -TEST(IoSlam3dAddOns, ReadWriteEdgeSE3PlaneSensorCalib) { - readWriteVectorBasedEdge(); -} diff --git a/unit_test/solver/linear_solver_test.cpp b/unit_test/solver/linear_solver_test.cpp index f25393275..1dfebffd5 100644 --- a/unit_test/solver/linear_solver_test.cpp +++ b/unit_test/solver/linear_solver_test.cpp @@ -116,7 +116,7 @@ TYPED_TEST_P(LS, SolvePattern) { ASSERT_TRUE(!state || spinv.rowBlockIndices().size() == blockIndices.size()); if (!state) { // solver does not implement solving for a pattern return in // this case - std::cerr << "Solver does not support solvePattern()" << std::endl; + std::cerr << "Solver does not support solvePattern()\n"; SUCCEED(); return; } @@ -144,7 +144,7 @@ TYPED_TEST_P(LS, SolveBlocks) { ASSERT_TRUE(!state || blocks != nullptr); if (!state) { // solver does not implement solving for a pattern return in // this case - std::cerr << "Solver does not support solveBlocks()" << std::endl; + std::cerr << "Solver does not support solveBlocks()\n"; SUCCEED(); return; } diff --git a/unit_test/solver/sparse_system_helper.cpp b/unit_test/solver/sparse_system_helper.cpp index 171998c9a..c8caaa859 100644 --- a/unit_test/solver/sparse_system_helper.cpp +++ b/unit_test/solver/sparse_system_helper.cpp @@ -26,16 +26,14 @@ #include "sparse_system_helper.h" -#include #include #include -#include "g2o/core/io_helper.h" +namespace g2o::internal { -namespace g2o { -namespace internal { +namespace { -static std::vector readIndices(const std::string& indices) { +std::vector readIndices(const std::string& indices) { std::stringstream tokens(indices); int numElems = 0; @@ -49,147 +47,149 @@ static std::vector readIndices(const std::string& indices) { return result; } +} // namespace + // return the serialized sparse matrix std::string sparseMatrixString() { std::stringstream aux; - aux << "RBI : 12 3 6 9 12 15 18 21 24 27 30 33 36" << std::endl; - aux << "CBI : 12 3 6 9 12 15 18 21 24 27 30 33 36" << std::endl; - aux << "BLOCK : 0 0" << std::endl; - aux << "2500 1.79023e-15 0" << std::endl; - aux << "-1.79023e-15 2500 0" << std::endl; - aux << "0 0 25000" << std::endl; - aux << "BLOCK : 1 1" << std::endl; - aux << "500 0 0" << std::endl; - aux << "0 500 0" << std::endl; - aux << "0 0 5000" << std::endl; - aux << "BLOCK : 2 2" << std::endl; - aux << "500 8.88178e-16 0" << std::endl; - aux << "-8.88178e-16 500 0" << std::endl; - aux << "0 0 5000" << std::endl; - aux << "BLOCK : 3 3" << std::endl; - aux << "500 4.44089e-16 0" << std::endl; - aux << "-4.44089e-16 500 0" << std::endl; - aux << "0 0 5000" << std::endl; - aux << "BLOCK : 4 4" << std::endl; - aux << "500 0 0" << std::endl; - aux << "0 500 0" << std::endl; - aux << "0 0 5000" << std::endl; - aux << "BLOCK : 5 5" << std::endl; - aux << "500 0 0" << std::endl; - aux << "0 500 0" << std::endl; - aux << "0 0 5000" << std::endl; - aux << "BLOCK : 6 6" << std::endl; - aux << "500 -8.88178e-16 0" << std::endl; - aux << "8.88178e-16 500 0" << std::endl; - aux << "0 0 5000" << std::endl; - aux << "BLOCK : 0 7" << std::endl; - aux << "-500 0 -2000" << std::endl; - aux << "0 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 7 7" << std::endl; - aux << "500 0 2000" << std::endl; - aux << "0 500 500" << std::endl; - aux << "2000 500 13500" << std::endl; - aux << "BLOCK : 0 8" << std::endl; - aux << "-500 0 -1500" << std::endl; - aux << "0 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 8 8" << std::endl; - aux << "500 0 1500" << std::endl; - aux << "0 500 500" << std::endl; - aux << "1500 500 10000" << std::endl; - aux << "BLOCK : 0 9" << std::endl; - aux << "-500 1.38778e-17 -1000" << std::endl; - aux << "-1.38778e-17 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 9 9" << std::endl; - aux << "500 1.38778e-17 1000" << std::endl; - aux << "-1.38778e-17 500 500" << std::endl; - aux << "1000 500 7500" << std::endl; - aux << "BLOCK : 0 10" << std::endl; - aux << "-500 0 -500" << std::endl; - aux << "0 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 10 10" << std::endl; - aux << "500 0 500" << std::endl; - aux << "0 500 500" << std::endl; - aux << "500 500 6000" << std::endl; - aux << "BLOCK : 0 11" << std::endl; - aux << "-500 1.77636e-15 -500" << std::endl; - aux << "-1.77636e-15 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 1 11" << std::endl; - aux << "-500 0 -500" << std::endl; - aux << "0 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 2 11" << std::endl; - aux << "-500 8.88178e-16 8.88178e-16" << std::endl; - aux << "-8.88178e-16 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 3 11" << std::endl; - aux << "-500 4.44089e-16 500" << std::endl; - aux << "-4.44089e-16 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 4 11" << std::endl; - aux << "-500 0 1000" << std::endl; - aux << "0 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 5 11" << std::endl; - aux << "-500 0 1500" << std::endl; - aux << "0 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 6 11" << std::endl; - aux << "-500 -8.88178e-16 1000" << std::endl; - aux << "8.88178e-16 -500 -500" << std::endl; - aux << "0 0 -5000" << std::endl; - aux << "BLOCK : 11 11" << std::endl; - aux << "4000 2.22045e-15 -3500" << std::endl; - aux << "-2.22045e-15 4000 4000" << std::endl; - aux << "-3500 4000 54500" << std::endl; + aux << "RBI : 12 3 6 9 12 15 18 21 24 27 30 33 36\n"; + aux << "CBI : 12 3 6 9 12 15 18 21 24 27 30 33 36\n"; + aux << "BLOCK : 0 0\n"; + aux << "2500 1.79023e-15 0\n"; + aux << "-1.79023e-15 2500 0\n"; + aux << "0 0 25000\n"; + aux << "BLOCK : 1 1\n"; + aux << "500 0 0\n"; + aux << "0 500 0\n"; + aux << "0 0 5000\n"; + aux << "BLOCK : 2 2\n"; + aux << "500 8.88178e-16 0\n"; + aux << "-8.88178e-16 500 0\n"; + aux << "0 0 5000\n"; + aux << "BLOCK : 3 3\n"; + aux << "500 4.44089e-16 0\n"; + aux << "-4.44089e-16 500 0\n"; + aux << "0 0 5000\n"; + aux << "BLOCK : 4 4\n"; + aux << "500 0 0\n"; + aux << "0 500 0\n"; + aux << "0 0 5000\n"; + aux << "BLOCK : 5 5\n"; + aux << "500 0 0\n"; + aux << "0 500 0\n"; + aux << "0 0 5000\n"; + aux << "BLOCK : 6 6\n"; + aux << "500 -8.88178e-16 0\n"; + aux << "8.88178e-16 500 0\n"; + aux << "0 0 5000\n"; + aux << "BLOCK : 0 7\n"; + aux << "-500 0 -2000\n"; + aux << "0 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 7 7\n"; + aux << "500 0 2000\n"; + aux << "0 500 500\n"; + aux << "2000 500 13500\n"; + aux << "BLOCK : 0 8\n"; + aux << "-500 0 -1500\n"; + aux << "0 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 8 8\n"; + aux << "500 0 1500\n"; + aux << "0 500 500\n"; + aux << "1500 500 10000\n"; + aux << "BLOCK : 0 9\n"; + aux << "-500 1.38778e-17 -1000\n"; + aux << "-1.38778e-17 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 9 9\n"; + aux << "500 1.38778e-17 1000\n"; + aux << "-1.38778e-17 500 500\n"; + aux << "1000 500 7500\n"; + aux << "BLOCK : 0 10\n"; + aux << "-500 0 -500\n"; + aux << "0 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 10 10\n"; + aux << "500 0 500\n"; + aux << "0 500 500\n"; + aux << "500 500 6000\n"; + aux << "BLOCK : 0 11\n"; + aux << "-500 1.77636e-15 -500\n"; + aux << "-1.77636e-15 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 1 11\n"; + aux << "-500 0 -500\n"; + aux << "0 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 2 11\n"; + aux << "-500 8.88178e-16 8.88178e-16\n"; + aux << "-8.88178e-16 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 3 11\n"; + aux << "-500 4.44089e-16 500\n"; + aux << "-4.44089e-16 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 4 11\n"; + aux << "-500 0 1000\n"; + aux << "0 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 5 11\n"; + aux << "-500 0 1500\n"; + aux << "0 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 6 11\n"; + aux << "-500 -8.88178e-16 1000\n"; + aux << "8.88178e-16 -500 -500\n"; + aux << "0 0 -5000\n"; + aux << "BLOCK : 11 11\n"; + aux << "4000 2.22045e-15 -3500\n"; + aux << "-2.22045e-15 4000 4000\n"; + aux << "-3500 4000 54500\n"; return aux.str(); } std::string denseInverseMatrixString() { std::stringstream aux; - aux << "# rows: 36" << std::endl; - aux << "# columns: 36" << std::endl; + aux << "# rows: 36\n"; + aux << "# columns: 36\n"; // clang-format off - aux << "0.0047999999999999987 6.9908161755443545e-19 0.00040000000000000181 0.0027999999999999995 3.1776437161565264e-19 0.00040000000000000083 0.0023999999999999985 3.1776437161565264e-19 0.00040000000000000083 0.0019999999999999974 3.1776437161565264e-19 0.00040000000000000083 0.0015999999999999968 3.1776437161565264e-19 0.00040000000000000083 0.0011999999999999958 3.1776437161565264e-19 0.00040000000000000083 0.0015999999999999968 3.1776437161565264e-19 0.00040000000000000083 0.0031999999999999893 -0.00040000000000000137 0.00040000000000000213 0.0035999999999999921 -0.00040000000000000127 0.00040000000000000192 0.0039999999999999931 -0.00040000000000000127 0.00040000000000000192 0.0043999999999999959 -0.00040000000000000116 0.00040000000000000186 0.0023999999999999989 -0.00040000000000000051 0.00040000000000000083" << std::endl; - aux << "6.9908161755443545e-19 0.0039999999999999957 7.1333868948638727e-19 6.9908161755443391e-19 0.0019999999999999974 3.4954080877721633e-19 3.4954080877721763e-19 0.0019999999999999974 3.4954080877721633e-19 1.3481509610710651e-33 0.0019999999999999974 3.4954080877721633e-19 -3.4954080877721484e-19 0.0019999999999999974 3.4954080877721633e-19 -6.9908161755443121e-19 0.0019999999999999974 3.4954080877721633e-19 -3.4954080877721484e-19 0.0019999999999999974 3.4954080877721633e-19 -2.5180710211002832e-18 0.0039999999999999949 8.042881596636798e-19 -1.7137828614366046e-18 0.0039999999999999949 8.0428815966367999e-19 -9.0949470177292492e-19 0.0039999999999999949 8.0428815966367999e-19 -1.0520654210924454e-19 0.0039999999999999949 8.0428815966367999e-19 3.4954080877721768e-19 0.001999999999999997 3.4954080877721633e-19" << std::endl; - aux << "0.00040000000000000181 7.1333868948638727e-19 0.00040000000000000029 0.00040000000000000105 3.4078112616241102e-19 0.00020000000000000023 0.00020000000000000085 3.4078112616241102e-19 0.00020000000000000023 6.7762635780344027e-19 3.4078112616241102e-19 0.00020000000000000023 -0.00019999999999999949 3.4078112616241102e-19 0.00020000000000000023 -0.00039999999999999975 3.4078112616241102e-19 0.00020000000000000023 -0.00019999999999999949 3.4078112616241102e-19 0.00020000000000000023 -0.0011999999999999986 -0.00039999999999999942 0.00040000000000000018 -0.00079999999999999906 -0.00039999999999999953 0.00040000000000000029 -0.00039999999999999904 -0.00039999999999999953 0.00040000000000000029 1.463672932855431e-18 -0.00039999999999999953 0.00040000000000000029 0.0002000000000000009 -0.00019999999999999987 0.00020000000000000023" << std::endl; - aux << "0.0027999999999999995 6.9908161755443391e-19 0.00040000000000000105 0.0047999999999999996 3.1776437161565192e-19 0.00040000000000000051 0.0023999999999999989 3.1776437161565192e-19 0.00040000000000000051 0.0019999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0015999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0011999999999999971 3.1776437161565192e-19 0.00040000000000000051 0.0015999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0011999999999999945 -0.00040000000000000051 0.00040000000000000116 0.0015999999999999953 -0.00040000000000000045 0.0004000000000000011 0.0019999999999999957 -0.00040000000000000045 0.0004000000000000011 0.0023999999999999976 -0.0004000000000000004 0.00040000000000000105 0.0023999999999999994 -0.00040000000000000024 0.00040000000000000051" << std::endl; - aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0039999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl; - aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00040000000000000018 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl; - aux << "0.0023999999999999985 3.4954080877721763e-19 0.00020000000000000085 0.0023999999999999989 1.588821858078263e-19 0.00020000000000000042 0.0041999999999999989 1.588821858078263e-19 0.00020000000000000042 0.0019999999999999974 1.588821858078263e-19 0.00020000000000000042 0.0017999999999999973 1.588821858078263e-19 0.00020000000000000042 0.0015999999999999968 1.588821858078263e-19 0.00020000000000000042 0.0017999999999999973 1.588821858078263e-19 0.00020000000000000042 0.0015999999999999938 -0.00020000000000000069 0.00020000000000000101 0.0017999999999999952 -0.00020000000000000061 0.00020000000000000093 0.0019999999999999957 -0.00020000000000000061 0.00020000000000000093 0.0021999999999999971 -0.00020000000000000052 0.0002000000000000009 0.0021999999999999984 -0.00020000000000000025 0.00020000000000000042" << std::endl; - aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0039999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl; - aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00040000000000000018 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl; - aux << "0.0019999999999999974 1.3481509610710651e-33 6.7762635780344027e-19 0.0019999999999999979 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999974 6.98149604840373e-34 2.9815559743351372e-19 0.0039999999999999975 6.98149604840373e-34 2.9815559743351372e-19 0.001999999999999997 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999966 6.98149604840373e-34 2.9815559743351372e-19 0.001999999999999997 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999931 -8.9446679230054116e-19 8.6736173798840355e-19 0.0019999999999999948 -7.8604657505199071e-19 7.8604657505199071e-19 0.0019999999999999953 -7.8604657505199071e-19 7.8604657505199071e-19 0.0019999999999999961 -7.0473141211557788e-19 7.3183646642771549e-19 0.0019999999999999979 -2.7105054312137611e-19 2.9815559743351372e-19" << std::endl; - aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0039999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl; - aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00040000000000000018 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl; - aux << "0.0015999999999999968 -3.4954080877721484e-19 -0.00019999999999999949 0.0015999999999999979 -1.588821858078249e-19 -0.00019999999999999979 0.0017999999999999973 -1.588821858078249e-19 -0.00019999999999999979 0.001999999999999997 -1.588821858078249e-19 -0.00019999999999999979 0.0041999999999999971 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999968 -1.588821858078249e-19 -0.00019999999999999979 0.0021999999999999967 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999933 0.00019999999999999887 -0.00019999999999999925 0.0021999999999999949 0.00019999999999999903 -0.00019999999999999936 0.0019999999999999953 0.00019999999999999903 -0.00019999999999999936 0.0017999999999999956 0.00019999999999999912 -0.00019999999999999939 0.0017999999999999976 0.00019999999999999966 -0.00019999999999999979" << std::endl; - aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0039999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl; - aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00040000000000000018 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl; - aux << "0.0011999999999999958 -6.9908161755443121e-19 -0.00039999999999999975 0.0011999999999999971 -3.1776437161565057e-19 -0.00039999999999999996 0.0015999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.0019999999999999966 -3.1776437161565057e-19 -0.00039999999999999996 0.0023999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.0047999999999999961 -3.1776437161565057e-19 -0.00039999999999999996 0.0023999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.002799999999999993 0.00039999999999999872 -0.00039999999999999942 0.002399999999999995 0.00039999999999999899 -0.00039999999999999964 0.0019999999999999948 0.00039999999999999899 -0.00039999999999999964 0.0015999999999999951 0.00039999999999999904 -0.00039999999999999964 0.001599999999999997 0.00039999999999999969 -0.00039999999999999996" << std::endl; - aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0039999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl; - aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00040000000000000018 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl; - aux << "0.0015999999999999968 -3.4954080877721484e-19 -0.00019999999999999949 0.0015999999999999979 -1.588821858078249e-19 -0.00019999999999999979 0.0017999999999999973 -1.588821858078249e-19 -0.00019999999999999979 0.001999999999999997 -1.588821858078249e-19 -0.00019999999999999979 0.0021999999999999967 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999968 -1.588821858078249e-19 -0.00019999999999999979 0.0041999999999999971 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999933 0.00019999999999999887 -0.00019999999999999925 0.0021999999999999949 0.00019999999999999903 -0.00019999999999999936 0.0019999999999999953 0.00019999999999999903 -0.00019999999999999936 0.0017999999999999956 0.00019999999999999912 -0.00019999999999999939 0.0017999999999999976 0.00019999999999999966 -0.00019999999999999979" << std::endl; - aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0039999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19" << std::endl; - aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00040000000000000018 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl; - aux << "0.0031999999999999893 -2.5180710211002832e-18 -0.0011999999999999986 0.0011999999999999945 -1.2272590733885761e-18 -0.00039999999999999986 0.0015999999999999938 -1.2272590733885761e-18 -0.00039999999999999986 0.0019999999999999931 -1.2272590733885761e-18 -0.00039999999999999986 0.0023999999999999933 -1.2272590733885761e-18 -0.00039999999999999986 0.002799999999999993 -1.2272590733885761e-18 -0.00039999999999999986 0.0023999999999999933 -1.2272590733885761e-18 -0.00039999999999999986 0.013199999999999977 0.0019999999999999944 -0.0019999999999999974 0.0067999999999999849 0.0011999999999999958 -0.0011999999999999986 0.0055999999999999852 0.0011999999999999958 -0.0011999999999999986 0.0043999999999999873 0.001199999999999996 -0.0011999999999999986 0.0015999999999999942 0.00039999999999999866 -0.00039999999999999986" << std::endl; - aux << "-0.00040000000000000137 0.0039999999999999949 -0.00039999999999999942 -0.00040000000000000051 0.0019999999999999966 -0.00019999999999999982 -0.00020000000000000069 0.0019999999999999966 -0.00019999999999999982 -8.9446679230054116e-19 0.0019999999999999966 -0.00019999999999999982 0.00019999999999999887 0.0019999999999999966 -0.00019999999999999982 0.00039999999999999872 0.0019999999999999966 -0.00019999999999999982 0.00019999999999999887 0.0019999999999999966 -0.00019999999999999982 0.0019999999999999944 0.0065999999999999939 -0.00059999999999999908 0.00079999999999999668 0.0043999999999999933 -0.00039999999999999931 0.00039999999999999747 0.0043999999999999933 -0.00039999999999999931 -2.0328790734103208e-18 0.0043999999999999933 -0.00039999999999999931 -0.00020000000000000074 0.0021999999999999967 -0.00019999999999999982" << std::endl; - aux << "0.00040000000000000213 8.042881596636798e-19 0.00040000000000000018 0.00040000000000000116 3.8625586125105733e-19 0.0002000000000000002 0.00020000000000000101 3.8625586125105733e-19 0.0002000000000000002 8.6736173798840355e-19 3.8625586125105733e-19 0.0002000000000000002 -0.00019999999999999925 3.8625586125105733e-19 0.0002000000000000002 -0.00039999999999999942 3.8625586125105733e-19 0.0002000000000000002 -0.00019999999999999925 3.8625586125105733e-19 0.0002000000000000002 -0.0019999999999999974 -0.00059999999999999908 0.00059999999999999995 -0.00079999999999999852 -0.00039999999999999931 0.00040000000000000013 -0.00039999999999999845 -0.00039999999999999931 0.00040000000000000013 1.9244588561617704e-18 -0.00039999999999999931 0.00040000000000000013 0.00020000000000000107 -0.00019999999999999982 0.0002000000000000002" << std::endl; - aux << "0.0035999999999999921 -1.7137828614366046e-18 -0.00079999999999999906 0.0015999999999999953 -8.4100321213751965e-19 -0.00019999999999999996 0.0017999999999999952 -8.4100321213751965e-19 -0.00019999999999999996 0.0019999999999999948 -8.4100321213751965e-19 -0.00019999999999999996 0.0021999999999999949 -8.4100321213751965e-19 -0.00019999999999999996 0.002399999999999995 -8.4100321213751965e-19 -0.00019999999999999996 0.0021999999999999949 -8.4100321213751965e-19 -0.00019999999999999996 0.0067999999999999849 0.00079999999999999668 -0.00079999999999999852 0.0097999999999999893 0.001399999999999997 -0.0013999999999999989 0.0051999999999999894 0.00079999999999999711 -0.00079999999999999895 0.0043999999999999899 0.00079999999999999722 -0.00079999999999999906 0.0017999999999999954 0.00019999999999999912 -0.00019999999999999996" << std::endl; - aux << "-0.00040000000000000127 0.0039999999999999949 -0.00039999999999999953 -0.00040000000000000045 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000061 0.0019999999999999966 -0.00019999999999999987 -7.8604657505199071e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999899 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.0011999999999999958 0.0043999999999999933 -0.00039999999999999931 0.001399999999999997 0.0065999999999999939 -0.00059999999999999941 0.00039999999999999785 0.0043999999999999933 -0.00039999999999999942 -1.7889335846010823e-18 0.0043999999999999933 -0.00039999999999999942 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999987" << std::endl; - aux << "0.00040000000000000192 8.0428815966367999e-19 0.00040000000000000029 0.0004000000000000011 3.8625586125105738e-19 0.0002000000000000002 0.00020000000000000093 3.8625586125105738e-19 0.0002000000000000002 7.8604657505199071e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.0013999999999999989 -0.00059999999999999941 0.00060000000000000027 -0.00039999999999999877 -0.00039999999999999942 0.00040000000000000029 1.6263032587282567e-18 -0.00039999999999999942 0.00040000000000000029 0.00020000000000000099 -0.00019999999999999985 0.0002000000000000002" << std::endl; - aux << "0.0039999999999999931 -9.0949470177292492e-19 -0.00039999999999999904 0.0019999999999999957 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999957 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999948 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0055999999999999852 0.00039999999999999747 -0.00039999999999999845 0.0051999999999999894 0.00039999999999999785 -0.00039999999999999877 0.0075999999999999896 0.00079999999999999754 -0.00079999999999999874 0.0043999999999999907 0.0003999999999999979 -0.00039999999999999888 0.0019999999999999961 -6.7762635780344027e-19 2.1684043449710089e-19" << std::endl; - aux << "-0.00040000000000000127 0.0039999999999999949 -0.00039999999999999953 -0.00040000000000000045 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000061 0.0019999999999999966 -0.00019999999999999987 -7.8604657505199071e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999899 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.0011999999999999958 0.0043999999999999933 -0.00039999999999999931 0.00079999999999999711 0.0043999999999999933 -0.00039999999999999942 0.00079999999999999754 0.0065999999999999939 -0.00059999999999999941 -1.7889335846010823e-18 0.0043999999999999933 -0.00039999999999999942 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999987" << std::endl; - aux << "0.00040000000000000192 8.0428815966367999e-19 0.00040000000000000029 0.0004000000000000011 3.8625586125105738e-19 0.0002000000000000002 0.00020000000000000093 3.8625586125105738e-19 0.0002000000000000002 7.8604657505199071e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.00079999999999999895 -0.00039999999999999942 0.00040000000000000029 -0.00079999999999999874 -0.00059999999999999941 0.00060000000000000027 1.6263032587282567e-18 -0.00039999999999999942 0.00040000000000000029 0.00020000000000000099 -0.00019999999999999985 0.0002000000000000002" << std::endl; - aux << "0.0043999999999999959 -1.0520654210924454e-19 1.463672932855431e-18 0.0023999999999999976 -6.8491489635404718e-20 0.00020000000000000058 0.0021999999999999971 -6.8491489635404718e-20 0.00020000000000000058 0.0019999999999999961 -6.8491489635404718e-20 0.00020000000000000058 0.0017999999999999956 -6.8491489635404718e-20 0.00020000000000000058 0.0015999999999999951 -6.8491489635404718e-20 0.00020000000000000058 0.0017999999999999956 -6.8491489635404718e-20 0.00020000000000000058 0.0043999999999999873 -2.0328790734103208e-18 1.9244588561617704e-18 0.0043999999999999899 -1.7889335846010823e-18 1.6263032587282567e-18 0.0043999999999999907 -1.7889335846010823e-18 1.6263032587282567e-18 0.006599999999999993 0.00019999999999999819 -0.00019999999999999836 0.0021999999999999971 -0.00020000000000000066 0.00020000000000000058" << std::endl; - aux << "-0.00040000000000000116 0.0039999999999999949 -0.00039999999999999953 -0.0004000000000000004 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000052 0.0019999999999999966 -0.00019999999999999987 -7.0473141211557788e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999912 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999904 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999912 0.0019999999999999966 -0.00019999999999999987 0.001199999999999996 0.0043999999999999933 -0.00039999999999999931 0.00079999999999999722 0.0043999999999999933 -0.00039999999999999942 0.0003999999999999979 0.0043999999999999933 -0.00039999999999999942 0.00019999999999999819 0.0065999999999999939 -0.00059999999999999941 -0.00020000000000000058 0.0021999999999999967 -0.00019999999999999987" << std::endl; - aux << "0.00040000000000000186 8.0428815966367999e-19 0.00040000000000000029 0.00040000000000000105 3.8625586125105738e-19 0.0002000000000000002 0.0002000000000000009 3.8625586125105738e-19 0.0002000000000000002 7.3183646642771549e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999939 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999939 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.00079999999999999906 -0.00039999999999999942 0.00040000000000000029 -0.00039999999999999888 -0.00039999999999999942 0.00040000000000000029 -0.00019999999999999836 -0.00059999999999999941 0.00060000000000000027 0.00020000000000000093 -0.00019999999999999985 0.0002000000000000002" << std::endl; - aux << "0.0023999999999999989 3.4954080877721768e-19 0.0002000000000000009 0.0023999999999999994 1.5888218580782632e-19 0.00020000000000000044 0.0021999999999999984 1.5888218580782632e-19 0.00020000000000000044 0.0019999999999999979 1.5888218580782632e-19 0.00020000000000000044 0.0017999999999999976 1.5888218580782632e-19 0.00020000000000000044 0.001599999999999997 1.5888218580782632e-19 0.00020000000000000044 0.0017999999999999976 1.5888218580782632e-19 0.00020000000000000044 0.0015999999999999942 -0.00020000000000000074 0.00020000000000000107 0.0017999999999999954 -0.00020000000000000066 0.00020000000000000099 0.0019999999999999961 -0.00020000000000000066 0.00020000000000000099 0.0021999999999999971 -0.00020000000000000058 0.00020000000000000093 0.0021999999999999988 -0.00020000000000000028 0.00020000000000000044" << std::endl; - aux << "-0.00040000000000000051 0.001999999999999997 -0.00019999999999999987 -0.00040000000000000024 0.0019999999999999983 -0.00020000000000000001 -0.00020000000000000025 0.0019999999999999983 -0.00020000000000000001 -2.7105054312137611e-19 0.0019999999999999983 -0.00020000000000000001 0.00019999999999999966 0.0019999999999999983 -0.00020000000000000001 0.00039999999999999969 0.0019999999999999983 -0.00020000000000000001 0.00019999999999999966 0.0019999999999999983 -0.00020000000000000001 0.00039999999999999866 0.0021999999999999967 -0.00019999999999999982 0.00019999999999999912 0.0021999999999999967 -0.00019999999999999985 -6.7762635780344027e-19 0.0021999999999999967 -0.00019999999999999985 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999985 -0.00020000000000000028 0.0021999999999999984 -0.00020000000000000001" << std::endl; - aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017" << std::endl; + aux << "0.0047999999999999987 6.9908161755443545e-19 0.00040000000000000181 0.0027999999999999995 3.1776437161565264e-19 0.00040000000000000083 0.0023999999999999985 3.1776437161565264e-19 0.00040000000000000083 0.0019999999999999974 3.1776437161565264e-19 0.00040000000000000083 0.0015999999999999968 3.1776437161565264e-19 0.00040000000000000083 0.0011999999999999958 3.1776437161565264e-19 0.00040000000000000083 0.0015999999999999968 3.1776437161565264e-19 0.00040000000000000083 0.0031999999999999893 -0.00040000000000000137 0.00040000000000000213 0.0035999999999999921 -0.00040000000000000127 0.00040000000000000192 0.0039999999999999931 -0.00040000000000000127 0.00040000000000000192 0.0043999999999999959 -0.00040000000000000116 0.00040000000000000186 0.0023999999999999989 -0.00040000000000000051 0.00040000000000000083\n"; + aux << "6.9908161755443545e-19 0.0039999999999999957 7.1333868948638727e-19 6.9908161755443391e-19 0.0019999999999999974 3.4954080877721633e-19 3.4954080877721763e-19 0.0019999999999999974 3.4954080877721633e-19 1.3481509610710651e-33 0.0019999999999999974 3.4954080877721633e-19 -3.4954080877721484e-19 0.0019999999999999974 3.4954080877721633e-19 -6.9908161755443121e-19 0.0019999999999999974 3.4954080877721633e-19 -3.4954080877721484e-19 0.0019999999999999974 3.4954080877721633e-19 -2.5180710211002832e-18 0.0039999999999999949 8.042881596636798e-19 -1.7137828614366046e-18 0.0039999999999999949 8.0428815966367999e-19 -9.0949470177292492e-19 0.0039999999999999949 8.0428815966367999e-19 -1.0520654210924454e-19 0.0039999999999999949 8.0428815966367999e-19 3.4954080877721768e-19 0.001999999999999997 3.4954080877721633e-19\n"; + aux << "0.00040000000000000181 7.1333868948638727e-19 0.00040000000000000029 0.00040000000000000105 3.4078112616241102e-19 0.00020000000000000023 0.00020000000000000085 3.4078112616241102e-19 0.00020000000000000023 6.7762635780344027e-19 3.4078112616241102e-19 0.00020000000000000023 -0.00019999999999999949 3.4078112616241102e-19 0.00020000000000000023 -0.00039999999999999975 3.4078112616241102e-19 0.00020000000000000023 -0.00019999999999999949 3.4078112616241102e-19 0.00020000000000000023 -0.0011999999999999986 -0.00039999999999999942 0.00040000000000000018 -0.00079999999999999906 -0.00039999999999999953 0.00040000000000000029 -0.00039999999999999904 -0.00039999999999999953 0.00040000000000000029 1.463672932855431e-18 -0.00039999999999999953 0.00040000000000000029 0.0002000000000000009 -0.00019999999999999987 0.00020000000000000023\n"; + aux << "0.0027999999999999995 6.9908161755443391e-19 0.00040000000000000105 0.0047999999999999996 3.1776437161565192e-19 0.00040000000000000051 0.0023999999999999989 3.1776437161565192e-19 0.00040000000000000051 0.0019999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0015999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0011999999999999971 3.1776437161565192e-19 0.00040000000000000051 0.0015999999999999979 3.1776437161565192e-19 0.00040000000000000051 0.0011999999999999945 -0.00040000000000000051 0.00040000000000000116 0.0015999999999999953 -0.00040000000000000045 0.0004000000000000011 0.0019999999999999957 -0.00040000000000000045 0.0004000000000000011 0.0023999999999999976 -0.0004000000000000004 0.00040000000000000105 0.0023999999999999994 -0.00040000000000000024 0.00040000000000000051\n"; + aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0039999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19\n"; + aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00040000000000000018 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017\n"; + aux << "0.0023999999999999985 3.4954080877721763e-19 0.00020000000000000085 0.0023999999999999989 1.588821858078263e-19 0.00020000000000000042 0.0041999999999999989 1.588821858078263e-19 0.00020000000000000042 0.0019999999999999974 1.588821858078263e-19 0.00020000000000000042 0.0017999999999999973 1.588821858078263e-19 0.00020000000000000042 0.0015999999999999968 1.588821858078263e-19 0.00020000000000000042 0.0017999999999999973 1.588821858078263e-19 0.00020000000000000042 0.0015999999999999938 -0.00020000000000000069 0.00020000000000000101 0.0017999999999999952 -0.00020000000000000061 0.00020000000000000093 0.0019999999999999957 -0.00020000000000000061 0.00020000000000000093 0.0021999999999999971 -0.00020000000000000052 0.0002000000000000009 0.0021999999999999984 -0.00020000000000000025 0.00020000000000000042\n"; + aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0039999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19\n"; + aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00040000000000000018 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017\n"; + aux << "0.0019999999999999974 1.3481509610710651e-33 6.7762635780344027e-19 0.0019999999999999979 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999974 6.98149604840373e-34 2.9815559743351372e-19 0.0039999999999999975 6.98149604840373e-34 2.9815559743351372e-19 0.001999999999999997 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999966 6.98149604840373e-34 2.9815559743351372e-19 0.001999999999999997 6.98149604840373e-34 2.9815559743351372e-19 0.0019999999999999931 -8.9446679230054116e-19 8.6736173798840355e-19 0.0019999999999999948 -7.8604657505199071e-19 7.8604657505199071e-19 0.0019999999999999953 -7.8604657505199071e-19 7.8604657505199071e-19 0.0019999999999999961 -7.0473141211557788e-19 7.3183646642771549e-19 0.0019999999999999979 -2.7105054312137611e-19 2.9815559743351372e-19\n"; + aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0039999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19\n"; + aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00040000000000000018 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017\n"; + aux << "0.0015999999999999968 -3.4954080877721484e-19 -0.00019999999999999949 0.0015999999999999979 -1.588821858078249e-19 -0.00019999999999999979 0.0017999999999999973 -1.588821858078249e-19 -0.00019999999999999979 0.001999999999999997 -1.588821858078249e-19 -0.00019999999999999979 0.0041999999999999971 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999968 -1.588821858078249e-19 -0.00019999999999999979 0.0021999999999999967 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999933 0.00019999999999999887 -0.00019999999999999925 0.0021999999999999949 0.00019999999999999903 -0.00019999999999999936 0.0019999999999999953 0.00019999999999999903 -0.00019999999999999936 0.0017999999999999956 0.00019999999999999912 -0.00019999999999999939 0.0017999999999999976 0.00019999999999999966 -0.00019999999999999979\n"; + aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0039999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19\n"; + aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00040000000000000018 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017\n"; + aux << "0.0011999999999999958 -6.9908161755443121e-19 -0.00039999999999999975 0.0011999999999999971 -3.1776437161565057e-19 -0.00039999999999999996 0.0015999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.0019999999999999966 -3.1776437161565057e-19 -0.00039999999999999996 0.0023999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.0047999999999999961 -3.1776437161565057e-19 -0.00039999999999999996 0.0023999999999999968 -3.1776437161565057e-19 -0.00039999999999999996 0.002799999999999993 0.00039999999999999872 -0.00039999999999999942 0.002399999999999995 0.00039999999999999899 -0.00039999999999999964 0.0019999999999999948 0.00039999999999999899 -0.00039999999999999964 0.0015999999999999951 0.00039999999999999904 -0.00039999999999999964 0.001599999999999997 0.00039999999999999969 -0.00039999999999999996\n"; + aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0039999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19\n"; + aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00040000000000000018 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017\n"; + aux << "0.0015999999999999968 -3.4954080877721484e-19 -0.00019999999999999949 0.0015999999999999979 -1.588821858078249e-19 -0.00019999999999999979 0.0017999999999999973 -1.588821858078249e-19 -0.00019999999999999979 0.001999999999999997 -1.588821858078249e-19 -0.00019999999999999979 0.0021999999999999967 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999968 -1.588821858078249e-19 -0.00019999999999999979 0.0041999999999999971 -1.588821858078249e-19 -0.00019999999999999979 0.0023999999999999933 0.00019999999999999887 -0.00019999999999999925 0.0021999999999999949 0.00019999999999999903 -0.00019999999999999936 0.0019999999999999953 0.00019999999999999903 -0.00019999999999999936 0.0017999999999999956 0.00019999999999999912 -0.00019999999999999939 0.0017999999999999976 0.00019999999999999966 -0.00019999999999999979\n"; + aux << "3.1776437161565264e-19 0.0019999999999999974 3.4078112616241102e-19 3.1776437161565192e-19 0.0019999999999999983 1.5888218580782565e-19 1.588821858078263e-19 0.0019999999999999983 1.5888218580782565e-19 6.98149604840373e-34 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0019999999999999983 1.5888218580782565e-19 -3.1776437161565057e-19 0.0019999999999999983 1.5888218580782565e-19 -1.588821858078249e-19 0.0039999999999999983 1.5888218580782565e-19 -1.2272590733885761e-18 0.0019999999999999966 3.8625586125105733e-19 -8.4100321213751965e-19 0.0019999999999999966 3.8625586125105738e-19 -4.5474735088646227e-19 0.0019999999999999966 3.8625586125105738e-19 -6.8491489635404718e-20 0.0019999999999999966 3.8625586125105738e-19 1.5888218580782632e-19 0.0019999999999999983 1.5888218580782565e-19\n"; + aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00040000000000000018 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017\n"; + aux << "0.0031999999999999893 -2.5180710211002832e-18 -0.0011999999999999986 0.0011999999999999945 -1.2272590733885761e-18 -0.00039999999999999986 0.0015999999999999938 -1.2272590733885761e-18 -0.00039999999999999986 0.0019999999999999931 -1.2272590733885761e-18 -0.00039999999999999986 0.0023999999999999933 -1.2272590733885761e-18 -0.00039999999999999986 0.002799999999999993 -1.2272590733885761e-18 -0.00039999999999999986 0.0023999999999999933 -1.2272590733885761e-18 -0.00039999999999999986 0.013199999999999977 0.0019999999999999944 -0.0019999999999999974 0.0067999999999999849 0.0011999999999999958 -0.0011999999999999986 0.0055999999999999852 0.0011999999999999958 -0.0011999999999999986 0.0043999999999999873 0.001199999999999996 -0.0011999999999999986 0.0015999999999999942 0.00039999999999999866 -0.00039999999999999986\n"; + aux << "-0.00040000000000000137 0.0039999999999999949 -0.00039999999999999942 -0.00040000000000000051 0.0019999999999999966 -0.00019999999999999982 -0.00020000000000000069 0.0019999999999999966 -0.00019999999999999982 -8.9446679230054116e-19 0.0019999999999999966 -0.00019999999999999982 0.00019999999999999887 0.0019999999999999966 -0.00019999999999999982 0.00039999999999999872 0.0019999999999999966 -0.00019999999999999982 0.00019999999999999887 0.0019999999999999966 -0.00019999999999999982 0.0019999999999999944 0.0065999999999999939 -0.00059999999999999908 0.00079999999999999668 0.0043999999999999933 -0.00039999999999999931 0.00039999999999999747 0.0043999999999999933 -0.00039999999999999931 -2.0328790734103208e-18 0.0043999999999999933 -0.00039999999999999931 -0.00020000000000000074 0.0021999999999999967 -0.00019999999999999982\n"; + aux << "0.00040000000000000213 8.042881596636798e-19 0.00040000000000000018 0.00040000000000000116 3.8625586125105733e-19 0.0002000000000000002 0.00020000000000000101 3.8625586125105733e-19 0.0002000000000000002 8.6736173798840355e-19 3.8625586125105733e-19 0.0002000000000000002 -0.00019999999999999925 3.8625586125105733e-19 0.0002000000000000002 -0.00039999999999999942 3.8625586125105733e-19 0.0002000000000000002 -0.00019999999999999925 3.8625586125105733e-19 0.0002000000000000002 -0.0019999999999999974 -0.00059999999999999908 0.00059999999999999995 -0.00079999999999999852 -0.00039999999999999931 0.00040000000000000013 -0.00039999999999999845 -0.00039999999999999931 0.00040000000000000013 1.9244588561617704e-18 -0.00039999999999999931 0.00040000000000000013 0.00020000000000000107 -0.00019999999999999982 0.0002000000000000002\n"; + aux << "0.0035999999999999921 -1.7137828614366046e-18 -0.00079999999999999906 0.0015999999999999953 -8.4100321213751965e-19 -0.00019999999999999996 0.0017999999999999952 -8.4100321213751965e-19 -0.00019999999999999996 0.0019999999999999948 -8.4100321213751965e-19 -0.00019999999999999996 0.0021999999999999949 -8.4100321213751965e-19 -0.00019999999999999996 0.002399999999999995 -8.4100321213751965e-19 -0.00019999999999999996 0.0021999999999999949 -8.4100321213751965e-19 -0.00019999999999999996 0.0067999999999999849 0.00079999999999999668 -0.00079999999999999852 0.0097999999999999893 0.001399999999999997 -0.0013999999999999989 0.0051999999999999894 0.00079999999999999711 -0.00079999999999999895 0.0043999999999999899 0.00079999999999999722 -0.00079999999999999906 0.0017999999999999954 0.00019999999999999912 -0.00019999999999999996\n"; + aux << "-0.00040000000000000127 0.0039999999999999949 -0.00039999999999999953 -0.00040000000000000045 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000061 0.0019999999999999966 -0.00019999999999999987 -7.8604657505199071e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999899 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.0011999999999999958 0.0043999999999999933 -0.00039999999999999931 0.001399999999999997 0.0065999999999999939 -0.00059999999999999941 0.00039999999999999785 0.0043999999999999933 -0.00039999999999999942 -1.7889335846010823e-18 0.0043999999999999933 -0.00039999999999999942 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999987\n"; + aux << "0.00040000000000000192 8.0428815966367999e-19 0.00040000000000000029 0.0004000000000000011 3.8625586125105738e-19 0.0002000000000000002 0.00020000000000000093 3.8625586125105738e-19 0.0002000000000000002 7.8604657505199071e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.0013999999999999989 -0.00059999999999999941 0.00060000000000000027 -0.00039999999999999877 -0.00039999999999999942 0.00040000000000000029 1.6263032587282567e-18 -0.00039999999999999942 0.00040000000000000029 0.00020000000000000099 -0.00019999999999999985 0.0002000000000000002\n"; + aux << "0.0039999999999999931 -9.0949470177292492e-19 -0.00039999999999999904 0.0019999999999999957 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999957 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999948 -4.5474735088646227e-19 2.1684043449710089e-19 0.0019999999999999953 -4.5474735088646227e-19 2.1684043449710089e-19 0.0055999999999999852 0.00039999999999999747 -0.00039999999999999845 0.0051999999999999894 0.00039999999999999785 -0.00039999999999999877 0.0075999999999999896 0.00079999999999999754 -0.00079999999999999874 0.0043999999999999907 0.0003999999999999979 -0.00039999999999999888 0.0019999999999999961 -6.7762635780344027e-19 2.1684043449710089e-19\n"; + aux << "-0.00040000000000000127 0.0039999999999999949 -0.00039999999999999953 -0.00040000000000000045 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000061 0.0019999999999999966 -0.00019999999999999987 -7.8604657505199071e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999899 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999903 0.0019999999999999966 -0.00019999999999999987 0.0011999999999999958 0.0043999999999999933 -0.00039999999999999931 0.00079999999999999711 0.0043999999999999933 -0.00039999999999999942 0.00079999999999999754 0.0065999999999999939 -0.00059999999999999941 -1.7889335846010823e-18 0.0043999999999999933 -0.00039999999999999942 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999987\n"; + aux << "0.00040000000000000192 8.0428815966367999e-19 0.00040000000000000029 0.0004000000000000011 3.8625586125105738e-19 0.0002000000000000002 0.00020000000000000093 3.8625586125105738e-19 0.0002000000000000002 7.8604657505199071e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999936 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.00079999999999999895 -0.00039999999999999942 0.00040000000000000029 -0.00079999999999999874 -0.00059999999999999941 0.00060000000000000027 1.6263032587282567e-18 -0.00039999999999999942 0.00040000000000000029 0.00020000000000000099 -0.00019999999999999985 0.0002000000000000002\n"; + aux << "0.0043999999999999959 -1.0520654210924454e-19 1.463672932855431e-18 0.0023999999999999976 -6.8491489635404718e-20 0.00020000000000000058 0.0021999999999999971 -6.8491489635404718e-20 0.00020000000000000058 0.0019999999999999961 -6.8491489635404718e-20 0.00020000000000000058 0.0017999999999999956 -6.8491489635404718e-20 0.00020000000000000058 0.0015999999999999951 -6.8491489635404718e-20 0.00020000000000000058 0.0017999999999999956 -6.8491489635404718e-20 0.00020000000000000058 0.0043999999999999873 -2.0328790734103208e-18 1.9244588561617704e-18 0.0043999999999999899 -1.7889335846010823e-18 1.6263032587282567e-18 0.0043999999999999907 -1.7889335846010823e-18 1.6263032587282567e-18 0.006599999999999993 0.00019999999999999819 -0.00019999999999999836 0.0021999999999999971 -0.00020000000000000066 0.00020000000000000058\n"; + aux << "-0.00040000000000000116 0.0039999999999999949 -0.00039999999999999953 -0.0004000000000000004 0.0019999999999999966 -0.00019999999999999987 -0.00020000000000000052 0.0019999999999999966 -0.00019999999999999987 -7.0473141211557788e-19 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999912 0.0019999999999999966 -0.00019999999999999987 0.00039999999999999904 0.0019999999999999966 -0.00019999999999999987 0.00019999999999999912 0.0019999999999999966 -0.00019999999999999987 0.001199999999999996 0.0043999999999999933 -0.00039999999999999931 0.00079999999999999722 0.0043999999999999933 -0.00039999999999999942 0.0003999999999999979 0.0043999999999999933 -0.00039999999999999942 0.00019999999999999819 0.0065999999999999939 -0.00059999999999999941 -0.00020000000000000058 0.0021999999999999967 -0.00019999999999999987\n"; + aux << "0.00040000000000000186 8.0428815966367999e-19 0.00040000000000000029 0.00040000000000000105 3.8625586125105738e-19 0.0002000000000000002 0.0002000000000000009 3.8625586125105738e-19 0.0002000000000000002 7.3183646642771549e-19 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999939 3.8625586125105738e-19 0.0002000000000000002 -0.00039999999999999964 3.8625586125105738e-19 0.0002000000000000002 -0.00019999999999999939 3.8625586125105738e-19 0.0002000000000000002 -0.0011999999999999986 -0.00039999999999999931 0.00040000000000000013 -0.00079999999999999906 -0.00039999999999999942 0.00040000000000000029 -0.00039999999999999888 -0.00039999999999999942 0.00040000000000000029 -0.00019999999999999836 -0.00059999999999999941 0.00060000000000000027 0.00020000000000000093 -0.00019999999999999985 0.0002000000000000002\n"; + aux << "0.0023999999999999989 3.4954080877721768e-19 0.0002000000000000009 0.0023999999999999994 1.5888218580782632e-19 0.00020000000000000044 0.0021999999999999984 1.5888218580782632e-19 0.00020000000000000044 0.0019999999999999979 1.5888218580782632e-19 0.00020000000000000044 0.0017999999999999976 1.5888218580782632e-19 0.00020000000000000044 0.001599999999999997 1.5888218580782632e-19 0.00020000000000000044 0.0017999999999999976 1.5888218580782632e-19 0.00020000000000000044 0.0015999999999999942 -0.00020000000000000074 0.00020000000000000107 0.0017999999999999954 -0.00020000000000000066 0.00020000000000000099 0.0019999999999999961 -0.00020000000000000066 0.00020000000000000099 0.0021999999999999971 -0.00020000000000000058 0.00020000000000000093 0.0021999999999999988 -0.00020000000000000028 0.00020000000000000044\n"; + aux << "-0.00040000000000000051 0.001999999999999997 -0.00019999999999999987 -0.00040000000000000024 0.0019999999999999983 -0.00020000000000000001 -0.00020000000000000025 0.0019999999999999983 -0.00020000000000000001 -2.7105054312137611e-19 0.0019999999999999983 -0.00020000000000000001 0.00019999999999999966 0.0019999999999999983 -0.00020000000000000001 0.00039999999999999969 0.0019999999999999983 -0.00020000000000000001 0.00019999999999999966 0.0019999999999999983 -0.00020000000000000001 0.00039999999999999866 0.0021999999999999967 -0.00019999999999999982 0.00019999999999999912 0.0021999999999999967 -0.00019999999999999985 -6.7762635780344027e-19 0.0021999999999999967 -0.00019999999999999985 -0.00020000000000000066 0.0021999999999999967 -0.00019999999999999985 -0.00020000000000000028 0.0021999999999999984 -0.00020000000000000001\n"; + aux << "0.00040000000000000083 3.4954080877721633e-19 0.00020000000000000023 0.00040000000000000051 1.5888218580782565e-19 0.00020000000000000017 0.00020000000000000042 1.5888218580782565e-19 0.00020000000000000017 2.9815559743351372e-19 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999996 1.5888218580782565e-19 0.00020000000000000017 -0.00019999999999999979 1.5888218580782565e-19 0.00020000000000000017 -0.00039999999999999986 -0.00019999999999999982 0.0002000000000000002 -0.00019999999999999996 -0.00019999999999999987 0.0002000000000000002 2.1684043449710089e-19 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000058 -0.00019999999999999987 0.0002000000000000002 0.00020000000000000044 -0.00020000000000000001 0.00020000000000000017\n"; // clang-format on return aux.str(); } @@ -340,5 +340,4 @@ g2o::VectorX createTestVectorX() { return result; } -} // namespace internal -} // namespace g2o +} // namespace g2o::internal diff --git a/unit_test/stuff/filesys_tools_tests.cpp b/unit_test/stuff/filesys_tools_tests.cpp index fd1be156f..6f3436d50 100644 --- a/unit_test/stuff/filesys_tools_tests.cpp +++ b/unit_test/stuff/filesys_tools_tests.cpp @@ -41,7 +41,9 @@ TEST(Stuff, GetFileExtension) { TEST(Stuff, GetPureFilename) { EXPECT_EQ("test", g2o::getPureFilename("test.txt")); EXPECT_EQ("test", g2o::getPureFilename("test")); +#ifndef WINDOWS EXPECT_EQ("/home/g2o/test", g2o::getPureFilename("/home/g2o/test.txt")); +#endif EXPECT_EQ("", g2o::getPureFilename("")); } diff --git a/unit_test/test_helper/CMakeLists.txt b/unit_test/test_helper/CMakeLists.txt index 371bc795a..fbb2f4248 100644 --- a/unit_test/test_helper/CMakeLists.txt +++ b/unit_test/test_helper/CMakeLists.txt @@ -1,4 +1,5 @@ add_library(unittest_helper ${G2O_LIB_TYPE} + utils.cpp allocate_optimizer.cpp allocate_optimizer.h ) -target_link_libraries(unittest_helper solver_eigen) +target_link_libraries(unittest_helper solver_eigen stuff gmock) diff --git a/unit_test/test_helper/io.h b/unit_test/test_helper/io.h deleted file mode 100644 index 0d68c9f5c..000000000 --- a/unit_test/test_helper/io.h +++ /dev/null @@ -1,106 +0,0 @@ -// g2o - General Graph Optimization -// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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 TESTHELPER_IO_H -#define TESTHELPER_IO_H - -#include -#include -#include - -#include "gtest/gtest.h" - -namespace g2o { - -template -void readWriteGraphElement(const T& output, T* input) { - std::stringstream data; - EXPECT_TRUE(output.write(data)); - EXPECT_TRUE(input->read(data)); -} - -template -void randomizeInformationMatrix(Eigen::MatrixBase& m) { - m = Derived::Random(m.rows(), m.cols()); - m = m * m.transpose(); - m += m.rows() * Derived::Identity(); -} - -/** - * Small wrapper to allow passing optional pointers - * TODO when we switch to C++17 use std::optional instead - */ -template -struct OptionalPtr { - OptionalPtr() : ptr(new T) {} - OptionalPtr(std::shared_ptr p) : ptr(p) {} // NOLINT - std::shared_ptr ptr; -}; - -template -struct RandomEstimate { - static typename T::EstimateType create() { return T::EstimateType::Random(); } - static bool isApprox(const typename T::EstimateType& a, - const typename T::EstimateType& b) { - return a.isApprox(b, 1e-5); - } -}; - -template -struct RandomMeasurement { - static typename T::Measurement create() { return T::Measurement::Random(); } - static bool isApprox(const typename T::Measurement& a, - const typename T::Measurement& b) { - return a.isApprox(b, 1e-5); - } -}; - -template > -void readWriteVectorBasedVertex( - OptionalPtr outputVertex = OptionalPtr()) { - outputVertex.ptr->setEstimate(RandomEstimateFunctor::create()); - T inputVertex; - readWriteGraphElement(*outputVertex.ptr, &inputVertex); - ASSERT_TRUE(RandomEstimateFunctor::isApprox(outputVertex.ptr->estimate(), - inputVertex.estimate())); -} - -template > -void readWriteVectorBasedEdge(OptionalPtr outputEdge = OptionalPtr()) { - outputEdge.ptr->setMeasurement(RandomMeasurementFunctor::create()); - randomizeInformationMatrix(outputEdge.ptr->information()); - T inputEdge; - readWriteGraphElement(*outputEdge.ptr, &inputEdge); - ASSERT_TRUE(RandomMeasurementFunctor::isApprox(outputEdge.ptr->measurement(), - inputEdge.measurement())); - ASSERT_TRUE( - outputEdge.ptr->information().isApprox(inputEdge.information(), 1e-5)); - ASSERT_TRUE(outputEdge.ptr->parameterIds() == inputEdge.parameterIds()); -} - -} // namespace g2o - -#endif diff --git a/unit_test/test_helper/random_state.h b/unit_test/test_helper/random_state.h index 19acd2bf8..324b3354c 100644 --- a/unit_test/test_helper/random_state.h +++ b/unit_test/test_helper/random_state.h @@ -24,15 +24,16 @@ // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + #include -#include "g2o/config.h" #include "g2o/core/eigen_types.h" #include "g2o/stuff/sampler.h" +#include "g2o/types/slam2d/se2.h" #include "g2o/types/slam3d/se3quat.h" -namespace g2o { -namespace internal { +namespace g2o::internal { inline Isometry3 randomIsometry3() { const g2o::Vector3 rotAxisAngle = g2o::Vector3::Random() + g2o::Vector3::Random(); @@ -65,5 +66,48 @@ struct RandomDouble { } }; -} // namespace internal -} // namespace g2o +template +struct RandomValue { + using Type = T; + template + struct FakeDependency : public std::false_type {}; + static Type create() { +#ifndef _MSC_VER + static_assert(FakeDependency::value, + "No specialization for RandomValue provided"); +#endif + return T{}; + } +}; + +template <> +struct RandomValue { + using Type = double; + static Type create() { return RandomDouble::create(); } +}; + +template <> +struct RandomValue { + using Type = g2o::SE2; + static Type create() { return Type(g2o::Vector3::Random()); } +}; + +template <> +struct RandomValue { + using Type = g2o::SE3Quat; + static Type create() { return RandomSE3Quat::create(); } +}; + +template <> +struct RandomValue { + using Type = g2o::Isometry3; + static Type create() { return RandomIsometry3::create(); } +}; + +template +struct RandomValue> { + using Type = g2o::VectorN; + static Type create() { return g2o::VectorN::Random(); } +}; + +} // namespace g2o::internal diff --git a/unit_test/test_helper/typed_basic_tests.h b/unit_test/test_helper/typed_basic_tests.h new file mode 100644 index 000000000..a95b02c54 --- /dev/null +++ b/unit_test/test_helper/typed_basic_tests.h @@ -0,0 +1,285 @@ +// g2o - General Graph Optimization +// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, 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. + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "g2o/core/io/io_format.h" +#include "g2o/core/optimizable_graph.h" +#include "g2o/core/sparse_optimizer.h" +#include "g2o/stuff/string_tools.h" +#include "unit_test/test_helper/allocate_optimizer.h" +#include "unit_test/test_helper/eigen_matcher.h" +#include "unit_test/test_helper/random_state.h" +#include "unit_test/test_helper/utils.h" + +namespace g2o::internal::testing { +/** + * @brief Matcher for testing Numeric and Analytic Jacobian to be approximately + * equal. + */ +MATCHER_P2(JacobianApproxEqual, expect, prec, + std::string(negation ? "isn't" : "is") + " approx equal to " + + ::testing::PrintToString(expect)) { + if (arg.size() != expect.size()) return false; + for (int j = 0; j < arg.size(); ++j) { + double diff = std::abs(arg(j) - expect(j)); + if (diff > prec(arg(j), expect(j))) return false; + } + return true; +} +} // namespace g2o::internal::testing + +/** + * @brief A typed test for Edges. + * + * @tparam T A tuple like type whereas the first type gives the type of the Edge + * and the second determines the type of a parameter. Providing the type of a + * parameter is optional. + */ +template +struct FixedSizeEdgeBasicTests : public ::testing::Test { + public: + using EpsilonFunction = std::function; + using EdgeType = typename std::tuple_element<0, T>::type; + + // TODO(Rainer): Allow parameterization for the epsilon function + EpsilonFunction epsilon = [](const double, const double) { return 1e-2; }; + + FixedSizeEdgeBasicTests() = default; + + void SetUp() override { + constexpr std::size_t kTupleSize = std::tuple_size_v; + + // Construct a small graph + edge_ = std::make_shared(); + + // Initialize the vertices of the edge to a random state + for (std::size_t i = 0; i < edge_->vertices().size(); ++i) { + auto v = std::shared_ptr( + edge_->createVertex(i)); + edge_->vertices()[i] = v; + v->setId(i); + initializeNthVertex(i, *edge_); + this->optimizer_ptr_->addVertex(v); + } + + // Add the parameter types if required + if constexpr (kTupleSize > 1) { + using ParamTuple = typename TupleTypes::Tail; + constexpr std::size_t kParamSize = std::tuple_size_v; + for (std::size_t i = 0; i < kParamSize; ++i) { + addNthParameter(i, *edge_); + } + } + + // Generate a random information matrix + const typename EdgeType::InformationType information_matrix = []() { + const typename EdgeType::InformationType aux = + EdgeType::InformationType::Random(); + typename EdgeType::InformationType result = 0.5 * (aux + aux.transpose()); + result.diagonal().array() += result.cols(); + return result; + }(); + + edge_->setInformation(information_matrix); + edge_->setMeasurement( + g2o::internal::RandomValue::create()); + this->optimizer_ptr_->addEdge(edge_); + + this->numeric_jacobian_workspace_.updateSize(*edge_); + this->numeric_jacobian_workspace_.allocate(); + } + + protected: + std::shared_ptr edge_; + + std::unique_ptr optimizer_ptr_ = + g2o::internal::createOptimizerForTests(); + + template + static std::enable_if_t initializeNthVertex(int, EdgeType&) {} + + template + static std::enable_if_t initializeNthVertex(int i, + EdgeType& t) { + if (i == I) { + auto vertex = t.template vertexXn(); + vertex->setEstimate( + g2o::internal::RandomValue::EstimateType>::create()); + return; + } + initializeNthVertex(i, t); + } + + template + std::enable_if_t addNthParameter(int, EdgeType&) {} + + template + std::enable_if_t addNthParameter(int i, EdgeType& edge) { + if (i != I) { + addNthParameter(i, edge); + } + using ParameterType = typename std::tuple_element::type; + auto param = std::make_shared(); + param->setId(i + 100); + // TODO(Rainer): Modify the parameter to a random value + this->optimizer_ptr_->addParameter(param); + edge.setParameterId(i, param->id()); + } + + template + struct TupleTypes; + + template + struct TupleTypes> { + using Head = HeadType; + using Tail = std::tuple; + }; + + /** + * @brief Helper template to compute the Jacobian numerically and by the + * implementation of the Edge class. + * + * @tparam Ints Index sequence over the EdgeType::kNrOfVertices + */ + template + void compute(EdgeType& edge, std::index_sequence /*unused*/) { + // calling the analytic Jacobian but writing to the numeric workspace + edge.template BaseFixedSizedEdge< + EdgeType::kDimension, typename EdgeType::Measurement, + typename EdgeType::template VertexXnType...>:: + linearizeOplus(this->numeric_jacobian_workspace_); + // copy result into analytic workspace + this->jacobian_workspace_ = this->numeric_jacobian_workspace_; + this->numeric_jacobian_workspace_.setZero(); + + // compute the numeric Jacobian into the this->numeric_jacobian_workspace_ + // workspace as setup by the previous call + edge.template BaseFixedSizedEdge< + EdgeType::kDimension, typename EdgeType::Measurement, + typename EdgeType::template VertexXnType...>::linearizeOplus(); + } + + g2o::JacobianWorkspace jacobian_workspace_; + g2o::JacobianWorkspace numeric_jacobian_workspace_; +}; +TYPED_TEST_SUITE_P(FixedSizeEdgeBasicTests); + +TYPED_TEST_P(FixedSizeEdgeBasicTests, SaveAndLoad) { + using namespace testing; // NOLINT + constexpr g2o::io::Format kFormat = g2o::io::Format::kG2O; + + std::stringstream buffer; + bool save_result = this->optimizer_ptr_->save(buffer, kFormat); + ASSERT_THAT(save_result, IsTrue()); + EXPECT_THAT(buffer.str(), Not(IsEmpty())); + + auto loaded_optimizer = g2o::internal::createOptimizerForTests(); + loaded_optimizer->load(buffer, kFormat); + + // Check that serialization result is the same + std::stringstream buffer_after_loading; + save_result = loaded_optimizer->save(buffer_after_loading, kFormat); + ASSERT_THAT(save_result, IsTrue()); + + // Prepare check of serialization result + const std::vector before_lines = + g2o::strSplit(buffer.str(), "\n"); + const std::vector after_lines = + g2o::strSplit(buffer_after_loading.str(), "\n"); + ASSERT_THAT(before_lines, SizeIs(after_lines.size())); + + // Compare before and after line by line + for (std::size_t i = 0; i < before_lines.size(); ++i) { + const std::vector before_tokens = + g2o::strSplit(before_lines[i], " "); + const std::vector after_tokens = + g2o::strSplit(after_lines[i], " "); + ASSERT_THAT(before_tokens, SizeIs(after_tokens.size())); + if (before_tokens.empty()) continue; + EXPECT_THAT(before_tokens.front(), Eq(after_tokens.front())); + for (std::size_t j = 1; j < before_tokens.size(); ++j) { + const std::string& before_token = before_tokens[j]; + const std::string& after_token = after_tokens[j]; + const double before_value = std::stod(before_token); + const double after_value = std::stod(after_token); + EXPECT_THAT(before_value, DoubleNear(after_value, 1e-4)) + << " Line " << i << " differs token " << j << ": " << before_tokens[j] + << " " << after_tokens[j]; + } + } +} + +// TODO(Rainer): With MSVC on Windows tests with parameters are failing. +#ifndef WINDOWS +TYPED_TEST_P(FixedSizeEdgeBasicTests, Jacobian) { + using EdgeType = typename std::tuple_element<0, TypeParam>::type; + + this->compute(*(this->edge_), + std::make_index_sequence()); + + // compare the two Jacobians + for (std::size_t i = 0; i < this->edge_->vertices().size(); ++i) { + int numElems = EdgeType::kDimension; + auto* vertex = static_cast( + this->edge_->vertex(i).get()); + numElems *= vertex->dimension(); + g2o::VectorX::ConstMapType n( + this->numeric_jacobian_workspace_.workspaceForVertex(i), numElems); + g2o::VectorX::ConstMapType a( + this->jacobian_workspace_.workspaceForVertex(i), numElems); + EXPECT_THAT(g2o::internal::print_wrap(a), + g2o::internal::testing::JacobianApproxEqual( + g2o::internal::print_wrap(n), this->epsilon)); + } +} +#endif + +#ifdef WINDOWS +REGISTER_TYPED_TEST_SUITE_P(FixedSizeEdgeBasicTests, SaveAndLoad); +#else +REGISTER_TYPED_TEST_SUITE_P(FixedSizeEdgeBasicTests, SaveAndLoad, Jacobian); +#endif + +namespace g2o::internal { +class DefaultTypeNames { + public: + template + static std::string GetName(int) { + return ExtractTupleHead(::testing::internal::GetTypeName()); + } +}; + +} // namespace g2o::internal diff --git a/g2o/core/io_helper.h b/unit_test/test_helper/utils.cpp similarity index 75% rename from g2o/core/io_helper.h rename to unit_test/test_helper/utils.cpp index 91555fa14..be68a51e4 100644 --- a/g2o/core/io_helper.h +++ b/unit_test/test_helper/utils.cpp @@ -24,24 +24,22 @@ // 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_CORE_IO_HELPER_H -#define G2O_CORE_IO_HELPER_H +#include "utils.h" -#include -#include +#include -namespace g2o::internal { -template -bool writeVector(std::ostream& os, const Eigen::DenseBase& b) { - for (int i = 0; i < b.size(); i++) os << b(i) << " "; - return os.good(); -} +#include "g2o/stuff/string_tools.h" -template -bool readVector(std::istream& is, Eigen::DenseBase& b) { - for (int i = 0; i < b.size() && is.good(); i++) is >> b(i); - return is.good() || is.eof(); +namespace g2o::internal { +std::string ExtractTupleHead(const std::string& input) { + const std::regex tuple_regex(".*tuple<([^,>]+)[,>].*"); + std::smatch match; + if (!std::regex_match(input, match, tuple_regex)) return input; + if (match.size() != 2) return input; + std::string result = match[1].str(); + if (strStartsWith(result, "g2o::")) { + return result.substr(5); + } + return result; } } // namespace g2o::internal - -#endif diff --git a/g2o/types/sclam2d/types_sclam2d.h b/unit_test/test_helper/utils.h similarity index 82% rename from g2o/types/sclam2d/types_sclam2d.h rename to unit_test/test_helper/utils.h index a4256dccc..8f7b08676 100644 --- a/g2o/types/sclam2d/types_sclam2d.h +++ b/unit_test/test_helper/utils.h @@ -1,5 +1,5 @@ // g2o - General Graph Optimization -// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -24,10 +24,14 @@ // 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_TYPES_SCLAM_H -#define G2O_TYPES_SCLAM_H +#pragma once -#include "edge_se2_odom_differential_calib.h" -#include "edge_se2_sensor_calib.h" +#include -#endif +#include "unit_test/test_helper/g2o_test_helper_api.h" + +namespace g2o::internal { + +G2O_TEST_HELPER_API std::string ExtractTupleHead(const std::string& input); + +} // namespace g2o::internal