Adds support to create Octree from PointCloud data

src/fcl/fcl.pyx

@@ -464,15 +464,61 @@ cdef class BVHModel(CollisionGeometry):
 cdef class OcTree(CollisionGeometry):
     cdef octomap.OcTree* tree
 
-    def __cinit__(self, r, data):
+    def __cinit__(self, r, data=None, points=None, origin=None, maxrange=-1., lazy_eval=False, discretize=False):
+        self.thisptr = NULL
+
+        self.tree = new octomap.OcTree(r)
+
+        if data is not None:
+            self.readBinaryData(data)
+        elif points is not None:
+            self.insertPointCloud(points, origin, maxrange, lazy_eval, discretize)
+
+    def readBinaryData(self, data):
         cdef std.stringstream ss
         cdef vector[char] vd = data
-        ss.write(vd.data(), len(data))
 
-        self.tree = new octomap.OcTree(r)
+        if self.thisptr != NULL:
+            del self.thisptr
+
+        ss.write(vd.data(), len(data))
         self.tree.readBinaryData(ss)
         self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree))
 
+    def insertPointCloud(self, points, origin=None, maxrange=-1., lazy_eval=False, discretize=False):
+        cdef octomap.Pointcloud pc = octomap.Pointcloud()
+
+        if self.thisptr != NULL:
+            del self.thisptr
+
+        for p in points:
+            pc.push_back(<float>p[0],
+                         <float>p[1],
+                         <float>p[2])
+        if origin is not None:
+            self.tree.insertPointCloud(
+                pc,
+                octomap.Vector3(<float>origin[0],
+                                <float>origin[1],
+                                <float>origin[2]),
+                <double?>maxrange,
+                <bool>lazy_eval,
+                <bool>discretize
+            )
+        else:
+            self.tree.insertPointCloud(
+                pc,
+                octomap.Vector3(<float>0.,
+                                <float>0.,
+                                <float>0.),
+                <double?>maxrange,
+                <bool>lazy_eval,
+                <bool>discretize
+            )
+        self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree))
+
+
+
 
 ###############################################################################
 # Collision managers

src/fcl/octomap_defs.pxd

@@ -1,4 +1,23 @@
 cimport std_defs as std
+from libcpp cimport bool
+
+cdef extern from "octomap/math/Vector3.h" namespace "octomath":
+    cdef cppclass Vector3:
+        Vector3() except +
+        Vector3(float, float, float) except +
+        Vector3(Vector3& other) except +
+        float& x()
+        float& y()
+        float& z()
+
+cdef extern from "octomap/octomap_types.h" namespace "octomap":
+    ctypedef Vector3 point3d
+
+cdef extern from "octomap/Pointcloud.h" namespace "octomap":
+    cdef cppclass Pointcloud:
+        Pointcloud() except +
+        void push_back(float, float, float)
+        void push_back(point3d* p)
 
 
 cdef extern from "octomap/OccupancyOcTreeBase.h" namespace "octomap":
@@ -17,3 +36,8 @@ cdef extern from "octomap/OcTree.h" namespace "octomap":
     cdef cppclass OcTree(OccupancyOcTreeBase):
         # Constructing
         OcTree(double resolution) except +
+
+        void insertPointCloud(Pointcloud& scan, point3d& sensor_origin,
+                              double maxrange, bool lazy_eval, bool discretize) except +
+
+