From f0aa43742e82e4b7cf56ecae54a68d7ee8cc591b Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Thu, 5 Apr 2018 20:24:53 +0200
Subject: [PATCH] [Geometry] Add {from,to}EulerAngles convertor
---
CMakeLists.txt | 5 ++-
python/main.cpp | 3 +-
src/geometry-conversion.cpp | 27 ++++++++++++++
src/geometry-conversion.hpp | 74 +++++++++++++++++++++++++++++++++++++
src/geometry.hpp | 4 +-
5 files changed, 110 insertions(+), 3 deletions(-)
create mode 100644 src/geometry-conversion.cpp
create mode 100644 src/geometry-conversion.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 032b57a8..03f01554 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
#
-# Copyright (c) 2015-2017 LAAS-CNRS
+# Copyright (c) 2015-2018 LAAS-CNRS
#
# This file is part of eigenpy.
# eigenpy is free software: you can redistribute it and/or
@@ -94,6 +94,7 @@ SET(HEADERS
fwd.hpp
map.hpp
geometry.hpp
+ geometry-conversion.hpp
memory.hpp
registration.hpp
angle-axis.hpp
@@ -126,12 +127,14 @@ SET(${PROJECT_NAME}_SOLVERS_SOURCES
src/solvers/preconditioners.cpp
src/solvers/solvers.cpp
)
+
SET(${PROJECT_NAME}_SOURCES
${${PROJECT_NAME}_SOLVERS_SOURCES}
src/exception.cpp
src/eigenpy.cpp
src/angle-axis.cpp
src/quaternion.cpp
+ src/geometry-conversion.cpp
)
ADD_LIBRARY(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
diff --git a/python/main.cpp b/python/main.cpp
index b2ce8223..513f173c 100644
--- a/python/main.cpp
+++ b/python/main.cpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2017, Justin Carpentier, CNRS
+// Copyright (c) 2017-2018, Justin Carpentier, CNRS
//
// This file is part of eigenpy
// eigenpy is free software: you can redistribute it
@@ -30,6 +30,7 @@ BOOST_PYTHON_MODULE(eigenpy)
enableEigenPy();
exposeAngleAxis();
exposeQuaternion();
+ exposeGeometryConversion();
{
boost::python::scope solvers = boost::python::class_<SolversScope>("solvers");
diff --git a/src/geometry-conversion.cpp b/src/geometry-conversion.cpp
new file mode 100644
index 00000000..33f8a4d6
--- /dev/null
+++ b/src/geometry-conversion.cpp
@@ -0,0 +1,27 @@
+/*
+ * Copyright 2018, Justin Carpentier, LAAS-CNRS
+ *
+ * This file is part of eigenpy.
+ * eigenpy is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * eigenpy is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with eigenpy. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "eigenpy/memory.hpp"
+#include "eigenpy/geometry.hpp"
+#include "eigenpy/geometry-conversion.hpp"
+
+namespace eigenpy
+{
+ void exposeGeometryConversion()
+ {
+ EulerAnglesConvertor<double>::expose();
+ }
+} // namespace eigenpy
diff --git a/src/geometry-conversion.hpp b/src/geometry-conversion.hpp
new file mode 100644
index 00000000..f9127272
--- /dev/null
+++ b/src/geometry-conversion.hpp
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2018, Justin Carpentier, LAAS-CNRS
+ *
+ * This file is part of eigenpy.
+ * eigenpy is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * eigenpy is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with eigenpy. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __eigenpy_geometry_conversion_hpp__
+#define __eigenpy_geometry_conversion_hpp__
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <boost/python.hpp>
+
+namespace eigenpy
+{
+
+ namespace bp = boost::python;
+
+ template<typename Scalar,int Options=0>
+ struct EulerAnglesConvertor
+ {
+
+ typedef typename Eigen::Matrix<Scalar,3,1,Options> Vector3;
+ typedef typename Eigen::Matrix<Scalar,3,3,Options> Matrix3;
+ typedef typename Vector3::Index Index;
+
+ typedef typename Eigen::AngleAxis<Scalar> AngleAxis;
+
+ static void expose()
+ {
+ bp::def("toEulerAngles",&EulerAnglesConvertor::toEulerAngles,
+ bp::args("mat (dim 3x3)","a0","a1","a2"),
+ "It returns the Euler-angles of the rotation matrix mat using the convention defined by the triplet (a0,a1,a2).");
+
+ bp::def("fromEulerAngles",&EulerAnglesConvertor::fromEulerAngles,
+ bp::args("ea (vector of Euler angles)","a0","a1","a2"),
+ "It returns the rotation matrix associated to the Euler angles using the convention defined by the triplet (a0,a1,a2).");
+ }
+
+ static Vector3 toEulerAngles(const Matrix3 & mat,
+ Index a0,
+ Index a1,
+ Index a2)
+ {
+ return mat.eulerAngles(a0,a1,a2);
+ }
+
+ static Matrix3 fromEulerAngles(const Vector3 & ea,
+ Index a0,
+ Index a1,
+ Index a2)
+ {
+ Matrix3 mat;
+ mat = AngleAxis(ea[0], Vector3::Unit(a0))
+ * AngleAxis(ea[1], Vector3::Unit(a1))
+ * AngleAxis(ea[2], Vector3::Unit(a2));
+ return mat;
+ }
+ };
+
+
+} // namespace eigenpy
+
+#endif // define __eigenpy_geometry_conversion_hpp__
diff --git a/src/geometry.hpp b/src/geometry.hpp
index a0421a90..f5befbd7 100644
--- a/src/geometry.hpp
+++ b/src/geometry.hpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2014, Nicolas Mansard, LAAS-CNRS
+ * Copyright 2014-2018, Nicolas Mansard, Justin Carpentier, LAAS-CNRS
*
* This file is part of eigenpy.
* eigenpy is free software: you can redistribute it and/or
@@ -23,6 +23,8 @@ namespace eigenpy
void exposeQuaternion();
void exposeAngleAxis();
+ void exposeGeometryConversion();
+
} // namespace eigenpy
#endif // define __eigenpy_geometry_hpp__
--
GitLab