From cf46bbfee2d855fb0a5b017987840864a8c83da2 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 24 Jul 2014 15:04:10 +0200
Subject: [PATCH] Added the geometry module.
---
CMakeLists.txt | 40 +++++-
python/test_unit.py | 42 +++---
python/tgeometry.py | 71 +++++++++++
src/angle-axis.cpp | 10 ++
src/angle-axis.hpp | 131 +++++++++++++++++++
src/eigenpy.hpp | 154 +++-------------------
src/exception.cpp | 26 ++++
src/exception.hpp | 54 ++++++++
src/geometry.hpp | 8 ++
src/map.hpp | 108 ++++++++++++++++
src/quaternion.cpp | 10 ++
src/quaternion.hpp | 163 ++++++++++++++++++++++++
src/simple.cpp | 24 ++++
src/simple.hpp | 51 ++++++++
unittest/geometry.cpp | 54 ++++++++
unittest/{libeigenpy.cpp => matrix.cpp} | 7 +-
unittest/timer.h | 42 ++++++
17 files changed, 833 insertions(+), 162 deletions(-)
create mode 100644 python/tgeometry.py
create mode 100644 src/angle-axis.cpp
create mode 100644 src/angle-axis.hpp
create mode 100644 src/exception.cpp
create mode 100644 src/exception.hpp
create mode 100644 src/geometry.hpp
create mode 100644 src/map.hpp
create mode 100644 src/quaternion.cpp
create mode 100644 src/quaternion.hpp
create mode 100644 src/simple.cpp
create mode 100644 src/simple.hpp
create mode 100644 unittest/geometry.cpp
rename unittest/{libeigenpy.cpp => matrix.cpp} (91%)
create mode 100644 unittest/timer.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f637077d..e05d205a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -36,8 +36,15 @@ INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
# ----------------------------------------------------
SET(${PROJECT_NAME}_HEADERS
src/eigenpy.hpp
+ src/exception.hpp
+ src/map.hpp
+ src/simple.hpp
+ src/geometry.hpp
+ src/angle-axis.hpp
+ src/quaternion.hpp
)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/eigenpy")
+INCLUDE_DIRECTORIES(${${PROJECT_NAME}_BINARY_DIR}/include/eigenpy)
FOREACH(header ${${PROJECT_NAME}_HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME)
@@ -52,17 +59,42 @@ FOREACH(header ${${PROJECT_NAME}_HEADERS})
ENDIF(WIN32)
INSTALL(FILES ${${PROJECT_NAME}_SOURCE_DIR}/${header}
DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ)
ENDFOREACH(header)
# ----------------------------------------------------
# --- TARGETS ----------------------------------------
# ----------------------------------------------------
-ADD_LIBRARY(eigenpy SHARED unittest/libeigenpy.cpp)
+
+ADD_LIBRARY(eigenpy SHARED
+ src/exception.cpp
+ src/simple.cpp
+ src/angle-axis.cpp
+ src/quaternion.cpp
+ )
TARGET_LINK_LIBRARIES(eigenpy ${Boost_LIBRARIES})
+INSTALL(TARGETS eigenpy DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
+
+
+# ----------------------------------------------------
+# --- UNIT TEST --------------------------------------
+# ----------------------------------------------------
+ADD_LIBRARY(matrix SHARED unittest/matrix.cpp)
+TARGET_LINK_LIBRARIES(matrix ${Boost_LIBRARIES} eigenpy)
+SET_TARGET_PROPERTIES(matrix PROPERTIES PREFIX "")
+
+ADD_LIBRARY(geometry SHARED unittest/geometry.cpp)
+TARGET_LINK_LIBRARIES(geometry ${Boost_LIBRARIES} eigenpy)
+SET_TARGET_PROPERTIES(geometry PROPERTIES PREFIX "")
+
+ADD_LIBRARY(se3 SHARED unittest/se3.cpp)
+TARGET_LINK_LIBRARIES(se3 ${Boost_LIBRARIES} eigenpy)
+SET_TARGET_PROPERTIES(se3 PROPERTIES PREFIX "")
+
+#ADD_LIBRARY(align SHARED unittest/libalign.cpp)
+#TARGET_LINK_LIBRARIES(align ${Boost_LIBRARIES} eigenpy)
-ADD_EXECUTABLE(se3 EXCLUDE_FROM_ALL unittest/se3.cpp)
-#TARGET_INCLUDE_DIRECTORIES(se3 unittest/)
+ADD_EXECUTABLE(tse3 EXCLUDE_FROM_ALL unittest/tse3.cpp)
PKG_CONFIG_APPEND_CFLAGS(${_Eigen_CFLAGS})
PKG_CONFIG_APPEND_CFLAGS("-I${PYTHON_INCLUDE_DIRS}")
diff --git a/python/test_unit.py b/python/test_unit.py
index 76a18361..bb7b523e 100644
--- a/python/test_unit.py
+++ b/python/test_unit.py
@@ -1,19 +1,19 @@
import numpy as np
-import libeigenpy
+import matrix as eigenpy
verbose = False
if verbose: print "===> From MatrixXd to Py"
-M = libeigenpy.naturals(3,3,verbose)
+M = eigenpy.naturals(3,3,verbose)
Mcheck = np.reshape(np.array(range(9),np.double),[3,3])
assert np.array_equal(Mcheck,M)
if verbose: print "===> From Matrix3d to Py"
-M33= libeigenpy.naturals33(verbose)
+M33= eigenpy.naturals33(verbose)
assert np.array_equal(Mcheck,M33)
if verbose: print "===> From VectorXd to Py"
-v = libeigenpy.naturalsX(3,verbose)
+v = eigenpy.naturalsX(3,verbose)
vcheck = np.array([range(3),],np.double).T
assert np.array_equal(vcheck ,v)
@@ -24,43 +24,43 @@ Mref = np.reshape(np.array(range(64),np.double),[8,8])
if verbose: print "===> Matrix 8x8"
M = Mref
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block 0:3x0:3"
M = Mref[0:3,0:3]
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block 1:3x1:3"
M = Mref[1:3,1:3]
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block 1:5:2x1:5:2"
M = Mref[1:5:2,1:5:2]
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block 1:8:3x1:5"
M = Mref[1:8:3,1:5]
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block transpose 1:8:3x1:6:2"
M = Mref[1:8:3,0:6:2].T
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block Vector 1x0:6:2"
M = Mref[1:2,0:6:2]
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block Vector 1x0:6:2 tanspose"
M = Mref[1:2,0:6:2].T
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block Vector 0:6:2x1"
M = Mref[0:6:2,1:2]
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> Block Vector 0:6:2x1 tanspose"
M = Mref[0:6:2,1:2].T
-assert( np.array_equal(M,libeigenpy.reflex(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex(M,verbose)) );
if verbose: print "===> From Py to Eigen::VectorXd"
if verbose: print "===> From Py to Eigen::VectorXd"
@@ -68,15 +68,15 @@ if verbose: print "===> From Py to Eigen::VectorXd"
if verbose: print "===> Block Vector 0:6:2x1 1 dim"
M = Mref[0:6:2,1].T
-assert( np.array_equal(np.array([M,]).T,libeigenpy.reflexV(M,verbose)) );
+assert( np.array_equal(np.array([M,]).T,eigenpy.reflexV(M,verbose)) );
if verbose: print "===> Block Vector 0:6:2x1"
M = Mref[0:6:2,1:2]
-assert( np.array_equal(M,libeigenpy.reflexV(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflexV(M,verbose)) );
if verbose: print "===> Block Vector 0:6:2x1 transpose"
M = Mref[0:6:2,1:2].T
-assert( np.array_equal(M.T,libeigenpy.reflexV(M,verbose)) );
+assert( np.array_equal(M.T,eigenpy.reflexV(M,verbose)) );
if verbose: print "===> From Py to Eigen::Matrix3d"
if verbose: print "===> From Py to Eigen::Matrix3d"
@@ -84,13 +84,13 @@ if verbose: print "===> From Py to Eigen::Matrix3d"
if verbose: print "===> Block Vector 0:3x0:6:2 "
M = Mref[0:3,0:6:2]
-assert( np.array_equal(M,libeigenpy.reflex33(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex33(M,verbose)) );
if verbose: print "===> Block Vector 0:3x0:6:2 T"
M = Mref[0:3,0:6].T
try:
- assert( np.array_equal(M,libeigenpy.reflex33(M,verbose)) );
-except libeigenpy.exception, e:
+ assert( np.array_equal(M,eigenpy.reflex33(M,verbose)) );
+except eigenpy.Exception, e:
if verbose: print "As expected, got the following /ROW/ error:", e.message
if verbose: print "===> From Py to Eigen::Vector3d"
@@ -98,6 +98,6 @@ if verbose: print "===> From Py to Eigen::Vector3d"
if verbose: print "===> From Py to Eigen::Vector3d"
M = Mref[0:3,1:2]
-assert( np.array_equal(M,libeigenpy.reflex3(M,verbose)) );
+assert( np.array_equal(M,eigenpy.reflex3(M,verbose)) );
diff --git a/python/tgeometry.py b/python/tgeometry.py
new file mode 100644
index 00000000..211d23e0
--- /dev/null
+++ b/python/tgeometry.py
@@ -0,0 +1,71 @@
+from geometry import *
+import numpy as np
+from numpy import cos,sin
+
+verbose = True
+
+def isapprox(a,b,epsilon=1e-6):
+ if a.__class__ == b.__class__ == np.ndarray:
+ return np.allclose(a,b,epsilon)
+ else:
+ return abs(a-b)<epsilon
+
+# --- Quaternion ---------------------------------------------------------------
+q = Quaternion(1,2,3,4)
+q.normalize()
+assert(isapprox(np.linalg.norm(q.coeffs()),q.norm()))
+assert(isapprox(np.linalg.norm(q.coeffs()),1))
+
+r = AngleAxis(q)
+q2 = Quaternion(r)
+assert(q==q2)
+assert(isapprox(q.coeffs(),q2.coeffs()))
+
+Rq = q.matrix()
+Rr = r.matrix()
+assert(isapprox(np.dot(Rq,Rq.T),np.eye(3)))
+assert(isapprox(Rr,Rq))
+
+qR = Quaternion(Rr)
+assert(q==qR)
+assert(isapprox(q.coeffs(),qR.coeffs()))
+
+assert(isapprox(qR[3],1./np.sqrt(30)))
+try:
+ qR[5]
+ print "Error, this message should not appear."
+except Exception,e:
+ if verbose: print "As expected, catched exception: ",e.message
+
+# --- Angle Vector ------------------------------------------------
+r = AngleAxis(.1,np.array([1,0,0],np.double))
+if verbose: print "Rx(.1) = \n\n",r.matrix(),"\n"
+assert( isapprox(r.matrix()[2,2],cos(r.angle)))
+assert( isapprox(r.axis,np.array([[1,0,0],],np.double).T) )
+assert( isapprox(r.angle,0.1) )
+
+r.axis = np.array([0,1,0],np.double)
+assert( isapprox(r.matrix()[0,0],cos(r.angle)))
+
+ri = r.inverse()
+assert( isapprox(ri.angle,-.1) )
+
+R = r.matrix()
+r2 = AngleAxis(np.dot(R,R))
+assert( isapprox(r2.angle,r.angle*2) )
+
+# --- USER FUNCTIONS -----------------------------------------------------------
+ro = testOutAngleAxis()
+assert(ro.__class__ == AngleAxis)
+res = testInAngleAxis(r)
+assert( res==r.angle )
+
+qo = testOutQuaternion()
+assert(qo.__class__ == Quaternion)
+res = testInQuaternion_fx(q)
+assert(q.norm() == res)
+try:
+ testInQuaternion(q)
+ print "Error, this message should not appear."
+except:
+ if verbose: print "As expected, catch a Boost::python::ArgError exception."
diff --git a/src/angle-axis.cpp b/src/angle-axis.cpp
new file mode 100644
index 00000000..988f1651
--- /dev/null
+++ b/src/angle-axis.cpp
@@ -0,0 +1,10 @@
+#include "eigenpy/angle-axis.hpp"
+#include "eigenpy/geometry.hpp"
+
+namespace eigenpy
+{
+ void exposeAngleAxis()
+ {
+ AngleAxisVisitor<Eigen::AngleAxisd>::expose();
+ }
+} // namespace eigenpy
diff --git a/src/angle-axis.hpp b/src/angle-axis.hpp
new file mode 100644
index 00000000..7cb2a5d5
--- /dev/null
+++ b/src/angle-axis.hpp
@@ -0,0 +1,131 @@
+#ifndef __eigenpy_Angle_Axis_hpp__
+#define __eigenpy_Angle_Axis_hpp__
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <boost/python.hpp>
+#include "eigenpy/simple.hpp"
+
+namespace eigenpy
+{
+ template<>
+ struct UnalignedEquivalent<Eigen::AngleAxisd>
+ {
+ typedef Eigen::AngleAxis<double> type;
+ };
+
+ namespace bp = boost::python;
+
+ template<typename D>
+ class AngleAxisVisitor
+ : public boost::python::def_visitor< AngleAxisVisitor<D> >
+ {
+ typedef D AngleAxis;
+ typedef typename eigenpy::UnalignedEquivalent<D>::type AngleAxisUnaligned;
+
+ typedef typename AngleAxisUnaligned::Scalar Scalar;
+ typedef typename eigenpy::UnalignedEquivalent<typename AngleAxisUnaligned::Vector3>::type Vector3;
+ typedef typename eigenpy::UnalignedEquivalent<typename AngleAxisUnaligned::Matrix3>::type Matrix3;
+
+ typedef eigenpy::UnalignedEquivalent<Eigen::Quaternion<Scalar> > Quaternion;
+
+ public:
+
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def(bp::init<Scalar,Vector3>
+ ((bp::arg("angle"),bp::arg("axis")),
+ "Initialize from angle and axis"))
+ .def(bp::init<Matrix3>
+ ((bp::arg("rotationMatrix")),
+ "Initialize from a rotation matrix"))
+ .def("__init__",bp::make_constructor(&AngleAxisVisitor::constructFromQuaternion,
+ bp::default_call_policies(),
+ (bp::arg("quaternion"))),"Initialize from quaternion")
+ .def(bp::init<AngleAxisUnaligned>((bp::arg("clone"))))
+
+ .def("matrix",&AngleAxisUnaligned::toRotationMatrix,"Return the corresponding rotation matrix 3x3.")
+ .def("vector",&AngleAxisVisitor::toVector3,"Return the correspond angle*axis vector3")
+ .add_property("axis",&AngleAxisVisitor::getAxis,&AngleAxisVisitor::setAxis)
+ .add_property("angle",&AngleAxisVisitor::getAngle,&AngleAxisVisitor::setAngle)
+
+ /* --- Methods --- */
+ .def("normalize",&AngleAxisVisitor::normalize,"Normalize the axis vector (without changing the angle).")
+ .def("inverse",&AngleAxisUnaligned::inverse,"Return the inverse rotation.")
+ .def("apply",&AngleAxisVisitor::apply,(bp::arg("vector3")),"Apply the rotation map to the vector")
+
+ /* --- Operators --- */
+ .def(bp::self * bp::other<Vector3>())
+ .def("__eq__",&AngleAxisVisitor::__eq__)
+ .def("__ne__",&AngleAxisVisitor::__ne__)
+ .def("__abs__",&AngleAxisVisitor::getAngleAbs)
+ ;
+ }
+ private:
+
+ static AngleAxisUnaligned* constructFromQuaternion(const Eigen::Quaternion<Scalar,Eigen::DontAlign> & qu)
+ {
+ Eigen::Quaternion<Scalar> q = qu;
+ return new AngleAxisUnaligned(q);
+ }
+
+ static Vector3 apply(const AngleAxisUnaligned & r, const Vector3 & v ) { return r*v; }
+
+ static Vector3 getAxis(const AngleAxisUnaligned& self) { return self.axis(); }
+ static void setAxis(AngleAxisUnaligned& self, const Vector3 & r)
+ {
+ self = AngleAxisUnaligned( self.angle(),r );
+ }
+
+ static double getAngle(const AngleAxisUnaligned& self) { return self.angle(); }
+ static void setAngle( AngleAxisUnaligned& self, const double & th)
+ {
+ self = AngleAxisUnaligned( th,self.axis() );
+ }
+ static double getAngleAbs(const AngleAxisUnaligned& self) { return std::abs(self.angle()); }
+
+ static bool __eq__(const AngleAxisUnaligned & u, const AngleAxisUnaligned & v)
+ {
+ return u.isApprox(v);
+ }
+ static bool __ne__(const AngleAxisUnaligned & u, const AngleAxisUnaligned & v)
+ {
+ return !__eq__(u,v);
+ }
+
+ static Vector3 toVector3( const AngleAxisUnaligned & self ) { return self.axis()*self.angle(); }
+ static void normalize( AngleAxisUnaligned & self )
+ {
+ setAxis(self,self.axis() / self.axis().norm());
+ }
+
+ private:
+
+ static PyObject* convert(AngleAxis const& q)
+ {
+ AngleAxisUnaligned qx = q;
+ return boost::python::incref(boost::python::object(qx).ptr());
+ }
+
+ public:
+
+ static void expose()
+ {
+ bp::class_<AngleAxisUnaligned>("AngleAxis",
+ "AngleAxis representation of rotations.\n\n",
+ bp::init<>())
+ .def(AngleAxisVisitor<D>());
+
+ // TODO: check the problem of fix-size Angle Axis.
+ //bp::to_python_converter< AngleAxis,AngleAxisVisitor<D> >();
+
+ }
+
+ };
+
+} // namespace eigenpy
+
+
+#endif //ifndef __eigenpy_Angle_Axis_hpp__
diff --git a/src/eigenpy.hpp b/src/eigenpy.hpp
index 7c90b994..4c322e3a 100644
--- a/src/eigenpy.hpp
+++ b/src/eigenpy.hpp
@@ -18,60 +18,17 @@
#include <boost/python.hpp>
#include <numpy/arrayobject.h>
#include <iostream>
+#include <eigenpy/exception.hpp>
+#include <eigenpy/simple.hpp>
+#include <eigenpy/map.hpp>
namespace eigenpy
{
- template< typename MatType, int IsVector>
- struct MapNumpyTraits {};
-
- /* Wrap a numpy::array with an Eigen::Map. No memory copy. */
- template< typename MatType >
- struct MapNumpy
- {
- typedef MapNumpyTraits<MatType, MatType::IsVectorAtCompileTime> Impl;
- typedef typename Impl::EigenMap EigenMap;
-
- static inline EigenMap map( PyArrayObject* pyArray );
- };
-
- /* Eigenpy exception. They can be catch with python (equivalent eigenpy.exception class). */
- class exception : public std::exception
- {
- public:
- exception(std::string msg) : message(msg) {}
- const char *what() const throw()
- {
- return this->message.c_str();
- }
- ~exception() throw() {}
- std::string getMessage() { return message; }
- static void registerException();
-
- private:
- static void translateException( exception const & e );
- static PyObject * pyType;
- std::string message;
- };
/* Enable the Eigen--Numpy serialization for the templated MatrixBase class. */
template<typename MatType,typename EigenEquivalentType>
void enableEigenPySpecific();
- /* Enable Eigen-Numpy serialization for a set of standard MatrixBase instance. */
- void enableEigenPy()
- {
- exception::registerException();
-
- enableEigenPySpecific<Eigen::MatrixXd,Eigen::MatrixXd>();
- enableEigenPySpecific<Eigen::Matrix2d,Eigen::Matrix2d>();
- enableEigenPySpecific<Eigen::Matrix3d,Eigen::Matrix3d>();
- enableEigenPySpecific<Eigen::Matrix4d,Eigen::Matrix4d>();
-
- enableEigenPySpecific<Eigen::VectorXd,Eigen::VectorXd>();
- enableEigenPySpecific<Eigen::Vector2d,Eigen::Vector2d>();
- enableEigenPySpecific<Eigen::Vector3d,Eigen::Vector3d>();
- enableEigenPySpecific<Eigen::Vector4d,Eigen::Vector4d>();
- }
}
/* --- DETAILS ------------------------------------------------------------------ */
@@ -85,69 +42,6 @@ namespace eigenpy
template <> struct NumpyEquivalentType<int> { enum { type_code = NPY_INT };};
template <> struct NumpyEquivalentType<float> { enum { type_code = NPY_FLOAT };};
- /* --- MAP ON NUMPY ----------------------------------------------------------- */
- template<typename MatType>
- struct MapNumpyTraits<MatType,0>
- {
- typedef Eigen::Stride<Eigen::Dynamic,Eigen::Dynamic> Stride;
- typedef Eigen::Map<MatType,0,Stride> EigenMap;
- typedef typename MatType::Scalar T;
-
- static EigenMap mapImpl( PyArrayObject* pyArray )
- {
- assert( PyArray_NDIM(pyArray) == 2 );
-
- const int R = PyArray_DIMS(pyArray)[0];
- const int C = PyArray_DIMS(pyArray)[1];
- const int itemsize = PyArray_ITEMSIZE(pyArray);
- const int stride1 = PyArray_STRIDE(pyArray, 0) / itemsize;
- const int stride2 = PyArray_STRIDE(pyArray, 1) / itemsize;
-
- if( (MatType::RowsAtCompileTime!=R)
- && (MatType::RowsAtCompileTime!=Eigen::Dynamic) )
- { throw eigenpy::exception("The number of rows does not fit with the matrix type."); }
- if( (MatType::ColsAtCompileTime!=C)
- && (MatType::ColsAtCompileTime!=Eigen::Dynamic) )
- { throw eigenpy::exception("The number of columns does not fit with the matrix type."); }
-
- T* pyData = reinterpret_cast<T*>(PyArray_DATA(pyArray));
- return EigenMap( pyData, R,C, Stride(stride2,stride1) );
- }
- };
-
- template<typename MatType>
- struct MapNumpyTraits<MatType,1>
- {
- typedef Eigen::InnerStride<Eigen::Dynamic> Stride;
- typedef Eigen::Map<MatType,0,Stride> EigenMap;
- typedef typename MatType::Scalar T;
-
- static EigenMap mapImpl( PyArrayObject* pyArray )
- {
- assert( PyArray_NDIM(pyArray) <= 2 );
-
- int rowMajor;
- if( PyArray_NDIM(pyArray)==1 ) rowMajor = 0;
- else rowMajor = (PyArray_DIMS(pyArray)[0]>PyArray_DIMS(pyArray)[1])?0:1;
-
- const int R = PyArray_DIMS(pyArray)[rowMajor];
- const int itemsize = PyArray_ITEMSIZE(pyArray);
- const int stride = PyArray_STRIDE(pyArray, rowMajor) / itemsize;;
-
- if( (MatType::MaxSizeAtCompileTime!=R)
- && (MatType::MaxSizeAtCompileTime!=Eigen::Dynamic) )
- { throw eigenpy::exception("The number of elements does not fit with the vector type."); }
-
- T* pyData = reinterpret_cast<T*>(PyArray_DATA(pyArray));
- return EigenMap( pyData, R, 1, Stride(stride) );
- }
- };
-
- template< typename MatType >
- typename MapNumpy<MatType>::EigenMap MapNumpy<MatType>::map( PyArrayObject* pyArray )
- {
- return Impl::mapImpl(pyArray);
- }
/* --- TO PYTHON -------------------------------------------------------------- */
template< typename MatType,typename EquivalentEigenType >
@@ -174,7 +68,7 @@ namespace eigenpy
template<typename MatType, int ROWS,int COLS>
struct TraitsMatrixConstructor
{
- static MatType & construct(void*storage,int r,int c)
+ static MatType & construct(void*storage,int /*r*/,int /*c*/)
{
return * new(storage) MatType();
}
@@ -192,7 +86,7 @@ namespace eigenpy
template<typename MatType,int R>
struct TraitsMatrixConstructor<MatType,R,Eigen::Dynamic>
{
- static MatType & construct(void*storage,int r,int c)
+ static MatType & construct(void*storage,int /*r*/,int c)
{
return * new(storage) MatType(c);
}
@@ -201,7 +95,7 @@ namespace eigenpy
template<typename MatType,int C>
struct TraitsMatrixConstructor<MatType,Eigen::Dynamic,C>
{
- static MatType & construct(void*storage,int r,int c)
+ static MatType & construct(void*storage,int r,int /*c*/)
{
return * new(storage) MatType(r);
}
@@ -235,7 +129,7 @@ namespace eigenpy
return 0;
}
- if (PyArray_ObjectType(obj_ptr, 0) != NumpyEquivalentType<T>::type_code)
+ if ((PyArray_ObjectType(obj_ptr, 0)) != NumpyEquivalentType<T>::type_code)
{
std::cerr << "The internal type as no Eigen equivalent." << std::endl;
return 0;
@@ -275,30 +169,22 @@ namespace eigenpy
void enableEigenPySpecific()
{
import_array();
- boost::python::to_python_converter<MatType,
- eigenpy::EigenToPy<MatType,EigenEquivalentType> >();
- eigenpy::EigenFromPy<MatType,EigenEquivalentType>();
- }
- /* --- EXCEPTION ----------------------------------------------------------------- */
- PyObject * exception::pyType;
+ #ifdef EIGEN_DONT_VECTORIZE
+ boost::python::to_python_converter<MatType,
+ eigenpy::EigenToPy<MatType,MatType> >();
+ eigenpy::EigenFromPy<MatType,MatType>();
+ #else
+ typedef typename eigenpy::UnalignedEquivalent<MatType>::type MatTypeDontAlign;
+
+ boost::python::to_python_converter<MatType,
+ eigenpy::EigenToPy<MatType,MatType> >();
+ boost::python::to_python_converter<MatTypeDontAlign,
+ eigenpy::EigenToPy<MatTypeDontAlign,MatTypeDontAlign> >();
+ eigenpy::EigenFromPy<MatTypeDontAlign,MatTypeDontAlign>();
+#endif
- void exception::translateException( exception const & e )
- {
- assert(NULL!=pyType);
- // Return an exception object of type pyType and value object(e).
- PyErr_SetObject(pyType,boost::python::object(e).ptr());
}
- void exception::registerException()
- {
- pyType = boost::python::class_<eigenpy::exception>
- ("exception",boost::python::init<std::string>())
- .add_property("message", &eigenpy::exception::getMessage)
- .ptr();
-
- boost::python::register_exception_translator<eigenpy::exception>
- (&eigenpy::exception::translateException);
- }
} // namespace eigenpy
diff --git a/src/exception.cpp b/src/exception.cpp
new file mode 100644
index 00000000..3c20f3a3
--- /dev/null
+++ b/src/exception.cpp
@@ -0,0 +1,26 @@
+#include <eigenpy/exception.hpp>
+
+
+namespace eigenpy
+{
+ PyObject * Exception::pyType;
+
+ void Exception::translateException( Exception const & e )
+ {
+ assert(NULL!=pyType);
+ // Return an exception object of type pyType and value object(e).
+ PyErr_SetObject(Exception::pyType,boost::python::object(e).ptr());
+ }
+
+ void Exception::registerException()
+ {
+ pyType = boost::python::class_<eigenpy::Exception>
+ ("Exception",boost::python::init<std::string>())
+ .add_property("message", &eigenpy::Exception::copyMessage)
+ .ptr();
+
+ boost::python::register_exception_translator<eigenpy::Exception>
+ (&eigenpy::Exception::translateException);
+ }
+
+} // namespace eigenpy
diff --git a/src/exception.hpp b/src/exception.hpp
new file mode 100644
index 00000000..db676f51
--- /dev/null
+++ b/src/exception.hpp
@@ -0,0 +1,54 @@
+/*
+ * Copyright 2014, Nicolas Mansard, 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 <exception>
+#include <string>
+#include <boost/python.hpp>
+
+#ifndef __eigenpy_Exception_hpp__
+#define __eigenpy_Exception_hpp__
+
+namespace eigenpy
+{
+ /*
+ * Eigenpy exception. They can be catch with python (equivalent eigenpy.exception class).
+ */
+ class Exception : public std::exception
+ {
+ public:
+ Exception() : message() {}
+ Exception(std::string msg) : message(msg) {}
+ const char *what() const throw()
+ {
+ return this->getMessage().c_str();
+ }
+ ~Exception() throw() {}
+ virtual const std::string & getMessage() const { return message; }
+ std::string copyMessage() const { return getMessage(); }
+
+ /* Call this static function to "enable" the translation of this C++ exception in Python. */
+ static void registerException();
+
+ private:
+ static void translateException( Exception const & e );
+ static PyObject * pyType;
+ protected:
+ std::string message;
+ };
+
+} // namespace eigenpy
+
+#endif // ifndef __eigenpy_Exception_hpp__
diff --git a/src/geometry.hpp b/src/geometry.hpp
new file mode 100644
index 00000000..b2b88761
--- /dev/null
+++ b/src/geometry.hpp
@@ -0,0 +1,8 @@
+namespace eigenpy
+{
+ typedef Eigen::Quaternion<double,Eigen::DontAlign> Quaterniond_fx;
+ //typedef Eigen::AngleAxis<double> AngleAxis_fx;
+
+ void exposeQuaternion();
+ void exposeAngleAxis();
+} // namespace eigenpy
diff --git a/src/map.hpp b/src/map.hpp
new file mode 100644
index 00000000..fd9bd8a8
--- /dev/null
+++ b/src/map.hpp
@@ -0,0 +1,108 @@
+/*
+ * Copyright 2014, Nicolas Mansard, 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 <Eigen/Core>
+#include <boost/python.hpp>
+#include <numpy/arrayobject.h>
+#include <eigenpy/exception.hpp>
+
+namespace eigenpy
+{
+ template< typename MatType, int IsVector>
+ struct MapNumpyTraits {};
+
+ /* Wrap a numpy::array with an Eigen::Map. No memory copy. */
+ template< typename MatType >
+ struct MapNumpy
+ {
+ typedef MapNumpyTraits<MatType, MatType::IsVectorAtCompileTime> Impl;
+ typedef typename Impl::EigenMap EigenMap;
+
+ static inline EigenMap map( PyArrayObject* pyArray );
+ };
+
+} // namespace eigenpy
+
+/* --- DETAILS ------------------------------------------------------------------ */
+/* --- DETAILS ------------------------------------------------------------------ */
+/* --- DETAILS ------------------------------------------------------------------ */
+
+namespace eigenpy
+{
+ template<typename MatType>
+ struct MapNumpyTraits<MatType,0>
+ {
+ typedef Eigen::Stride<Eigen::Dynamic,Eigen::Dynamic> Stride;
+ typedef Eigen::Map<MatType,0,Stride> EigenMap;
+ typedef typename MatType::Scalar T;
+
+ static EigenMap mapImpl( PyArrayObject* pyArray )
+ {
+ assert( PyArray_NDIM(pyArray) == 2 );
+
+ const int R = PyArray_DIMS(pyArray)[0];
+ const int C = PyArray_DIMS(pyArray)[1];
+ const int itemsize = PyArray_ITEMSIZE(pyArray);
+ const int stride1 = PyArray_STRIDE(pyArray, 0) / itemsize;
+ const int stride2 = PyArray_STRIDE(pyArray, 1) / itemsize;
+
+ if( (MatType::RowsAtCompileTime!=R)
+ && (MatType::RowsAtCompileTime!=Eigen::Dynamic) )
+ { throw eigenpy::Exception("The number of rows does not fit with the matrix type."); }
+ if( (MatType::ColsAtCompileTime!=C)
+ && (MatType::ColsAtCompileTime!=Eigen::Dynamic) )
+ { throw eigenpy::Exception("The number of columns does not fit with the matrix type."); }
+
+ T* pyData = reinterpret_cast<T*>(PyArray_DATA(pyArray));
+ return EigenMap( pyData, R,C, Stride(stride2,stride1) );
+ }
+ };
+
+ template<typename MatType>
+ struct MapNumpyTraits<MatType,1>
+ {
+ typedef Eigen::InnerStride<Eigen::Dynamic> Stride;
+ typedef Eigen::Map<MatType,0,Stride> EigenMap;
+ typedef typename MatType::Scalar T;
+
+ static EigenMap mapImpl( PyArrayObject* pyArray )
+ {
+ assert( PyArray_NDIM(pyArray) <= 2 );
+
+ int rowMajor;
+ if( PyArray_NDIM(pyArray)==1 ) rowMajor = 0;
+ else rowMajor = (PyArray_DIMS(pyArray)[0]>PyArray_DIMS(pyArray)[1])?0:1;
+
+ const int R = PyArray_DIMS(pyArray)[rowMajor];
+ const int itemsize = PyArray_ITEMSIZE(pyArray);
+ const int stride = PyArray_STRIDE(pyArray, rowMajor) / itemsize;;
+
+ if( (MatType::MaxSizeAtCompileTime!=R)
+ && (MatType::MaxSizeAtCompileTime!=Eigen::Dynamic) )
+ { throw eigenpy::Exception("The number of elements does not fit with the vector type."); }
+
+ T* pyData = reinterpret_cast<T*>(PyArray_DATA(pyArray));
+ return EigenMap( pyData, R, 1, Stride(stride) );
+ }
+ };
+
+ template< typename MatType >
+ typename MapNumpy<MatType>::EigenMap MapNumpy<MatType>::map( PyArrayObject* pyArray )
+ {
+ return Impl::mapImpl(pyArray);
+ }
+
+} // namespace eigenpy
diff --git a/src/quaternion.cpp b/src/quaternion.cpp
new file mode 100644
index 00000000..90b5922e
--- /dev/null
+++ b/src/quaternion.cpp
@@ -0,0 +1,10 @@
+#include "eigenpy/quaternion.hpp"
+#include "eigenpy/geometry.hpp"
+
+namespace eigenpy
+{
+ void exposeQuaternion()
+ {
+ QuaternionVisitor<Eigen::Quaterniond>::expose();
+ }
+} // namespace eigenpy
diff --git a/src/quaternion.hpp b/src/quaternion.hpp
new file mode 100644
index 00000000..95e8bcb7
--- /dev/null
+++ b/src/quaternion.hpp
@@ -0,0 +1,163 @@
+#ifndef __eigenpy_Quaternion_hpp__
+#define __eigenpy_Quaternion_hpp__
+
+#include "eigenpy/exception.hpp"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include "eigenpy/simple.hpp"
+
+namespace eigenpy
+{
+
+ class ExceptionIndex : public Exception
+ {
+ public:
+ int index;
+ ExceptionIndex(int index,int imin,int imax) : Exception("")
+ {
+ std::ostringstream oss; oss << "Index " << index << " out of range " << imin << ".."<< imax <<".";
+ message = oss.str();
+ }
+ };
+
+ template<>
+ struct UnalignedEquivalent<Eigen::Quaterniond>
+ {
+ typedef Eigen::Quaternion<double,Eigen::DontAlign> type;
+ };
+
+ namespace bp = boost::python;
+
+ template<typename Quaternion>
+ class QuaternionVisitor
+ : public boost::python::def_visitor< QuaternionVisitor<Quaternion> >
+ {
+ typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;
+ typedef typename eigenpy::UnalignedEquivalent<Quaternion>::type QuaternionUnaligned;
+
+ typedef typename QuaternionUnaligned::Scalar Scalar;
+ typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vector3;
+ typedef typename QuaternionUnaligned::Coefficients Coefficients;
+ typedef Eigen::Matrix<Scalar,3,3,Eigen::DontAlign> Matrix3;
+
+ typedef Eigen::AngleAxis<Scalar> AngleAxisUnaligned;
+
+ public:
+
+ static PyObject* convert(Quaternion const& q)
+ {
+ QuaternionUnaligned qx = q;
+ return boost::python::incref(boost::python::object(qx).ptr());
+ }
+
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def(bp::init<Matrix3>((bp::arg("matrixRotation")),"Initialize from rotation matrix."))
+ .def(bp::init<AngleAxisUnaligned>((bp::arg("angleaxis")),"Initialize from angle axis."))
+ .def(bp::init<QuaternionUnaligned>((bp::arg("clone")),"Copy constructor."))
+ .def("__init__",bp::make_constructor(&QuaternionVisitor::fromTwoVectors,
+ bp::default_call_policies(),
+ (bp::arg("u"),bp::arg("v"))),"Initialize from two vector u,v")
+ .def(bp::init<Scalar,Scalar,Scalar,Scalar>
+ ((bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
+ "Initialize from coefficients.\n\n"
+ "... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
+ "The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
+
+ /* --- Methods --- */
+ .def("coeffs",&QuaternionVisitor::coeffs)
+ .def("matrix",&QuaternionUnaligned::toRotationMatrix)
+
+ .def("setFromTwoVectors",&QuaternionVisitor::setFromTwoVectors,((bp::arg("u"),bp::arg("v"))))
+ .def("conjugate",&QuaternionUnaligned::conjugate)
+ .def("inverse",&QuaternionUnaligned::inverse)
+ .def("norm",&QuaternionUnaligned::norm)
+ .def("normalize",&QuaternionUnaligned::normalize)
+ .def("normalized",&QuaternionUnaligned::normalized)
+ .def("apply",&QuaternionUnaligned::_transformVector)
+
+ /* --- Operators --- */
+ .def(bp::self * bp::self)
+ .def(bp::self *= bp::self)
+ .def(bp::self * bp::other<Vector3>())
+ .def("__eq__",&QuaternionVisitor::__eq__)
+ .def("__ne__",&QuaternionVisitor::__ne__)
+ .def("__abs__",&QuaternionUnaligned::norm)
+ .def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
+ .def("__setitem__",&QuaternionVisitor::__setitem__)
+ .def("__getitem__",&QuaternionVisitor::__getitem__)
+ ;
+ }
+ private:
+
+ static QuaternionUnaligned* fromTwoVectors(const Vector3& u, const Vector3& v)
+ {
+ QuaternionUnaligned* q(new QuaternionUnaligned); q->setFromTwoVectors(u,v);
+ return q;
+ }
+
+ static Coefficients coeffs(const QuaternionUnaligned& self)
+ {
+ return self.coeffs();
+ }
+
+ static void setFromTwoVectors(QuaternionUnaligned& self, const Vector3& u, const Vector3& v)
+ {
+ self.setFromTwoVectors(u,v);
+ }
+
+ static bool __eq__(const QuaternionUnaligned& u, const QuaternionUnaligned& v)
+ {
+ return u.isApprox(v,1e-9);
+ }
+ static bool __ne__(const QuaternionUnaligned& u, const QuaternionUnaligned& v)
+ {
+ return !__eq__(u,v);
+ }
+
+ static double __getitem__(const QuaternionUnaligned & self, int idx)
+ {
+ if((idx<0) || (idx>4)) throw eigenpy::ExceptionIndex(idx,0,4);
+ if(idx==0) return self.x();
+ else if(idx==1) return self.y();
+ else if(idx==2) return self.z();
+ else return self.w();
+ }
+
+ static void __setitem__(QuaternionUnaligned& self, int idx, double value)
+ {
+ if((idx<0) || (idx>4)) throw eigenpy::ExceptionIndex(idx,0,4);
+ if(idx==0) self.x()=value;
+ else if(idx==1) self.y()=value;
+ else if(idx==2) self.z()=value;
+ else self.w()=value;
+ }
+
+ static int __len__() { return 4; }
+
+ public:
+
+ static void expose()
+ {
+ bp::class_<QuaternionUnaligned>("Quaternion",
+ "Quaternion representing rotation.\n\n"
+ "Supported operations "
+ "('q is a Quaternion, 'v' is a Vector3): "
+ "'q*q' (rotation composition), "
+ "'q*=q', "
+ "'q*v' (rotating 'v' by 'q'), "
+ "'q==q', 'q!=q', 'q[0..3]'.",
+ bp::init<>())
+ .def(QuaternionVisitor<Quaternion>())
+ ;
+
+ bp::to_python_converter< Quaternion,QuaternionVisitor<Quaternion> >();
+ }
+
+ };
+
+} // namespace eigenpy
+
+#endif // ifndef __eigenpy_Quaternion_hpp__
diff --git a/src/simple.cpp b/src/simple.cpp
new file mode 100644
index 00000000..01aaf10b
--- /dev/null
+++ b/src/simple.cpp
@@ -0,0 +1,24 @@
+
+#include <eigenpy/eigenpy.hpp>
+#include <eigenpy/simple.hpp>
+
+namespace eigenpy
+{
+
+ /* Enable Eigen-Numpy serialization for a set of standard MatrixBase instance. */
+ void enableEigenPy()
+ {
+ Exception::registerException();
+
+ enableEigenPySpecific<Eigen::MatrixXd,Eigen::MatrixXd>();
+ enableEigenPySpecific<Eigen::Matrix2d,Eigen::Matrix2d>();
+ enableEigenPySpecific<Eigen::Matrix3d,Eigen::Matrix3d>();
+ enableEigenPySpecific<Eigen::Matrix4d,Eigen::Matrix4d>();
+
+ enableEigenPySpecific<Eigen::VectorXd,Eigen::VectorXd>();
+ enableEigenPySpecific<Eigen::Vector2d,Eigen::Vector2d>();
+ enableEigenPySpecific<Eigen::Vector3d,Eigen::Vector3d>();
+ enableEigenPySpecific<Eigen::Vector4d,Eigen::Vector4d>();
+ }
+
+} // namespace eigenpy
diff --git a/src/simple.hpp b/src/simple.hpp
new file mode 100644
index 00000000..62415671
--- /dev/null
+++ b/src/simple.hpp
@@ -0,0 +1,51 @@
+/*
+ * Copyright 2014, Nicolas Mansard, 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_Simple_hpp__
+#define __eigenpy_Simple_hpp__
+
+#include <Eigen/Core>
+#include <boost/python.hpp>
+
+namespace eigenpy
+{
+
+ /* Enable Eigen-Numpy serialization for a set of standard MatrixBase instance. */
+ void enableEigenPy();
+
+
+ template<typename D>
+ struct UnalignedEquivalent
+ {
+ typedef Eigen::MatrixBase<D> MatType;
+ typedef Eigen::Matrix<typename D::Scalar,
+ D::RowsAtCompileTime,
+ D::ColsAtCompileTime,
+ D::Options | Eigen::DontAlign,
+ D::MaxRowsAtCompileTime,
+ D::MaxColsAtCompileTime> type;
+ };
+
+ typedef UnalignedEquivalent<Eigen::MatrixXd> MatrixXd_fx;
+ typedef UnalignedEquivalent<Eigen::Matrix3d> Matrix3d_fx;
+ typedef UnalignedEquivalent<Eigen::Matrix4d> Matrix4d_fx;
+ typedef UnalignedEquivalent<Eigen::VectorXd> VectorXd_fx;
+ typedef UnalignedEquivalent<Eigen::Vector3d> Vector3d_fx;
+ typedef UnalignedEquivalent<Eigen::Vector4d> Vector4d_fx;
+
+} // namespace eigenpy
+
+#endif // ifndef __eigenpy_Simple_hpp__
diff --git a/unittest/geometry.cpp b/unittest/geometry.cpp
new file mode 100644
index 00000000..65dd0982
--- /dev/null
+++ b/unittest/geometry.cpp
@@ -0,0 +1,54 @@
+//#define EIGEN_DONT_VECTORIZE
+//#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+
+#include "eigenpy/simple.hpp"
+#include "eigenpy/geometry.hpp"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <iostream>
+
+#include <boost/python.hpp>
+namespace bp = boost::python;
+
+Eigen::AngleAxisd testOutAngleAxis()
+{
+ return Eigen::AngleAxisd(.1,Eigen::Vector3d::UnitZ());
+}
+
+double testInAngleAxis(Eigen::AngleAxisd aa)
+{
+ return aa.angle();
+}
+
+Eigen::Quaterniond testOutQuaternion()
+{
+ Eigen::Quaterniond res(1,2,3,4);
+ return res;
+}
+double testInQuaternion( Eigen::Quaterniond q )
+{
+ return q.norm();
+}
+double testInQuaternion_fx( eigenpy::Quaterniond_fx q )
+{
+ return q.norm();
+}
+
+
+
+BOOST_PYTHON_MODULE(geometry)
+{
+ eigenpy::enableEigenPy();
+
+ eigenpy::exposeAngleAxis();
+ eigenpy::exposeQuaternion();
+
+ bp::def("testOutAngleAxis",&testOutAngleAxis);
+ bp::def("testInAngleAxis",&testInAngleAxis);
+
+ bp::def("testOutQuaternion",&testOutQuaternion);
+ bp::def("testInQuaternion",&testInQuaternion);
+ bp::def("testInQuaternion_fx",&testInQuaternion_fx);
+
+}
+
diff --git a/unittest/libeigenpy.cpp b/unittest/matrix.cpp
similarity index 91%
rename from unittest/libeigenpy.cpp
rename to unittest/matrix.cpp
index 70d5016c..908b7dde 100644
--- a/unittest/libeigenpy.cpp
+++ b/unittest/matrix.cpp
@@ -14,7 +14,8 @@
* with eigenpy. If not, see <http://www.gnu.org/licenses/>.
*/
-#include "eigenpy/eigenpy.hpp"
+#include "eigenpy/simple.hpp"
+#include <iostream>
Eigen::MatrixXd naturals(int R,int C,bool verbose)
{
@@ -52,14 +53,14 @@ Eigen::Matrix3d naturals(bool verbose)
}
template<typename MatType>
-Eigen::MatrixXd reflex(const MatType & M, bool verbose)
+Eigen::MatrixXd reflex(const typename eigenpy::UnalignedEquivalent<MatType>::type & M, bool verbose)
{
if(verbose)
std::cout << "EigenMat = " << M << std::endl;
return Eigen::MatrixXd(M);
}
-BOOST_PYTHON_MODULE(libeigenpy)
+BOOST_PYTHON_MODULE(matrix)
{
namespace bp = boost::python;
eigenpy::enableEigenPy();
diff --git a/unittest/timer.h b/unittest/timer.h
new file mode 100644
index 00000000..9b741e2c
--- /dev/null
+++ b/unittest/timer.h
@@ -0,0 +1,42 @@
+#include <sys/time.h>
+#include <iostream>
+#include <stack>
+
+#define SMOOTH(s) for(int _smooth=0;_smooth<s;++_smooth)
+
+/* Return the time spent in secs. */
+inline double operator- ( const struct timeval & t1,const struct timeval & t0)
+{
+ return (t1.tv_sec - t0.tv_sec)+1e-6*(t1.tv_usec - t0.tv_usec);
+}
+
+struct StackTicToc
+{
+ enum Unit { S = 1, MS = 1000, US = 1000000 };
+ Unit DEFAULT_UNIT;
+ static std::string unitName(Unit u)
+ { switch(u) { case S: return "s"; case MS: return "ms"; case US: return "us"; } return ""; }
+
+ std::stack<struct timeval> stack;
+ mutable struct timeval t0;
+
+StackTicToc( Unit def = MS ) : DEFAULT_UNIT(def) {}
+
+ inline void tic() {
+ stack.push(t0);
+ gettimeofday(&(stack.top()),NULL);
+ }
+
+ inline double toc(const Unit factor)
+ {
+ gettimeofday(&t0,NULL);
+ double dt = (t0-stack.top())*factor;
+ stack.pop();
+ return dt;
+ }
+ inline void toc( std::ostream& os, double SMOOTH=1 )
+ {
+ os << toc(DEFAULT_UNIT)/SMOOTH << " " << unitName(DEFAULT_UNIT) << std::endl;
+ }
+};
+
--
GitLab