def do_euclidian_clustering(cloud):
# Euclidean Clustering
white_cloud = pcl_helper.XYZRGB_to_XYZ(cloud)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(5)
ec.set_MinClusterSize(1)
ec.set_MaxClusterSize(3500)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
cluster_color = pcl_helper.random_color_gen()
#cluster_color = pcl_helper.get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([
white_cloud[indice][0], white_cloud[indice][1],
white_cloud[indice][2],
pcl_helper.rgb_to_float(cluster_color)
])
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
ros_cluster_cloud = pcl_helper.pcl_to_ros(cluster_cloud)
return cluster_cloud
def PCLProc_ExtractClusters(pclpObjectsIn):
kdTreeCluster = pclpObjectsIn.make_kdtree()
# Create a cluster extraction object
clusterExtractor = pclpObjectsIn.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold & clusterSize min,max (in points)
clusterExtractor.set_ClusterTolerance(0.015)
clusterExtractor.set_MinClusterSize(120)
clusterExtractor.set_MaxClusterSize(1500)
# Search the k-d tree for clusters
clusterExtractor.set_SearchMethod(kdTreeCluster)
# Extract indices for each of the discovered clusters
clusterIndices = clusterExtractor.Extract()
# Assign a color corresponding to each segmented object in scene
clusterColor = pcl_helper.get_color_list(len(clusterIndices))
clusterColorPointList = []
for j, indices in enumerate(clusterIndices):
for i, indice in enumerate(indices):
clusterColorPointList.append([
pclpObjectsIn[indice][0], pclpObjectsIn[indice][1],
pclpObjectsIn[indice][2],
pcl_helper.rgb_to_float(clusterColor[j])
])
# Create new cloud containing all clusters, each with unique color
pclpcClusters = pcl.PointCloud_PointXYZRGB()
pclpcClusters.from_list(clusterColorPointList)
return clusterIndices, pclpcClusters
def do_euclidian_clustering(cloud):
# Euclidean Clustering
white_cloud = pcl_helper.XYZRGB_to_XYZ(
cloud) # <type 'pcl._pcl.PointCloud'>
tree = white_cloud.make_kdtree() # <type 'pcl._pcl.KdTree'>
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02) # for hammer
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(250)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract(
) # indices for each cluster (a list of lists)
# Assign a color to each cluster
cluster_color = pcl_helper.random_color_gen()
#cluster_color = pcl_helper.get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([
white_cloud[indice][0], white_cloud[indice][1],
white_cloud[indice][2],
pcl_helper.rgb_to_float(cluster_color)
])
# Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# publish to cloud
ros_cluster_cloud = pcl_helper.pcl_to_ros(cluster_cloud)
# save to local
pcl.save(cluster_cloud, 'cluster.pcd')
return cluster_cloud
def visualize_clusters(cloud, cluster_indices):
#cloud_xyz = pclh.XYZRGB_to_XYZ(cloud)
#Assign a color corresponding to each segmented object in scene
cluster_color = pclh.get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
color = pclh.rgb_to_float(cluster_color[j])
for idx in indices:
point = cloud[idx]
color_cluster_point_list.append([point[0], point[1], point[2], color])
#Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
return cluster_cloud
def do_euclidian_clustering(self, cloud):
# Euclidean Clustering
white_cloud = pcl_helper.XYZRGB_to_XYZ(cloud)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.3) #0.14, 0.08
ec.set_MinClusterSize(1)
ec.set_MaxClusterSize(3500)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
cluster_color = pcl_helper.random_color_gen()
#cluster_color = pcl_helper.get_color_list(len(cluster_indices))
#print("No. of cones visible at this moment ", len(cluster_indices))
#print(cluster_indices)
cluster_coords = Track()
#cone = TrackCone()
#cluster_coords.cones = []
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
x, y, z = 0, 0, 0
cone_clusters = []
for i, indice in enumerate(indices):
color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], pcl_helper.rgb_to_float(cluster_color)])
x = x + white_cloud[indice][0]
y = y + white_cloud[indice][1]
z = z + white_cloud[indice][2]
cone_clusters.append(x/len(indices) + self.carPosX)
cone_clusters.append(y/len(indices) + self.carPosY)
print(cone_clusters)
#print(color_cluster_point_list)
#cone.x = x/len(indices)
#cone.y = y/len(indices)
#cone.type = f'cone{j+1}'
#cluster_coords.cones = cone_clusters
#cluster_coords.data.append(TrackCone(data = cone_clusters))
cluster_coords.data.append(TrackCone(x = x/len(indices) + self.carPosX,
y = y/len(indices) + self.carPosY))
#cluster_coords.cones[j].x = x/len(indices)
#cluster_coords.cones[j].y = y/len(indices)
#cluster_coords.cones[j].type = 'cone{k}'
#print(len(cluster_coords.cones))
#print(len(cluster_coords.cones[0]))
#print("No. of cones visible at this moment ", len(color_cluster_point_list))
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
ros_cluster_cloud = pcl_helper.pcl_to_ros(cluster_cloud)
return cluster_cloud, cluster_coords
with open(txt_file, 'r') as f:
lines = f.readlines()
pc_list = []
color_list = []
pc_xyzrgb_list = []
for line in lines:
pc_xyzrgb = line.strip().split(',')
pc_xyz = [float(pc_xyzrgb[0]), float(pc_xyzrgb[1]), float(pc_xyzrgb[2])]
color = [int(pc_xyzrgb[3]), int(pc_xyzrgb[4]), int(pc_xyzrgb[5])]
#pc_list.append(pc_xyz)
#color_list.append(color)
float_color = pcl_helper.rgb_to_float(color)
pc_xyzrgb_list.append([pc_xyz[0], pc_xyz[1], pc_xyz[2], float_color])
#np_pc_list = np.asarray(pc_list)
#np_color = np.asarray(color_list)
#msg_pc = pcl_helper.xyzrgb_array_to_pointcloud2(points=np_pc_list, colors=np_color, stamp=time, frame_id='map')
XYZRGB_cloud = pcl.PointCloud_PointXYZRGB()
XYZRGB_cloud.from_list(pc_xyzrgb_list)
msg_pc = pcl_helper.pcl_to_ros(XYZRGB_cloud)
time = rospy.Time.now()
write_rosbag.write(topic='/soslab_sl/pointcloud', msg=msg_pc, t=time)
r.sleep()
def pcl_callback(pcl_msg):
# Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# create a voxelgrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()
# Choose a voxel (also known as leaf) size
LEAF_SIZE = 0.01
# set the voxel (or leaf)size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
#filename = 'voxel_downsampled.pcd'
#pcl.save(cloud_filtered, filename)
# PassThrough filter
# create pass through filter object
passthrough = cloud_filtered.make_passthrough_filter()
# assign axis and ranve to the passthrough filter object
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
# generate the resultant point cloud
cloud_filtered = passthrough.filter()
# RANSAC plane segmentation
# create the segmentation object
seg = cloud_filtered.make_segmenter()
# set the model to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered fitting the model
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
# Extract table
cloud_table = cloud_filtered.extract(inliers, negative=False)
#filename = 'cloud_table.pcd'
#pcl.save(cloud_table, filename)
# Extract objects
#cloud_objects = cloud_filtered.extract(inliers, negative=True)
# Extract outliers
# create the filter objectd
objects = cloud_filtered.extract(inliers, negative=True)
outlier_filter = objects.make_statistical_outlier_filter()
# set the number of points to analyze for any given point
outlier_filter.set_mean_k(50)
# set threshold scale factor
x = 1.0
# any point with a mena distance large than the global (mean distance + x * std_dev)
# will be considered outlier
outlier_filter.set_std_dev_mul_thresh(x)
# call the filter function
cloud_objects = outlier_filter.filter()
filename = 'cloud_objects.pcd'
pcl.save(cloud_objects, filename)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
# create cluster extraction
ec = white_cloud.make_EuclideanClusterExtraction()
# set the tolerances for
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(100)
ec.set_MaxClusterSize(2000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# asign colors to the individual point cloud indices for visualization
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([
white_cloud[indice][0], white_cloud[indice][1],
white_cloud[indice][2],
rgb_to_float(cluster_color[j])
])
# create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# TODO: Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)