Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding euclidean cluster extraction #1

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
68 changes: 68 additions & 0 deletions examples/cluster_extraction.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
'''
This file is a python port of the c++ tutorial from:
http://pointclouds.org/documentation/tutorials/cluster_extraction.php

You will also need the file, which was used in that tutorial. It might
be found here:
https://raw.github.com/PointCloudLibrary/data/master/tutorials/table_scene_lms400.pcd

The output of this code, are cloud_cluster_X.pcd files, that might be
viewed with the help of pcl_viewer.
'''
import pcl

# Read in the cloud data
my_pcl = pcl.load("table_scene_lms400.pcd")
print("PointCloud before filtering has: {} data points".format(my_pcl.size))

# Create the filtering object: downsample
# the dataset using a leaf size of 1cm
vg_filter = my_pcl.make_voxel_grid_filter()
vg_filter.set_leaf_size(0.01, 0.01, 0.01)
cloud_filtered = vg_filter.filter()
print("PointCloud after filtering has:"
"{} data points".format(cloud_filtered.size))

nr_points = cloud_filtered.size

while cloud_filtered.size > 0.3 * nr_points:
# Create the segmentation object for the planar model
# and set all the parameters
seg = cloud_filtered.make_segmenter()
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_max_iterations(100)
seg.set_distance_threshold(0.02)

indices, model = seg.segment()
cloud_plane = cloud_filtered.extract(indices, False)
print("PointCloud representing the planar"
"component: {} data points".format(cloud_plane.size))
cloud_filtered = cloud_filtered.extract(indices, True)

# Creating the KdTree object for the search method of the extraction
kdtree = pcl.KdTree(cloud_filtered)

# Creating euclidean cluster extraction
ec = cloud_filtered.make_euclidean_cluster_extractor()
ec.set_cluster_tolerance(0.02)
ec.set_min_cluster_size(100)
ec.set_max_cluster_size(25000)
ec.set_search_method(kdtree)
ec.set_input_cloud(cloud_filtered)
cluster_indices = ec.extract()

for i, indices in enumerate(cluster_indices):
cluster_points = [cloud_filtered[ind] for ind in indices]
pc = pcl.PointCloud(cluster_points)
# Setting width and height explicitly is not required, as
# its already done in from_list method in class PointCloud,
# however for completness with c++ example, let's do it again..
pc.width = pc.size
pc.height = 1
pc.is_dense = True
print("PointCloud representing the cluster:"
"{} data points.".format(pc.size))
file_name = str.encode("cloud_cluster_{}.pcd".format(i))
pc.to_file(file_name)
53 changes: 53 additions & 0 deletions pcl/_pcl.pyx
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,37 @@ SACMODEL_STICK = cpp.SACMODEL_STICK

cnp.import_array()

cdef class EuclideanClusterExtraction:
cdef cpp.EuclideanClusterExtraction_t *me

def __cinit__(self):
self.me = new cpp.EuclideanClusterExtraction_t()

def __dealloc__(self):
del self.me

def set_cluster_tolerance(self, double t):
self.me.setClusterTolerance(t)

def set_min_cluster_size(self, int s):
self.me.setMinClusterSize(s)

def set_max_cluster_size(self, int s):
self.me.setMaxClusterSize(s)

def set_input_cloud(self, PointCloud pc):
self.me.setInputCloud(pc.thisptr_shared)

def set_search_method(self, KdTree kt):
self.me.setSearchMethod(kt.thisptr_shared)

def extract(self):
cdef vector[cpp.PointIndices] ind
self.me.extract(ind)
result = []
for i in range(ind.size()):
result.append([ind[i].indices[j] for j in range(ind[i].indices.size())])
return result

cdef class Segmentation:
"""
Expand All @@ -83,6 +114,8 @@ cdef class Segmentation:
self.me.setMethodType (m)
def set_distance_threshold(self, float d):
self.me.setDistanceThreshold (d)
def set_max_iterations(self, int i):
self.me.setMaxIterations (i)

#yeah, I can't be bothered making this inherit from SACSegmentation, I forget the rules
#for how this works in cython templated extension types anyway
Expand Down Expand Up @@ -185,15 +218,18 @@ cdef class PointCloud:
property width:
""" property containing the width of the point cloud """
def __get__(self): return self.thisptr().width
def __set__(self, value): self.thisptr().width = value
property height:
""" property containing the height of the point cloud """
def __get__(self): return self.thisptr().height
def __set__(self, value): self.thisptr().height = value
property size:
""" property containing the number of points in the point cloud """
def __get__(self): return self.thisptr().size()
property is_dense:
""" property containing whether the cloud is dense or not """
def __get__(self): return self.thisptr().is_dense
def __set__(self, value): self.thisptr().is_dense = value

def __repr__(self):
return "<PointCloud of %d points>" % self.size
Expand Down Expand Up @@ -358,6 +394,12 @@ cdef class PointCloud:
error = cpp.savePLYFile(s, deref(self.thisptr()), binary)
return error

def make_euclidean_cluster_extractor(self):
ext = EuclideanClusterExtraction()
cdef cpp.EuclideanClusterExtraction_t *cext = <cpp.EuclideanClusterExtraction_t *>ext.me
cext.setInputCloud(self.thisptr_shared)
return ext

def make_segmenter(self):
"""
Return a pcl.Segmentation object with this object set as the input-cloud
Expand Down Expand Up @@ -603,6 +645,17 @@ cdef class PassThroughFilter:
self.me.filter(pc.thisptr()[0])
return pc

