Added plane3d data type

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
+ADD_SUBDIRECTORY(python)   # added for python binding

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

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 <g2o/types/slam3d/edge_se3.h>
+#include <g2o/types/slam3d_addons/edge_se3_plane.h>
+
+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<const g2o::VertexSE3*>(_vertices[0]);
+        const g2o::VertexPlane* v2 = static_cast<const g2o::VertexPlane*>(_vertices[1]);
+
+        Eigen::Isometry3d w2n = v1->estimate().inverse();
+        Plane3D local_plane = w2n * v2->estimate();
+        _error = local_plane.ominus(_measurement);
+    }
+}
+

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 <g2o/types/slam3d/edge_se3.h>
+#include <g2o/types/slam3d/vertex_se3.h>
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+
+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

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<const Eigen::Quaterniond::Coefficients,3> (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::vec)
 
@@ -292,4 +292,4 @@ void declareEigenTypes(py::module & m) {
 
 }
 
-}  // end namespace g2o
+}  // end namespace g2o

python/types/slam3d_addons/edge_plane.h

@@ -0,0 +1,33 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+
+#include <python/core/base_edge.h>
+#include <g2o/types/slam3d_addons/edge_plane.h>
+#include <g2o/types/slam3d_addons/edge_se3_plane_calib.h>
+
+
+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_<EdgePlane, BaseBinaryEdge<4, Vector4D, VertexPlane, VertexPlane>>(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

python/types/slam3d_addons/edge_se3_plane.h

@@ -0,0 +1,28 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+
+#include <g2o/types/slam3d/vertex_se3.h>
+#include <g2o/types/slam3d/edge_se3.h>
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+#include <g2o/types/slam3d_addons/edge_se3_plane.h>
+
+
+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_<EdgeSE3Plane, BaseBinaryEdge<3, Plane3D, VertexSE3, VertexPlane>>(m, "EdgeSE3Plane")
+            .def(py::init<>())
+
+            .def("compute_error", &EdgeSE3Plane::computeError)
+            .def("set_measurement", &EdgeSE3Plane::setMeasurement)
+        ;
+
+}
+
+}  // end namespace g2o

python/types/slam3d_addons/plane3d.h

@@ -0,0 +1,50 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/operators.h>
+
+#include <g2o/types/slam3d_addons/plane3d.h>
+#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_<Plane3D>(m, "Plane3D")
+        .def(py::init<>())
+        .def(py::init<const Eigen::Vector4d&>(),
+            "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

python/types/slam3d_addons/types_slam3d_addons.h

@@ -0,0 +1,51 @@
+#include <pybind11/pybind11.h>
+
+#include <g2o/types/slam3d/types_slam3d.h>
+#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
+#include <g2o/types/slam3d_addons/g2o_types_slam3d_addons_api.h>
+#include <g2o/core/factory.h>
+#include <g2o/stuff/macros.h>
+
+// #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"
+#include "edge_plane.h"
+#include "edge_se3_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, EdgeSE3Plane)
+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);
+    declareEdgePlane(m);
+    declareEdgeSE3Plane(m);
+
+
+}
+
+}

python/types/slam3d_addons/vertex_plane.h

@@ -0,0 +1,43 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+#include <g2o/types/slam3d_addons/plane3d.h>
+
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+
+namespace g2o {
+
+void declareVertexPlane(py::module & m) {
+
+    py::class_<VertexPlane, BaseVertex<3, Plane3D>>(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_<VertexSE3WriteGnuplotAction, WriteGnuplotAction>(m, "VertexSE3WriteGnuplotAction")
+        .def(py::init<>())
+        .def("__call__", &VertexSE3WriteGnuplotAction::operator())
+    ;
+
+    // #ifdef G2O_HAVE_OPENGL
+    py::class_<VertexSE3DrawAction, DrawAction>(m, "VertexSE3DrawAction")
+        .def(py::init<>())
+        .def("__call__", &VertexSE3DrawAction::operator())
+    ;
+    // #endif
+    */
+
+}
+
+}  // end namespace g2o

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);