get_occupied_voxel_centers and std::vector with allocator

psporysz · psporysz · commit f1c06bb38c6c · 2013-03-03T21:48:38.000+01:00
diff --git a/pcl.pyx b/pcl.pyx
@@ -565,3 +565,11 @@ cdef class OctreePointCloud:
     """
     def is_voxel_occupied_at_point(self, double point_x, double point_y, double point_z):
         return self.me.isVoxelOccupiedAtPoint(point_x, point_y, point_z)
+
+    """
+    Get list of centers of all occupied voxels.
+    """
+    def get_occupied_voxel_centers(self):
+        cdef cpp.AlignedPointTVector_t points_v
+        cdef int num = self.me.getOccupiedVoxelCenters (points_v)
+        return [(points_v[i].x, points_v[i].y, points_v[i].z) for i in range(num)]
diff --git a/pcl_defs.pxd b/pcl_defs.pxd
@@ -3,6 +3,7 @@ from libcpp.string cimport string
 from libcpp cimport bool
 
 from shared_ptr cimport shared_ptr
+from vector cimport vector as vector2
 
 cdef extern from "pcl/point_cloud.h" namespace "pcl":
     cdef cppclass PointCloud[T]:
@@ -72,6 +73,13 @@ 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
+
 cdef extern from "pcl/octree/octree_pointcloud.h" namespace "pcl::octree":
     cdef cppclass OctreePointCloud[T]:
         OctreePointCloud(double)
@@ -81,7 +89,7 @@ cdef extern from "pcl/octree/octree_pointcloud.h" namespace "pcl::octree":
         void addPointsFromInputCloud()
         void deleteTree()
         bool isVoxelOccupiedAtPoint(double, double, double)
-        int getOccupiedVoxelCenters(vector[PointXYZ])	
+        int getOccupiedVoxelCenters(AlignedPointTVector_t)	
         void deleteVoxelAtPoint(PointXYZ)
 
 ctypedef OctreePointCloud[PointXYZ] OctreePointCloud_t
diff --git a/tests/test.py b/tests/test.py
@@ -301,3 +301,5 @@ def testLoad(self):
         self.assertTrue(rs)
         rs = self.t.is_voxel_occupied_at_point(0.5, 0.5, 0.5) 
         self.assertFalse(rs)
+
+        self.assertEqual(len(self.t.get_occupied_voxel_centers()), 44)
diff --git a/vector.pxd b/vector.pxd
@@ -0,0 +1,68 @@
+cdef extern from "<vector>" namespace "std":
+    cdef cppclass vector[T, A]:
+        cppclass iterator:
+            T& operator*() nogil
+            iterator operator++() nogil
+            iterator operator--() nogil
+            bint operator==(iterator) nogil
+            bint operator!=(iterator) nogil
+            bint operator<(iterator) nogil
+            bint operator>(iterator) nogil
+            bint operator<=(iterator) nogil
+            bint operator>=(iterator) nogil
+        cppclass reverse_iterator:
+            T& operator*() nogil
+            iterator operator++() nogil
+            iterator operator--() nogil
+            bint operator==(reverse_iterator) nogil
+            bint operator!=(reverse_iterator) nogil
+            bint operator<(reverse_iterator) nogil
+            bint operator>(reverse_iterator) nogil
+            bint operator<=(reverse_iterator) nogil
+            bint operator>=(reverse_iterator) nogil
+        #cppclass const_iterator(iterator):
+        # pass
+        #cppclass const_reverse_iterator(reverse_iterator):
+        # pass
+        vector() nogil except +
+        vector(vector&) nogil except +
+        vector(size_t) nogil except +
+        vector(size_t, T&) nogil except +
+        #vector[input_iterator](input_iterator, input_iterator)
+        T& operator[](size_t) nogil
+        #vector& operator=(vector&)
+        bint operator==(vector&, vector&) nogil
+        bint operator!=(vector&, vector&) nogil
+        bint operator<(vector&, vector&) nogil
+        bint operator>(vector&, vector&) nogil
+        bint operator<=(vector&, vector&) nogil
+        bint operator>=(vector&, vector&) nogil
+        void assign(size_t, T&) nogil
+        #void assign[input_iterator](input_iterator, input_iterator)
+        T& at(size_t) nogil
+        T& back() nogil
+        iterator begin() nogil
+        #const_iterator begin()
+        size_t capacity() nogil
+        void clear() nogil
+        bint empty() nogil
+        iterator end() nogil
+        #const_iterator end()
+        iterator erase(iterator) nogil
+        iterator erase(iterator, iterator) nogil
+        T& front() nogil
+        iterator insert(iterator, T&) nogil
+        void insert(iterator, size_t, T&) nogil
+        void insert(iterator, iterator, iterator) nogil
+        size_t max_size() nogil
+        void pop_back() nogil
+        void push_back(T&) nogil
+        reverse_iterator rbegin() nogil
+        #const_reverse_iterator rbegin()
+        reverse_iterator rend() nogil
+        #const_reverse_iterator rend()
+        void reserve(size_t) nogil
+        void resize(size_t) nogil
+        void resize(size_t, T&) nogil
+        size_t size() nogil
+        void swap(vector&) nogil
