Merge pull request strawlab#56 from larsmans/sensor-info

nzjrs · nzjrs · commit 1bed4afb8104 · 2014-10-23T14:36:52.000+02:00
get sensor origin and orientation from PointCloud
diff --git a/pcl/_pcl.pyx b/pcl/_pcl.pyx
@@ -171,6 +171,19 @@ cdef class PointCloud:
     def __reduce__(self):
         return type(self), (self.to_array(),)
 
+    property sensor_origin:
+        def __get__(self):
+            cdef cpp.Vector4f origin = self.thisptr().sensor_origin_
+            cdef float *data = origin.data()
+            return np.array([data[0], data[1], data[2], data[3]],
+                            dtype=np.float32)
+
+    property sensor_orientation:
+        def __get__(self):
+            # NumPy doesn't have a quaternion type, so we return a 4-vector.
+            cdef cpp.Quaternionf o = self.thisptr().sensor_orientation_
+            return np.array([o.w(), o.x(), o.y(), o.z()])
+
     @cython.boundscheck(False)
     def from_array(self, cnp.ndarray[cnp.float32_t, ndim=2] arr not None):
         """
diff --git a/pcl/pcl_defs.pxd b/pcl/pcl_defs.pxd
@@ -7,6 +7,17 @@ from libcpp cimport bool
 from shared_ptr cimport shared_ptr
 from vector cimport vector as vector2
 
+cdef extern from "Eigen/Eigen" namespace "Eigen" nogil:
+    cdef cppclass Vector4f:
+        float *data()
+    cdef cppclass Quaternionf:
+        float w()
+        float x()
+        float y()
+        float z()
+    cdef cppclass aligned_allocator[T]:
+        pass
+
 cdef extern from "pcl/point_cloud.h" namespace "pcl":
     cdef cppclass PointCloud[T]:
         PointCloud() except +
@@ -21,6 +32,9 @@ cdef extern from "pcl/point_cloud.h" namespace "pcl":
         #T& at(int, int) except +
         shared_ptr[PointCloud[T]] makeShared()
 
+        Quaternionf sensor_orientation_
+        Vector4f sensor_origin_
+
 cdef extern from "indexing.hpp":
     # Use these instead of operator[] or at.
     PointXYZ *getptr(PointCloud[PointXYZ] *, size_t)
@@ -81,10 +95,6 @@ cdef extern from "pcl/search/kdtree.h" namespace "pcl::search":
     cdef cppclass KdTree[T]:
         KdTree()
 
-cdef extern from "Eigen/src/Core/util/Memory.h" namespace "Eigen":
-    cdef cppclass aligned_allocator[T]:
-        pass
-
 ctypedef aligned_allocator[PointXYZ] aligned_allocator_t 
 ctypedef vector2[PointXYZ, aligned_allocator_t] AlignedPointTVector_t
 
diff --git a/tests/test.py b/tests/test.py
@@ -7,6 +7,9 @@
 import pcl
 import numpy as np
 
+from numpy.testing import assert_array_equal
+
+
 _data = [(i,2*i,3*i+0.2) for i in range(5)]
 _DATA = \
 """0.0, 0.0, 0.2;
@@ -120,7 +123,7 @@ def test_pcd_read():
 COUNT 1 1 1
 WIDTH %(npts)d
 HEIGHT 1
-VIEWPOINT 0 0 0 0 1 0 0
+VIEWPOINT 0.1 0 0.5 0 1 0 0
 POINTS %(npts)d
 DATA ascii
 %(data)s"""
@@ -140,6 +143,11 @@ def test_pcd_read():
         ssd = sum((row - pt) ** 2)
         assert ssd < 1e-6
 
+    assert_array_equal(p.sensor_orientation,
+                       np.array([0, 1, 0, 0], dtype=np.float32))
+    assert_array_equal(p.sensor_origin,
+                       np.array([.1, 0, .5, 0], dtype=np.float32))
+
 SEGCYLMOD = [0.0552167, 0.0547035, 0.757707, -0.0270852, -4.41026, -2.88995, 0.0387603]
 SEGCYLIN = 11462
 
