improve KdTreeFLANN API and test it

Previously, constructing a KdTreeFLANN object made no sense since there
was no way to set its input cloud. It would segfault at first use.

Author: larsmans

examples/kdtree.py

@@ -18,7 +18,7 @@
 pc_1.from_array(points_1)
 pc_2 = pcl.PointCloud()
 pc_2.from_array(points_2)
-kd = pc_1.make_kdtree_flann()
+kd = pcl.KdTreeFLANN(pc_1)
 # find the single closest points to each point in point cloud 2
 # (and the sqr distances)
 indices, sqr_distances = kd.nearest_k_search_for_cloud(pc_2, 1)

pcl/_pcl.pyx

@@ -341,12 +341,10 @@ cdef class PointCloud:
     def make_kdtree_flann(self):
         """
         Return a pcl.kdTreeFLANN object with this object set as the input-cloud
+
+        Deprecated: use the pcl.KdTreeFLANN constructor on this cloud.
         """
-        kdtree = KdTreeFLANN()
-        cdef cpp.KdTreeFLANN_t *ckdtree = <cpp.KdTreeFLANN_t *>kdtree.me
-        cdef cpp.PointCloud_t *ccloud = <cpp.PointCloud_t *>self.thisptr
-        ckdtree.setInputCloud(ccloud.makeShared())
-        return kdtree
+        return KdTreeFLANN(self)
 
     def make_octree(self, double resolution):
         """
@@ -510,11 +508,20 @@ cdef class PassThroughFilter:
 cdef class KdTreeFLANN:
     """
     Finds k nearest neighbours from points in another pointcloud to points in
-    this pointcloud
+    a reference pointcloud.
+
+    Must be constructed from the reference point cloud, which is copied, so
+    changed to pc are not reflected in KdTreeFLANN(pc).
     """
     cdef cpp.KdTreeFLANN_t *me
-    def __cinit__(self):
+
+    def __cinit__(self, PointCloud pc):
+        # XXX it seems copying the entire pointcloud is the only option in
+        # PCL 1.7.1.
+        cdef cpp.PointCloud_t *ccloud = <cpp.PointCloud_t *>pc.thisptr
         self.me = new cpp.KdTreeFLANN_t()
+        self.me.setInputCloud(ccloud.makeShared())
+
     def __dealloc__(self):
         del self.me
 

tests/test.py

@@ -298,6 +298,37 @@ def testFilterBoth(self):
         back = fil.filter().size
         self.assertEqual(total,front+back)
 
+class TestKdTree(unittest.TestCase):
+    def setUp(self):
+        rng = np.random.RandomState(42)
+        # Define two dense sets of points of sizes 30 and 170, resp.
+        a = np.random.randn(100, 3).astype(np.float32)
+        a[:30] -= 42
+
+        self.pc = pcl.PointCloud(a)
+        self.kd = pcl.KdTreeFLANN(self.pc)
+
+    def testException(self):
+        self.assertRaises(TypeError, pcl.KdTreeFLANN)
+
+    def testKNN(self):
+        # Small cluster
+        ind, sqdist = self.kd.nearest_k_search_for_point(self.pc, 0, k=2)
+        for i in ind:
+            self.assertGreaterEqual(i, 0)
+            self.assertLess(i, 30)
+        for d in sqdist:
+            self.assertGreaterEqual(d, 0)
+
+        # Big cluster
+        for ref, k in ((80, 1), (59, 3), (60, 10)):
+            ind, sqdist = self.kd.nearest_k_search_for_point(self.pc, ref, k=k)
+            for i in ind:
+                self.assertGreaterEqual(i, 0)
+                self.assertGreaterEqual(i, 30)
+            for d in sqdist:
+                self.assertGreaterEqual(d, 0)
+
 class TestOctreePointCloud(unittest.TestCase):
     def setUp(self):
         self.t = pcl.OctreePointCloud(0.1)