expose the raw data (quaternion) of SO3

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:<path to eigency>"
 ```
@@ -56,6 +61,8 @@ git submodule update
 python setup.py build_ext --inplace
 # put in your bashrc
 export PYTHONPATH="$PYTHONPATH:<path to PySophus>"
+# alternately, install it to your default dist-packages
+python setup.py install
 ```
 
 Usage

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

sophus_defs.pxd

@@ -1,8 +1,6 @@
 # -*- coding: utf-8 -*-
 from eigency.core cimport *
 
-
-
 cdef extern from "<sophus/so3.hpp>" namespace "Sophus":
   cdef cppclass SO3[Scalar]:
       SO3() except +
@@ -13,6 +11,7 @@ cdef extern from "<sophus/so3.hpp>" namespace "Sophus":
       Matrix3d& matrix()
 
       Vector3d log()
+      Scalar* data()
 
       SO3 inverse()
       void normalize()