add StatisticalOutlierFilter

nzjrs · nzjrs · commit 45554c74face · 2013-02-02T23:42:01.000+01:00
diff --git a/pcl.pyx b/pcl.pyx
@@ -249,6 +249,16 @@ cdef class PointCloud:
 
         return seg
 
+    def make_statistical_outlier_filter(self):
+        """
+        Return a pcl.StatisticalOutlierRemovalFilter object with this object set as the input-cloud
+        """
+        fil = StatisticalOutlierRemovalFilter()
+        cdef cpp.StatisticalOutlierRemoval_t *cfil = <cpp.StatisticalOutlierRemoval_t *>fil.me
+        cdef cpp.PointCloud_t *ccloud = <cpp.PointCloud_t *>self.thisptr
+        cfil.setInputCloud(ccloud.makeShared())
+        return fil
+
     def filter_mls(self, double searchRadius, bool polynomialFit=True, int polynomialOrder=2):
         """
         Perform a Moving Least Squares filter on this cloud and return the
@@ -290,3 +300,42 @@ cdef class PointCloud:
 
         return pycloud
 
+cdef class StatisticalOutlierRemovalFilter:
+    """
+    Filter class uses point neighborhood statistics to filter outlier data.
+    """
+    cdef cpp.StatisticalOutlierRemoval_t *me
+    def __cinit__(self):
+        self.me = new cpp.StatisticalOutlierRemoval_t()
+    def __dealloc__(self):
+        del self.me
+
+    def set_mean_k(self, int k):
+        """
+        Set the number of points (k) to use for mean distance estimation. 
+        """
+        self.me.setMeanK(k)
+
+    def set_std_dev_mul_thresh(self, double std_mul):
+        """
+        Set the standard deviation multiplier threshold.
+        """
+        self.me.setStddevMulThresh(std_mul)
+
+    def set_negative(self, bool negative):
+        """
+        Set whether the indices should be returned, or all points except the indices. 
+        """
+        self.me.setNegative(negative)
+
+    def filter(self):
+        """
+        Apply the filter according to the previously set parameters and return
+        a new pointcloud
+        """
+        pc = PointCloud()
+        cdef cpp.PointCloud_t *ccloud = <cpp.PointCloud_t *>pc.thisptr
+        self.me.filter(deref(ccloud))
+        return pc
+
+
diff --git a/pcl_defs.pxd b/pcl_defs.pxd
@@ -133,3 +133,14 @@ ctypedef PointCloud[PointXYZ] PointCloud_t
 ctypedef PointCloud[Normal] PointNormalCloud_t
 ctypedef shared_ptr[PointCloud[PointXYZ]] PointCloudPtr_t
 
+cdef extern from "pcl/filters/statistical_outlier_removal.h" namespace "pcl":
+    cdef cppclass StatisticalOutlierRemoval[T]:
+        StatisticalOutlierRemoval()
+        void setMeanK (int nr_k)
+        void setStddevMulThresh (double std_mul)
+        void setNegative (bool negative)
+        void setInputCloud (shared_ptr[PointCloud[T]])
+        void filter(PointCloud[T] c)
+
+ctypedef StatisticalOutlierRemoval[PointXYZ] StatisticalOutlierRemoval_t
+
diff --git a/tests/test.py b/tests/test.py
@@ -204,4 +204,30 @@ def testExtractNeg(self):
         self.assertNotEqual(self.p, p2)
         self.assertEqual(p2.size, self.p.size - 3)
 
+class TestSegmenterNormal(unittest.TestCase):
+
+    def setUp(self):
+        self.p = pcl.PointCloud()
+        self.p.from_file("tests/table_scene_mug_stereo_textured_noplane.pcd")
+
+    def testFilterPos(self):
+        fil = self.p.make_statistical_outlier_filter()
+        fil.set_mean_k (50)
+        fil.set_std_dev_mul_thresh (1.0)
+        c = fil.filter()
+        self.assertEqual(c.size, 22747)
+        self.assertEqual(c.width, 22747)
+        self.assertEqual(c.height, 1)
+        self.assertTrue(c.is_dense)
+
+    def testFilterNeg(self):
+        fil = self.p.make_statistical_outlier_filter()
+        fil.set_mean_k (50)
+        fil.set_std_dev_mul_thresh (1.0)
+        fil.set_negative(True)
+        c = fil.filter()
+        self.assertEqual(c.size, 1013)
+        self.assertEqual(c.width, 1013)
+        self.assertEqual(c.height, 1)
+        self.assertTrue(c.is_dense)
 
