From 33f5d05e8503b1854c573d426ed55a4769121385 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Tue, 12 Jan 2021 13:07:25 +0000 Subject: [PATCH 1/8] force standard library, no c++11 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index d26ca8bcd..be9b0607f 100644 --- a/setup.py +++ b/setup.py @@ -798,7 +798,7 @@ def pkgconfig_win(flag, cut): # ext_args['extra_link_args'].append('-fopenmp -Xpreprocessor -lomp') pass else: - ext_args['extra_compile_args'].append("-std=c++11") + # ext_args['extra_compile_args'].append("-std=c++11") ext_args['library_dirs'].append("/usr/lib/x86_64-linux-gnu/") # gcc? use standard library # ext_args['extra_compile_args'].append("-stdlib=libstdc++") From 75538d4547325f35d93bfe2c181b930852780c23 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Wed, 13 Jan 2021 10:37:15 +0000 Subject: [PATCH 2/8] make factory function for NDT solver --- pcl/pxi/PointCloud_PointXYZ_172.pxi | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/pcl/pxi/PointCloud_PointXYZ_172.pxi b/pcl/pxi/PointCloud_PointXYZ_172.pxi index b44b27d7f..ddf03d513 100644 --- a/pcl/pxi/PointCloud_PointXYZ_172.pxi +++ b/pcl/pxi/PointCloud_PointXYZ_172.pxi @@ -560,6 +560,12 @@ cdef class PointCloud: cIterativeClosestPoint.setInputCloud( self.thisptr_shared) return iterativeClosestPoint + def make_NormalDistributionsTransform(self): + normalDistributionsTransform = NormalDistributionsTransform(self) + cdef pcl_reg.NormalDistributionsTransform_t *cNormalDistributionsTransform = normalDistributionsTransform.me + cNormalDistributionsTransform.setInputCloud( self.thisptr_shared) + return normalDistributionsTransform + def make_MomentOfInertiaEstimation(self): momentofinertiaestimation = MomentOfInertiaEstimation(self) cdef pcl_ftr.MomentOfInertiaEstimation_t *cMomentOfInertiaEstimation = momentofinertiaestimation.me From adc30fc929439eeb7042dbc17bc332aeb40ece26 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Wed, 13 Jan 2021 11:13:31 +0000 Subject: [PATCH 3/8] ndt routine works --- .../NormalDistributionsTransform_172.pxi | 93 ++++++++++++++----- 1 file changed, 72 insertions(+), 21 deletions(-) diff --git a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi index 06b0b0af0..74b17a3b5 100644 --- a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi +++ b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi @@ -15,33 +15,84 @@ cdef class NormalDistributionsTransform: def __dealloc__(self): del self.me - # def set_InputTarget(self, pcl_reg.RegistrationPtr_t cloud): - def set_InputTarget(self): - # self.me.setInputTarget (cloud.this_ptr()) - pass + # # def set_InputTarget(self, pcl_reg.RegistrationPtr_t cloud): + # def set_InputTarget(self): + # # self.me.setInputTarget (cloud.this_ptr()) + # pass - def set_Resolution(self, float resolution): - self.me.setResolution(resolution) - pass + # def set_Resolution(self, float resolution): + # self.me.setResolution(resolution) + # pass - def get_Resolution(self): - return self.me.getResolution() + # def get_Resolution(self): + # return self.me.getResolution() - def get_StepSize(self): - return self.me.getStepSize() + # def get_StepSize(self): + # return self.me.getStepSize() - def set_StepSize(self, double step_size): - self.me.setStepSize(step_size) + # def set_StepSize(self, double step_size): + # self.me.setStepSize(step_size) - def get_OulierRatio(self): - return self.me.getOulierRatio() + # def get_OulierRatio(self): + # return self.me.getOulierRatio() - def set_OulierRatio(self, double outlier_ratio): - self.me.setOulierRatio(outlier_ratio) + # def set_OulierRatio(self, double outlier_ratio): + # self.me.setOulierRatio(outlier_ratio) - def get_TransformationProbability(self): - return self.me.getTransformationProbability() + # def get_TransformationProbability(self): + # return self.me.getTransformationProbability() - def get_FinalNumIteration(self): - return self.me.getFinalNumIteration() + # def get_FinalNumIteration(self): + # return self.me.getFinalNumIteration() + cdef object run(self, pcl_reg.NormalDistributionsTransform_t ®, _pcl.PointCloud source, _pcl.PointCloud target, max_iter=None): + reg.setInputTarget(target.thisptr_shared) + + if max_iter is not None: + reg.setMaximumIterations(max_iter) + + cdef _pcl.PointCloud result = _pcl.PointCloud() + + reg.align(result.thisptr()[0]) + + # Get transformation matrix and convert from Eigen to NumPy format. + # cdef pcl_reg.Registration[cpp.PointXYZ, cpp.PointXYZ].Matrix4f mat + cdef Matrix4f mat + mat = reg.getFinalTransformation() + cdef np.ndarray[dtype=np.float32_t, ndim=2, mode='fortran'] transf + cdef np.float32_t *transf_data + + transf = np.empty((4, 4), dtype=np.float32, order='F') + transf_data = np.PyArray_DATA(transf) + + for i in range(16): + transf_data[i] = mat.data()[i] + + return reg.hasConverged(), transf, result, reg.getTransformationProbability() + + def ndt(self, _pcl.PointCloud source, _pcl.PointCloud target, max_iter=None): + """ + Align source to target using normal distributions transform (NDT). + Parameters + ---------- + source : PointCloud + Source point cloud. + target : PointCloud + Target point cloud. + max_iter : integer, optional + Maximum number of iterations. If not given, uses the default number + hardwired into PCL. + Returns + ------- + converged : bool + Whether the NDT algorithm converged in at most max_iter steps. + transf : np.ndarray, shape = [4, 4] + Transformation matrix. + estimate : PointCloud + Transformed version of source. + transf_prob : float + The registration alignment probability. + """ + cdef pcl_reg.NormalDistributionsTransform_t ndt + ndt.setInputCloud(source.thisptr_shared) + return self.run(ndt, source, target, max_iter) From d7c60ec36842bbfdff855bdde58feb7a82a0bc79 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Wed, 13 Jan 2021 11:40:40 +0000 Subject: [PATCH 4/8] adds bindings to mutators for NDT-specific parameters --- .../NormalDistributionsTransform_172.pxi | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi index 74b17a3b5..c560835bd 100644 --- a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi +++ b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi @@ -45,12 +45,9 @@ cdef class NormalDistributionsTransform: # def get_FinalNumIteration(self): # return self.me.getFinalNumIteration() - cdef object run(self, pcl_reg.NormalDistributionsTransform_t ®, _pcl.PointCloud source, _pcl.PointCloud target, max_iter=None): + cdef object run(self, pcl_reg.NormalDistributionsTransform_t ®, _pcl.PointCloud source, _pcl.PointCloud target): reg.setInputTarget(target.thisptr_shared) - - if max_iter is not None: - reg.setMaximumIterations(max_iter) - + cdef _pcl.PointCloud result = _pcl.PointCloud() reg.align(result.thisptr()[0]) @@ -70,7 +67,7 @@ cdef class NormalDistributionsTransform: return reg.hasConverged(), transf, result, reg.getTransformationProbability() - def ndt(self, _pcl.PointCloud source, _pcl.PointCloud target, max_iter=None): + def ndt(self, _pcl.PointCloud source, _pcl.PointCloud target, max_iter=None, resolution=None, step_size=None, outlier_ratio=None): """ Align source to target using normal distributions transform (NDT). Parameters @@ -82,6 +79,15 @@ cdef class NormalDistributionsTransform: max_iter : integer, optional Maximum number of iterations. If not given, uses the default number hardwired into PCL. + resolution : float, optional + Voxel grid resolution. If not given, uses the default number + hardwired into PCL. + step_size : float, optional + Newton line search maximum step length. If not given, uses the default number + hardwired into PCL. + outlier_ratio : float, optional + Point cloud outlier ratio. If not given, uses the default number + hardwired into PCL. Returns ------- converged : bool @@ -94,5 +100,15 @@ cdef class NormalDistributionsTransform: The registration alignment probability. """ cdef pcl_reg.NormalDistributionsTransform_t ndt + + if max_iter is not None: + reg.setMaximumIterations(max_iter) + if resolution is not None: + reg.setResolution(max_iter) + if step_size is not None: + reg.setStepSize(max_iter) + if outlier_ratio is not None: + reg.setOulierRatio(max_iter) + ndt.setInputCloud(source.thisptr_shared) return self.run(ndt, source, target, max_iter) From d1e4d1c4b5a4adff5ae91619e0c68b84a0c3d394 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Wed, 13 Jan 2021 11:43:35 +0000 Subject: [PATCH 5/8] fix ndt obj name --- pcl/pxi/registration/NormalDistributionsTransform_172.pxi | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi index c560835bd..efdec831e 100644 --- a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi +++ b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi @@ -102,13 +102,13 @@ cdef class NormalDistributionsTransform: cdef pcl_reg.NormalDistributionsTransform_t ndt if max_iter is not None: - reg.setMaximumIterations(max_iter) + ndt.setMaximumIterations(max_iter) if resolution is not None: - reg.setResolution(max_iter) + ndt.setResolution(max_iter) if step_size is not None: - reg.setStepSize(max_iter) + ndt.setStepSize(max_iter) if outlier_ratio is not None: - reg.setOulierRatio(max_iter) + ndt.setOulierRatio(max_iter) ndt.setInputCloud(source.thisptr_shared) return self.run(ndt, source, target, max_iter) From 09258c52d5e6bb5084ed0cdd355d598e881306fd Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Wed, 13 Jan 2021 11:45:29 +0000 Subject: [PATCH 6/8] fix run meth call --- pcl/pxi/registration/NormalDistributionsTransform_172.pxi | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi index efdec831e..23b102111 100644 --- a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi +++ b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi @@ -111,4 +111,4 @@ cdef class NormalDistributionsTransform: ndt.setOulierRatio(max_iter) ndt.setInputCloud(source.thisptr_shared) - return self.run(ndt, source, target, max_iter) + return self.run(ndt, source, target) From 8bdb2572134358e7fc4f915d81eae7b51fd22a14 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Thu, 14 Jan 2021 11:44:36 +0000 Subject: [PATCH 7/8] fix mutator use --- pcl/pxi/registration/NormalDistributionsTransform_172.pxi | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi index 23b102111..f886e5a4e 100644 --- a/pcl/pxi/registration/NormalDistributionsTransform_172.pxi +++ b/pcl/pxi/registration/NormalDistributionsTransform_172.pxi @@ -104,11 +104,11 @@ cdef class NormalDistributionsTransform: if max_iter is not None: ndt.setMaximumIterations(max_iter) if resolution is not None: - ndt.setResolution(max_iter) + ndt.setResolution(resolution) if step_size is not None: - ndt.setStepSize(max_iter) + ndt.setStepSize(step_size) if outlier_ratio is not None: - ndt.setOulierRatio(max_iter) + ndt.setOulierRatio(outlier_ratio) ndt.setInputCloud(source.thisptr_shared) return self.run(ndt, source, target) From b9b07801da8c5e266ab92dbb497b178536fc5e4d Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Thu, 14 Jan 2021 11:45:24 +0000 Subject: [PATCH 8/8] add visualisation cpp (generated) to ignored files --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index ffb5d8d44..095b96f75 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,7 @@ pcl/_pcl*.cpp pcl/_pcl.html pcl/registration.cpp +pcl/pcl_visualization.cpp *.py[co] *.so