From 05f77676c7447e16fd664c45813bfb6497085ecc Mon Sep 17 00:00:00 2001
From: Remi-C <Remi-C@fake.fake>
Date: Fri, 22 Aug 2014 15:54:51 +0200
Subject: [PATCH 1/2] work of colin csl allowing to acces normal of point
 clouds

---
 pcl/_pcl.pyx     | 19 +++++++++++++++++++
 pcl/pcl_defs.pxd |  5 +++++
 tests/test.py    | 10 ++++++++++
 3 files changed, 34 insertions(+)

diff --git a/pcl/_pcl.pyx b/pcl/_pcl.pyx
index 05e4500e4..369ee5429 100644
--- a/pcl/_pcl.pyx
+++ b/pcl/_pcl.pyx
@@ -303,6 +303,25 @@ cdef class PointCloud:
         cseg.setInputNormals (normals.makeShared());
 
         return seg
+        
+    def calc_normals(self, int ksearch=-1, double searchRadius=-1.0):
+        """
+        Return a numpy 2D array containing the normal of the pointcloud
+        :TODO: using a loop may be slow, maybe with some numpy magic it can be performed at once.
+        """
+        cdef cpp.PointNormalCloud_t normals
+        mpcl_compute_normals(deref(self.thisptr), ksearch, searchRadius, normals)
+        cdef float x,y,z
+        cdef int n = self.thisptr.size()
+        cdef cnp.ndarray[float, ndim=2] result = np.empty([n,3], dtype=np.float32)
+        cdef int i = 0
+        while i < n:
+            result[i,0] = normals.at(i).normal_x
+            result[i,1] = normals.at(i).normal_y
+            result[i,2] = normals.at(i).normal_z
+            i += 1
+        return result
+
 
     def make_statistical_outlier_filter(self):
         """
diff --git a/pcl/pcl_defs.pxd b/pcl/pcl_defs.pxd
index 19452c119..1b9e0e092 100644
--- a/pcl/pcl_defs.pxd
+++ b/pcl/pcl_defs.pxd
@@ -35,6 +35,11 @@ cdef extern from "pcl/point_types.h" namespace "pcl":
         float z
     cdef struct Normal:
         pass
+        Normal()
+        float normal_x
+        float normal_y
+        float normal_z
+        float curvature
 
 cdef extern from "pcl/features/normal_3d.h" namespace "pcl":
     cdef cppclass NormalEstimation[T, N]:
diff --git a/tests/test.py b/tests/test.py
index 29469ad4a..60ae47f87 100644
--- a/tests/test.py
+++ b/tests/test.py
@@ -221,6 +221,16 @@ def testResize(self):
         # better exceptions.
         self.assertRaises(MemoryError, self.p.resize, -1)
 
+class TestCalcNormals(unittest.TestCase):
+    def setUp(self):
+        self.p = pcl.PointCloud()
+        self.p.from_list([[0,0,0],[1,0,0],[0,1,0]])
+    def testCalcNormals(self):
+        normals = self.p.calc_normals(20)
+        truth = np.array([[0,0,1],[0,0,1],[0,0,1]])
+        self.assertEqual(normals, truth)
+        self.assertEqual(normals.size, self.p.size)
+
 class TestSegmenterNormal(unittest.TestCase):
 
     def setUp(self):

From 6f1a4a3a5066b9fa96190981debd965a7a241577 Mon Sep 17 00:00:00 2001
From: Remi-C <Remi-C@fake.fake>
Date: Fri, 22 Aug 2014 15:54:51 +0200
Subject: [PATCH 2/2] work of colin csl allowing to acces normal of point
 clouds

---
 pcl/_pcl.pyx     | 24 ++++++++++++++++++++++++
 pcl/pcl_defs.pxd |  6 +++++-
 tests/test.py    | 10 ++++++++++
 3 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/pcl/_pcl.pyx b/pcl/_pcl.pyx
index 05e4500e4..4ef717bc0 100644
--- a/pcl/_pcl.pyx
+++ b/pcl/_pcl.pyx
@@ -303,6 +303,30 @@ cdef class PointCloud:
         cseg.setInputNormals (normals.makeShared());
 
         return seg
+        
+    def calc_normals(self, int ksearch=-1, double searchRadius=-1.0):
+        """
+        Return a numpy 2D array containing the normal of the pointcloud
+        :TODO: using a loop may be slow, maybe with some numpy magic it can be performed at once.
+        """
+        if ksearch == -1 and searchRadius == -1.0:
+            raise ValueError, "At least one input parameter must be entered"
+        
+        cdef cpp.PointNormalCloud_t normals
+        mpcl_compute_normals(deref(self.thisptr), ksearch, searchRadius, normals)
+        cdef float x,y,z
+        cdef int n = self.thisptr.size()
+        cdef cnp.ndarray[float, ndim=2] result = np.empty([n,3], dtype=np.float32)
+        cdef int i = 0
+        while i < n:
+            result[i,0] = normals.at(i).normal_x
+            result[i,1] = normals.at(i).normal_y
+            result[i,2] = normals.at(i).normal_z
+            i += 1
+        return result
+
+
+
 
     def make_statistical_outlier_filter(self):
         """
diff --git a/pcl/pcl_defs.pxd b/pcl/pcl_defs.pxd
index 19452c119..5041d2693 100644
--- a/pcl/pcl_defs.pxd
+++ b/pcl/pcl_defs.pxd
@@ -34,7 +34,11 @@ cdef extern from "pcl/point_types.h" namespace "pcl":
         float y
         float z
     cdef struct Normal:
-        pass
+        Normal()
+        float normal_x
+        float normal_y
+        float normal_z
+        float curvature
 
 cdef extern from "pcl/features/normal_3d.h" namespace "pcl":
     cdef cppclass NormalEstimation[T, N]:
diff --git a/tests/test.py b/tests/test.py
index 29469ad4a..60ae47f87 100644
--- a/tests/test.py
+++ b/tests/test.py
@@ -221,6 +221,16 @@ def testResize(self):
         # better exceptions.
         self.assertRaises(MemoryError, self.p.resize, -1)
 
+class TestCalcNormals(unittest.TestCase):
+    def setUp(self):
+        self.p = pcl.PointCloud()
+        self.p.from_list([[0,0,0],[1,0,0],[0,1,0]])
+    def testCalcNormals(self):
+        normals = self.p.calc_normals(20)
+        truth = np.array([[0,0,1],[0,0,1],[0,0,1]])
+        self.assertEqual(normals, truth)
+        self.assertEqual(normals.size, self.p.size)
+
 class TestSegmenterNormal(unittest.TestCase):
 
     def setUp(self):