diff --git a/README.md b/README.md index a469633..94ffcbd 100644 --- a/README.md +++ b/README.md @@ -35,13 +35,18 @@ If you require any functionality that is not currently in the wrapper, feel free How to install -------------- +Install `cython` if you don't have already -First, setup `eigency` +``` +pip install cython +``` + +Then, setup `eigency` ``` git clone https://github.com/wouterboomsma/eigency.git cd eigency -python setup.py build_ext --inplace +python setup.py # put in your bashrc export PYTHONPATH="$PYTHONPATH:" ``` @@ -56,6 +61,8 @@ git submodule update python setup.py build_ext --inplace # put in your bashrc export PYTHONPATH="$PYTHONPATH:" +# alternately, install it to your default dist-packages +python setup.py install ``` Usage diff --git a/sophus.pyx b/sophus.pyx index 2faed62..045763b 100644 --- a/sophus.pyx +++ b/sophus.pyx @@ -41,6 +41,19 @@ cdef class SO3: def log(self): return ndarray(self.thisptr.log()) + + def data(self): + ptr = self.thisptr.data() + return numpy.array((ptr[0], ptr[1], ptr[2], ptr[3])) + + def setData(self, np.ndarray data): + data = data.reshape((4,)) + ptr = self.thisptr.data() + ptr[0] = data[0] + ptr[1] = data[1] + ptr[2] = data[2] + ptr[3] = data[3] + self.normalize() def inverse(self): res = SO3() @@ -139,7 +152,7 @@ cdef class SE3: Returns the SO3 part (rotation) """ return SO3(ndarray(self.thisptr.so3().matrix())) - + def log(self): return ndarray(self.thisptr.log()) @@ -157,6 +170,14 @@ cdef class SE3: def setRotationMatrix(self, np.ndarray matrix): self.thisptr.setRotationMatrix(Map[Matrix3d](matrix)) + def setQuaternion(self, np.ndarray quaternion): + quaternion = quaternion.reshape((4,)) + ptr = self.thisptr.so3().data() + ptr[0] = quaternion[0] + ptr[1] = quaternion[1] + ptr[2] = quaternion[2] + ptr[3] = quaternion[3] + def __mul__(SE3 x, SE3 y): """ Group multiplication operator diff --git a/sophus_defs.pxd b/sophus_defs.pxd index 540be98..8f00e22 100644 --- a/sophus_defs.pxd +++ b/sophus_defs.pxd @@ -1,8 +1,6 @@ # -*- coding: utf-8 -*- from eigency.core cimport * - - cdef extern from "" namespace "Sophus": cdef cppclass SO3[Scalar]: SO3() except + @@ -13,6 +11,7 @@ cdef extern from "" namespace "Sophus": Matrix3d& matrix() Vector3d log() + Scalar* data() SO3 inverse() void normalize()