Commit cf46bbfe authored by Nicolas Mansard's avatar Nicolas Mansard Committed by nmansard
Browse files

Added the geometry module.

parent 92f87dd4
......@@ -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}")
......
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)) );
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."
#include "eigenpy/angle-axis.hpp"
#include "eigenpy/geometry.hpp"
namespace eigenpy
{
void exposeAngleAxis()
{
AngleAxisVisitor<Eigen::AngleAxisd>::expose();
}
} // namespace eigenpy
#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__
......@@ -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);
}