cdef class KdTree:
cdef cpp.KdTreePtr_t thisptr_shared

def __cinit__(self, PointCloud pc not None):
me = new cpp.KdTree[cpp.PointXYZ]()
sp_assign(self.thisptr_shared, me)
me.setInputCloud(pc.thisptr_shared)

cdef inline cpp.KdTree[cpp.PointXYZ] *thisptr(self) nogil:
return self.thisptr_shared.get()

cdef class KdTreeFLANN:
"""
Finds k nearest neighbours from points in another pointcloud to points in
Expand Down
16 changes: 16 additions & 0 deletions pcl/pcl_defs.pxd
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,7 @@ cdef extern from "pcl/segmentation/sac_segmentation.h" namespace "pcl":
void setDistanceThreshold (float)
void setInputCloud (shared_ptr[PointCloud[T]])
void segment (PointIndices, ModelCoefficients)
void setMaxIterations (int)

ctypedef SACSegmentation[PointXYZ] SACSegmentation_t
ctypedef SACSegmentationFromNormals[PointXYZ,Normal] SACSegmentationNormal_t
Expand All @@ -97,6 +98,21 @@ ctypedef MovingLeastSquares[PointXYZ,PointXYZ] MovingLeastSquares_t
cdef extern from "pcl/search/kdtree.h" namespace "pcl::search":
cdef cppclass KdTree[T]:
KdTree()
void setInputCloud (shared_ptr[PointCloud[T]])

ctypedef shared_ptr[KdTree[PointXYZ]] KdTreePtr_t

cdef extern from "pcl/segmentation/extract_clusters.h" namespace "pcl":
cdef cppclass EuclideanClusterExtraction[T]:
void setInputCloud (shared_ptr[PointCloud[T]])
void setClusterTolerance (double)
void setMinClusterSize (int)
void setMaxClusterSize (int)
void extract (vector[PointIndices])
void setSearchMethod (shared_ptr[KdTree[T]])

ctypedef EuclideanClusterExtraction[PointXYZ] EuclideanClusterExtraction_t


ctypedef aligned_allocator[PointXYZ] aligned_allocator_t
ctypedef vector2[PointXYZ, aligned_allocator_t] AlignedPointTVector_t
Expand Down
50 changes: 50 additions & 0 deletions tests/test.py
Original file line number Diff line number Diff line change
Expand Up @@ -425,3 +425,53 @@ def testRadiusSearch(self):
rs = self.t.radius_search(good_point, 0.5)
self.assertEqual(len(rs[0]), 19730)
self.assertEqual(len(rs[1]), 19730)


class TestEuclideanClusterExtractor(unittest.TestCase):

def setUp(self):
rng = np.random.RandomState(42)
# Define two dense set of points of size 30 and 100, resp
a = rng.randn(100, 3).astype(np.float32)
a[:30] -= 42

self.tolerance = 42
self.split = set([30, 70])
self.pc = pcl.PointCloud(a)
self.kd = pcl.KdTree(self.pc)
self.ec = self.pc.make_euclidean_cluster_extractor()
self.ec.set_search_method(self.kd)

def testExtract(self):
self.ec.set_cluster_tolerance(self.tolerance)
cluster_indices = self.ec.extract()
for ind in cluster_indices:
self.split.remove(len(ind))
self.assertEqual(len(self.split), 0)

def testExtractWithMinSizeOfClusters(self):
self.ec.set_cluster_tolerance(self.tolerance)
self.ec.set_min_cluster_size(50)
cluster_indices = self.ec.extract()
self.assertEqual(len(cluster_indices), 1)
self.assertEqual(len(cluster_indices[0]), 70)

def testExtractWithMaxSizeOfClusters(self):
self.ec.set_cluster_tolerance(self.tolerance)
self.ec.set_max_cluster_size(50)
cluster_indices = self.ec.extract()
self.assertEqual(len(cluster_indices), 1)
self.assertEqual(len(cluster_indices[0]), 30)

def testExtractWithDifferentTolerance(self):
self.ec.set_cluster_tolerance(0.5)
cluster_indices_half = self.ec.extract()
self.ec.set_cluster_tolerance(2)
cluster_indices_two = self.ec.extract()
self.assertGreaterEqual(
len(cluster_indices_half), len(cluster_indices_two)
)


if __name__ == '__main__':
unittest.main()