From 39320095f7a5bfa5c14135ca78db5598b92f0801 Mon Sep 17 00:00:00 2001 From: Mahdi Ale Date: Mon, 19 Aug 2019 17:21:40 +0430 Subject: [PATCH] Makes point cloud organized by 3d array --- pcl/pxi/PointCloud_Normal.pxi | 23 +++++++++++++++ pcl/pxi/PointCloud_PointXYZ.pxi | 18 ++++++++++++ pcl/pxi/PointCloud_PointXYZRGB_180.pxi | 18 ++++++++++++ pcl/pxi/PointCloud_PointXYZ_172.pxi | 40 +++++++++++++++++++++++++- pcl/pxi/PointCloud_PointXYZ_180.pxi | 39 +++++++++++++++++++++++++ pcl/pxi/PointCloud_PointXYZ_190.pxi | 18 ++++++++++++ 6 files changed, 155 insertions(+), 1 deletion(-) diff --git a/pcl/pxi/PointCloud_Normal.pxi b/pcl/pxi/PointCloud_Normal.pxi index fa418b4fe..1f0a97cff 100644 --- a/pcl/pxi/PointCloud_Normal.pxi +++ b/pcl/pxi/PointCloud_Normal.pxi @@ -84,6 +84,29 @@ cdef class PointCloud_Normal: p = idx.getptr(self.thisptr(), i) p.normal_x, p.normal_y, p.normal_z, p.curvature = arr[i, 0], arr[i, 1], arr[i, 2], arr[i, 3] + + @cython.boundscheck(False) + def to_3d_array(self): + """ + Return this object as a 2D numpy array (float32) + """ + cdef float x,y,z + cdef cnp.npy_intp w = self.thisptr().width + cdef cnp.npy_intp h = self.thisptr().height + cdef cnp.ndarray[cnp.float32_t, ndim=3, mode="c"] result + cdef cpp.Normal *p + + result = np.empty((h, w, 4), dtype=np.float32) + for i in range(w): + for j in range(h): + + p = idx.getptr_at2(self.thisptr(), i, j) + result[j, i, 0] = p.normal_x + result[j, i, 1] = p.normal_y + result[j, i, 2] = p.normal_z + result[j, i, 3] = p.curvature + return result + @cython.boundscheck(False) def to_array(self): """ diff --git a/pcl/pxi/PointCloud_PointXYZ.pxi b/pcl/pxi/PointCloud_PointXYZ.pxi index 56de90423..379c4c281 100644 --- a/pcl/pxi/PointCloud_PointXYZ.pxi +++ b/pcl/pxi/PointCloud_PointXYZ.pxi @@ -158,6 +158,24 @@ cdef class PointCloud: new_orient[2], new_orient[3]) + @cython.boundscheck(False) + def from_3d_array(self, cnp.ndarray[cnp.float32_t, ndim=3] arr not None): + """ + Fill this object Organized from a 3D numpy array (float32) + """ + assert arr.shape[2] == 3 + + cdef cnp.npy_intp npts = arr.shape[0] * arr.shape[1] + self.resize(npts) + self.thisptr().width = arr.shape[1] + self.thisptr().height = arr.shape[0] + + cdef cpp.PointXYZ *p + for j in range(self.thisptr().width): + for i in range(self.thisptr().height): + p = idx.getptr_at2(self.thisptr(), j, i) + p.x, p.y, p.z = arr[i, j, 0], arr[i, j, 1], arr[i, j, 2] + @cython.boundscheck(False) def from_array(self, cnp.ndarray[cnp.float32_t, ndim=2] arr not None): """ diff --git a/pcl/pxi/PointCloud_PointXYZRGB_180.pxi b/pcl/pxi/PointCloud_PointXYZRGB_180.pxi index c93f7c1e2..937a069d7 100644 --- a/pcl/pxi/PointCloud_PointXYZRGB_180.pxi +++ b/pcl/pxi/PointCloud_PointXYZRGB_180.pxi @@ -149,6 +149,24 @@ cdef class PointCloud_PointXYZRGB: new_orient[2], new_orient[3]) + @cython.boundscheck(False) + def from_3d_array(self, cnp.ndarray[cnp.float32_t, ndim=3] arr not None): + """ + Fill this object Organized from a 3D numpy array (float32) + """ + assert arr.shape[2] == 4 + + cdef cnp.npy_intp npts = arr.shape[0] * arr.shape[1] + self.resize(npts) + self.thisptr().width = arr.shape[0] + self.thisptr().height = arr.shape[1] + + cdef cpp.PointXYZRGB *p + for j in range(self.thisptr().width): + for i in range(self.thisptr().height): + p = idx.getptr_at2(self.thisptr(), i, j) + p.x, p.y, p.z, p.rgb = arr[j, i, 0], arr[j, i, 1], arr[j, i, 2], arr[j, i, 3] + @cython.boundscheck(False) def from_array(self, cnp.ndarray[cnp.float32_t, ndim=2] arr not None): """ diff --git a/pcl/pxi/PointCloud_PointXYZ_172.pxi b/pcl/pxi/PointCloud_PointXYZ_172.pxi index b44b27d7f..cfd79e013 100644 --- a/pcl/pxi/PointCloud_PointXYZ_172.pxi +++ b/pcl/pxi/PointCloud_PointXYZ_172.pxi @@ -155,6 +155,23 @@ cdef class PointCloud: new_orient[1], new_orient[2], new_orient[3]) + @cython.boundscheck(False) + def from_3d_array(self, cnp.ndarray[cnp.float32_t, ndim=3] arr not None): + """ + Fill this object Organized from a 3D numpy array (float32) + """ + assert arr.shape[2] == 3 + + cdef cnp.npy_intp npts = arr.shape[0] * arr.shape[1] + self.resize(npts) + self.thisptr().width = arr.shape[1] + self.thisptr().height = arr.shape[0] + + cdef cpp.PointXYZ *p + for j in range(self.thisptr().width): + for i in range(self.thisptr().height): + p = idx.getptr_at2(self.thisptr(), j, i) + p.x, p.y, p.z = arr[i, j, 0], arr[i, j, 1], arr[i, j, 2] @cython.boundscheck(False) def from_array(self, cnp.ndarray[cnp.float32_t, ndim=2] arr not None): @@ -189,7 +206,28 @@ cdef class PointCloud: result[i, 0] = p.x result[i, 1] = p.y result[i, 2] = p.z - + + return result + + @cython.boundscheck(False) + def to_3d_array(self): + """ + Return this object as a 3D numpy array (float32) + """ + cdef float x, y, z + cdef cnp.npy_intp n = self.thisptr().size() + cdef cnp.ndarray[cnp.float32_t, ndim=3, mode="c"] result + cdef cpp.PointXYZ *p + cdef cnp.npy_intp w = self.thisptr().width + cdef cnp.npy_intp h = self.thisptr().height + result = np.empty((h, w, 3), dtype=np.float32) + for j in range(w): + for i in range(h): + p = idx.getptr_at2(self.thisptr(), j, i) + result[i, j, 0] = p.x + result[i, j, 1] = p.y + result[i, j, 2] = p.z + return result def from_list(self, _list): diff --git a/pcl/pxi/PointCloud_PointXYZ_180.pxi b/pcl/pxi/PointCloud_PointXYZ_180.pxi index 9eedc358a..f27fba8b6 100644 --- a/pcl/pxi/PointCloud_PointXYZ_180.pxi +++ b/pcl/pxi/PointCloud_PointXYZ_180.pxi @@ -156,6 +156,24 @@ cdef class PointCloud: new_orient[2], new_orient[3]) + @cython.boundscheck(False) + def from_3d_array(self, cnp.ndarray[cnp.float32_t, ndim=3] arr not None): + """ + Fill this object Organized from a 3D numpy array (float32) + """ + assert arr.shape[2] == 3 + + cdef cnp.npy_intp npts = arr.shape[0] * arr.shape[1] + self.resize(npts) + self.thisptr().width = arr.shape[1] + self.thisptr().height = arr.shape[0] + + cdef cpp.PointXYZ *p + for j in range(self.thisptr().width): + for i in range(self.thisptr().height): + p = idx.getptr_at2(self.thisptr(), j, i) + p.x, p.y, p.z = arr[i, j, 0], arr[i, j, 1], arr[i, j, 2] + @cython.boundscheck(False) def from_array(self, cnp.ndarray[cnp.float32_t, ndim=2] arr not None): """ @@ -192,6 +210,27 @@ cdef class PointCloud: return result + @cython.boundscheck(False) + def to_3d_array(self): + """ + Return this object as a 3D numpy array (float32) + """ + cdef float x, y, z + cdef cnp.npy_intp n = self.thisptr().size() + cdef cnp.ndarray[cnp.float32_t, ndim=3, mode="c"] result + cdef cpp.PointXYZ *p + cdef cnp.npy_intp w = self.thisptr().width + cdef cnp.npy_intp h = self.thisptr().height + result = np.empty((h, w, 3), dtype=np.float32) + for j in range(w): + for i in range(h): + p = idx.getptr_at2(self.thisptr(), j, i) + result[i, j, 0] = p.x + result[i, j, 1] = p.y + result[i, j, 2] = p.z + + return result + def from_list(self, _list): """ Fill this pointcloud from a list of 3-tuples diff --git a/pcl/pxi/PointCloud_PointXYZ_190.pxi b/pcl/pxi/PointCloud_PointXYZ_190.pxi index 95efe7fd1..dfb132748 100644 --- a/pcl/pxi/PointCloud_PointXYZ_190.pxi +++ b/pcl/pxi/PointCloud_PointXYZ_190.pxi @@ -156,6 +156,24 @@ cdef class PointCloud: new_orient[2], new_orient[3]) + @cython.boundscheck(False) + def from_3d_array(self, cnp.ndarray[cnp.float32_t, ndim=3] arr not None): + """ + Fill this object Organized from a 3D numpy array (float32) + """ + assert arr.shape[2] == 3 + + cdef cnp.npy_intp npts = arr.shape[0] * arr.shape[1] + self.resize(npts) + self.thisptr().width = arr.shape[1] + self.thisptr().height = arr.shape[0] + + cdef cpp.PointXYZ *p + for j in range(self.thisptr().width): + for i in range(self.thisptr().height): + p = idx.getptr_at2(self.thisptr(), j, i) + p.x, p.y, p.z = arr[i, j, 0], arr[i, j, 1], arr[i, j, 2] + @cython.boundscheck(False) def from_array(self, cnp.ndarray[cnp.float32_t, ndim=2] arr not None): """