From b05d2578e5e0de3514ba5fe26d7a068a06f1dbfd Mon Sep 17 00:00:00 2001 From: JaeOh Lee Date: Tue, 5 May 2020 01:16:19 -0400 Subject: [PATCH 1/5] Add VertexPlane Types --- python/types/slam3d_addons/edge_plane.h | 0 python/types/slam3d_addons/plane3d.h | 50 +++++++++++++++++++ .../types/slam3d_addons/types_slam3d_addons.h | 46 +++++++++++++++++ python/types/slam3d_addons/vertex_plane.h | 43 ++++++++++++++++ python/types/types.h | 4 ++ 5 files changed, 143 insertions(+) create mode 100644 python/types/slam3d_addons/edge_plane.h create mode 100644 python/types/slam3d_addons/plane3d.h create mode 100644 python/types/slam3d_addons/types_slam3d_addons.h create mode 100644 python/types/slam3d_addons/vertex_plane.h diff --git a/python/types/slam3d_addons/edge_plane.h b/python/types/slam3d_addons/edge_plane.h new file mode 100644 index 0000000..e69de29 diff --git a/python/types/slam3d_addons/plane3d.h b/python/types/slam3d_addons/plane3d.h new file mode 100644 index 0000000..8fe29b6 --- /dev/null +++ b/python/types/slam3d_addons/plane3d.h @@ -0,0 +1,50 @@ +#include +#include + +#include +#include "python/core/base_vertex.h" +#include "python/core/base_edge.h" + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace g2o { + +void declarePlane3D(py::module & m){ + + py::class_(m, "Plane3D") + .def(py::init<>()) + .def(py::init(), + "v"_a) + + .def("to_vector", &Plane3D::toVector) + .def("coeffs", &Plane3D::coeffs) + .def("from_vector", &Plane3D::fromVector, + "coeffs"_a) + .def_static("azimuth", &Plane3D::azimuth, + "v"_a) + .def_static("elevation", &Plane3D::elevation, + "v"_a) + .def("distance", &Plane3D::distance) + .def("normal", &Plane3D::normal) + + .def_static("rotation", &Plane3D::rotation, + "v"_a) + .def("oplus", &Plane3D::oplus, + "v"_a) + .def("ominus", &Plane3D::ominus, + "plane"_a) + + .def_static("normalize", &Plane3D::normalize, + "coeffs"_a) + + // operator + .def(Eigen::Isometry3d() * py::self) + ; + templatedBaseVertex<3, Plane3D>(m, "_3_Plane3D"); + templatedBaseEdge<3, Plane3D>(m, "_3_Plane3D"); + templatedBaseMultiEdge<3, Plane3D>(m, "_3_Plane3D"); + +} + +} // end namespace g2o \ No newline at end of file diff --git a/python/types/slam3d_addons/types_slam3d_addons.h b/python/types/slam3d_addons/types_slam3d_addons.h new file mode 100644 index 0000000..4807887 --- /dev/null +++ b/python/types/slam3d_addons/types_slam3d_addons.h @@ -0,0 +1,46 @@ +#include + +#include +#include +#include +#include +#include + +// #include "se3quat.h" +// #include "vertex_se3.h" +// #include "vertex_pointxyz.h" + +// #include "edge_pointxyz.h" +// #include "edge_se3.h" +// #include "edge_se3_pointxyz.h" +#include "plane3d.h" +#include "vertex_plane.h" + + +namespace g2o { + + +// register types +// slam3d_addons +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); + +G2O_REGISTER_TYPE(VERTEX_LINE3D, VertexLine3D); +G2O_REGISTER_TYPE(EDGE_SE3_LINE3D, EdgeSE3Line3D); +G2O_REGISTER_TYPE(EDGE_PLANE, EdgePlane); +G2O_REGISTER_TYPE(EDGE_SE3_CALIB, EdgeSE3Calib); + + +void declareTypesSlam3dAddons(py::module & m) { + + declarePlane3D(m); + + declareVertexPlane(m); + + +} + +} \ No newline at end of file diff --git a/python/types/slam3d_addons/vertex_plane.h b/python/types/slam3d_addons/vertex_plane.h new file mode 100644 index 0000000..369b6e6 --- /dev/null +++ b/python/types/slam3d_addons/vertex_plane.h @@ -0,0 +1,43 @@ +#include +#include + +#include +#include + + +namespace py = pybind11; +using namespace pybind11::literals; + + +namespace g2o { + +void declareVertexPlane(py::module & m) { + + py::class_>(m, "VertexPlane") + .def(py::init<>()) + + .def("set_to_origin_impl", &VertexPlane::setToOriginImpl) + .def("oplus_impl", &VertexPlane::oplusImpl) + .def("set_estimate_data_impl", &VertexPlane::setEstimateDataImpl) + .def("get_estimate_data", &VertexPlane::getEstimateData) + .def("estimate_dimension", &VertexPlane::estimateDimension) + ; + + + /* + py::class_(m, "VertexSE3WriteGnuplotAction") + .def(py::init<>()) + .def("__call__", &VertexSE3WriteGnuplotAction::operator()) + ; + + // #ifdef G2O_HAVE_OPENGL + py::class_(m, "VertexSE3DrawAction") + .def(py::init<>()) + .def("__call__", &VertexSE3DrawAction::operator()) + ; + // #endif + */ + +} + +} // end namespace g2o \ No newline at end of file diff --git a/python/types/types.h b/python/types/types.h index 55da2cf..d31fe7a 100644 --- a/python/types/types.h +++ b/python/types/types.h @@ -2,6 +2,7 @@ #include "slam2d/types_slam2d.h" #include "slam3d/types_slam3d.h" +#include "slam3d_addons/types_slam3d_addons.h" #include "sba/types_six_dof_expmap.h" #include "sba/types_sba.h" @@ -30,6 +31,9 @@ void declareTypes(py::module & m) { // slam3d declareTypesSlam3d(m); + // slam3d_addons + declareTypesSlam3dAddons(m); + // sba declareTypesSBA(m); declareTypesSixDofExpmap(m); From f94912fa9a01acc347903163e387a4ffa495a096 Mon Sep 17 00:00:00 2001 From: JaeOh Lee Date: Sun, 17 May 2020 23:54:05 -0400 Subject: [PATCH 2/5] Add edge_plane binding --- python/types/slam3d_addons/edge_plane.h | 33 +++++++++++++++++++ .../types/slam3d_addons/types_slam3d_addons.h | 2 ++ 2 files changed, 35 insertions(+) diff --git a/python/types/slam3d_addons/edge_plane.h b/python/types/slam3d_addons/edge_plane.h index e69de29..0c5d9e4 100644 --- a/python/types/slam3d_addons/edge_plane.h +++ b/python/types/slam3d_addons/edge_plane.h @@ -0,0 +1,33 @@ +#include +#include + +#include +#include +#include + + +namespace py = pybind11; +using namespace pybind11::literals; + + +namespace g2o { + +void declareEdgePlane(py::module & m) { + + templatedBaseBinaryEdge<4, Vector4D, VertexPlane, VertexPlane>(m, "_4_Vector4D_VertexPlane_VertexPlane"); + py::class_>(m, "EdgePlane") + .def(py::init<>()) + + .def("compute_error", &EdgePlane::computeError) + .def("set_measurement", &EdgePlane::setMeasurement) + .def("set_measurement_data", &EdgePlane::setMeasurementData) + .def("get_measurement_data", &EdgePlane::getMeasurementData) + .def("measurement_dimension", &EdgePlane::measurementDimension) + .def("set_measurement_from_state", &EdgePlane::setMeasurementFromState) + .def("initial_estimate_possible", &EdgePlane::initialEstimatePossible) + + ; + +} + +} // end namespace g2o diff --git a/python/types/slam3d_addons/types_slam3d_addons.h b/python/types/slam3d_addons/types_slam3d_addons.h index 4807887..cc9fbe6 100644 --- a/python/types/slam3d_addons/types_slam3d_addons.h +++ b/python/types/slam3d_addons/types_slam3d_addons.h @@ -15,6 +15,7 @@ // #include "edge_se3_pointxyz.h" #include "plane3d.h" #include "vertex_plane.h" +#include "edge_plane.h" namespace g2o { @@ -39,6 +40,7 @@ void declareTypesSlam3dAddons(py::module & m) { declarePlane3D(m); declareVertexPlane(m); + declareEdgePlane(m); } From 7a1a734910bd62cf9567d2fa07aadbdb204f43c1 Mon Sep 17 00:00:00 2001 From: JaeOh Lee Date: Tue, 19 May 2020 15:06:56 -0400 Subject: [PATCH 3/5] Update EdgeSE3Plane() custom operation --- g2o/types/slam3d_addons/CMakeLists.txt | 2 + g2o/types/slam3d_addons/edge_se3_plane.cpp | 43 +++++++++++++++++++ g2o/types/slam3d_addons/edge_se3_plane.h | 31 +++++++++++++ python/types/slam3d_addons/edge_se3_plane.h | 28 ++++++++++++ .../types/slam3d_addons/types_slam3d_addons.h | 2 + 5 files changed, 106 insertions(+) create mode 100644 g2o/types/slam3d_addons/edge_se3_plane.cpp create mode 100644 g2o/types/slam3d_addons/edge_se3_plane.h create mode 100644 python/types/slam3d_addons/edge_se3_plane.h diff --git a/g2o/types/slam3d_addons/CMakeLists.txt b/g2o/types/slam3d_addons/CMakeLists.txt index 8331c2a..8526bc7 100644 --- a/g2o/types/slam3d_addons/CMakeLists.txt +++ b/g2o/types/slam3d_addons/CMakeLists.txt @@ -7,6 +7,8 @@ ADD_LIBRARY(types_slam3d_addons ${G2O_LIB_TYPE} vertex_plane.h edge_se3_plane_calib.cpp edge_se3_plane_calib.h + edge_se3_plane.cpp + edge_se3_plane.h line3d.cpp line3d.h vertex_line3d.cpp vertex_line3d.h edge_se3_line.cpp edge_se3_line.h diff --git a/g2o/types/slam3d_addons/edge_se3_plane.cpp b/g2o/types/slam3d_addons/edge_se3_plane.cpp new file mode 100644 index 0000000..64a0346 --- /dev/null +++ b/g2o/types/slam3d_addons/edge_se3_plane.cpp @@ -0,0 +1,43 @@ +// refer from : https://raw.githubusercontent.com/koide3/hdl_graph_slam/5447b906f1f3d8eef28021ce15d8d5888d223f9e/include/g2o/edge_se3_plane.hpp + +#include +#include + +namespace g2o { + + EdgeSE3Plane::EdgeSE3Plane() : g2o::BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane>() { + + } + + bool EdgeSE3Plane::read(std::istream& is) { + Eigen::Vector4d v; + is >> v(0) >> v(1) >> v(2) >> v(3); + setMeasurement(Plane3D(v)); + for (int i = 0; i < information().rows(); ++i) + for (int j = i; j < information().cols(); ++j) { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; + } + + bool EdgeSE3Plane::write(std::ostream& os) const{ + Eigen::Vector4d v = _measurement.toVector(); + os << v(0) << " " << v(1) << " " << v(2) << " " << v(3) << " "; + for (int i = 0; i < information().rows(); ++i) + for (int j = i; j < information().cols(); ++j) + os << " " << information()(i, j); + return os.good(); + } + + void EdgeSE3Plane::computeError() { + const g2o::VertexSE3* v1 = static_cast(_vertices[0]); + const g2o::VertexPlane* v2 = static_cast(_vertices[1]); + + Eigen::Isometry3d w2n = v1->estimate().inverse(); + Plane3D local_plane = w2n * v2->estimate(); + _error = local_plane.ominus(_measurement); + } +} + diff --git a/g2o/types/slam3d_addons/edge_se3_plane.h b/g2o/types/slam3d_addons/edge_se3_plane.h new file mode 100644 index 0000000..01d6fea --- /dev/null +++ b/g2o/types/slam3d_addons/edge_se3_plane.h @@ -0,0 +1,31 @@ +#ifndef G2O_EDGE_SE3_PLANE_H_ +#define G2O_EDGE_SE3_PLANE_H_ +// refer from : https://raw.githubusercontent.com/koide3/hdl_graph_slam/5447b906f1f3d8eef28021ce15d8d5888d223f9e/include/g2o/edge_se3_plane.hpp + +#include "g2o/core/base_binary_edge.h" +#include "g2o_types_slam3d_addons_api.h" + +#include +#include +#include + +namespace g2o { + + class EdgeSE3Plane : public g2o::BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane> { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EdgeSE3Plane(); + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + void computeError(); + + virtual void setMeasurement(const g2o::Plane3D& m){ + _measurement = m; + } + + + }; +} + +#endif diff --git a/python/types/slam3d_addons/edge_se3_plane.h b/python/types/slam3d_addons/edge_se3_plane.h new file mode 100644 index 0000000..8f6b0c6 --- /dev/null +++ b/python/types/slam3d_addons/edge_se3_plane.h @@ -0,0 +1,28 @@ +#include +#include + +#include +#include +#include +#include + + +namespace py = pybind11; +using namespace pybind11::literals; + + +namespace g2o { + +void declareEdgeSE3Plane(py::module & m) { + + templatedBaseBinaryEdge<3, Plane3D, VertexSE3, VertexPlane>(m, "_3_Plane3D_VertexSE3_VertexPlane"); + py::class_>(m, "EdgeSE3Plane") + .def(py::init<>()) + + .def("compute_error", &EdgeSE3Plane::computeError) + .def("set_measurement", &EdgeSE3Plane::setMeasurement) + ; + +} + +} // end namespace g2o diff --git a/python/types/slam3d_addons/types_slam3d_addons.h b/python/types/slam3d_addons/types_slam3d_addons.h index cc9fbe6..a5ecddc 100644 --- a/python/types/slam3d_addons/types_slam3d_addons.h +++ b/python/types/slam3d_addons/types_slam3d_addons.h @@ -16,6 +16,7 @@ #include "plane3d.h" #include "vertex_plane.h" #include "edge_plane.h" +#include "edge_se3_plane.h" namespace g2o { @@ -41,6 +42,7 @@ void declareTypesSlam3dAddons(py::module & m) { declareVertexPlane(m); declareEdgePlane(m); + declareEdgeSE3Plane(m); } From b151c8fc2a4ec6c2f6f85f9147baa4f24ca52415 Mon Sep 17 00:00:00 2001 From: JaeOh Lee Date: Tue, 19 May 2020 16:28:21 -0400 Subject: [PATCH 4/5] Add G2O_REGISTER_TYPE(EDGE_SE3_PLANE, EdgeSE3Plane) --- python/types/slam3d_addons/types_slam3d_addons.h | 1 + 1 file changed, 1 insertion(+) diff --git a/python/types/slam3d_addons/types_slam3d_addons.h b/python/types/slam3d_addons/types_slam3d_addons.h index a5ecddc..a575934 100644 --- a/python/types/slam3d_addons/types_slam3d_addons.h +++ b/python/types/slam3d_addons/types_slam3d_addons.h @@ -28,6 +28,7 @@ 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, EdgeSE3Plane) G2O_REGISTER_TYPE(EDGE_SE3_PLANE_CALIB, EdgeSE3PlaneSensorCalib); G2O_REGISTER_TYPE(VERTEX_LINE3D, VertexLine3D); From 4c5304f3f3f22b3d8126c3b9b848f857c314f55e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=EC=9D=B4=EC=9E=AC=EC=98=A4?= Date: Wed, 11 May 2022 14:48:29 +0900 Subject: [PATCH 5/5] Fix build error on Ubuntu 20.04 --- CMakeLists.txt | 4 +++- python/core/eigen_types.h | 10 +++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7578bec..9329b92 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.1) SET(CMAKE_LEGACY_CYGWIN_WIN32 0) +set(OpenGL_GL_PREFERENCE LEGACY) + PROJECT(g2o) @@ -295,4 +297,4 @@ ADD_SUBDIRECTORY(EXTERNAL) ADD_SUBDIRECTORY(g2o) ADD_SUBDIRECTORY(contrib) # added for python binding -ADD_SUBDIRECTORY(python) # added for python binding \ No newline at end of file +ADD_SUBDIRECTORY(python) # added for python binding diff --git a/python/core/eigen_types.h b/python/core/eigen_types.h index b58d529..73850c9 100644 --- a/python/core/eigen_types.h +++ b/python/core/eigen_types.h @@ -182,10 +182,10 @@ void declareEigenTypes(py::module & m) { return Eigen::Quaterniond::FromTwoVectors(a, b); }) - .def("x", (double (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::x) - .def("y", (double (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::y) - .def("z", (double (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::z) - .def("w", (double (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::w) + .def("x", [](const Eigen::Quaterniond& q) { return q.x(); }) + .def("y", [](const Eigen::Quaterniond& q) { return q.y(); }) + .def("z", [](const Eigen::Quaterniond& q) { return q.z(); }) + .def("w", [](const Eigen::Quaterniond& q) { return q.w(); }) .def("vec", (const Eigen::VectorBlock (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::vec) @@ -292,4 +292,4 @@ void declareEigenTypes(py::module & m) { } -} // end namespace g2o \ No newline at end of file +} // end namespace g2o