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)