# -*- coding: utf-8 -*- from __future__ import print_function # This Code Base # http://ros-robot.blogspot.jp/2011/08/pclapi-point-cloud-library-pcl-pcl-api.html import numpy as np import pcl import random import pcl.pcl_visualization # pcl::PointCloud<pcl::PointXYZRGB> cloud; cloud = pcl.PointCloud_PointXYZRGB() # Fill in the cloud data # cloud.width = 15; # cloud.height = 10; # cloud.points.resize (cloud.width * cloud.height) # cloud.resize (np.array([15, 10], dtype=np.float)) # points = np.zeros((10, 15, 4), dtype=np.float32) points = np.zeros((150, 4), dtype=np.float32) RAND_MAX = 1.0 # Generate the data for i in range(0, 75): # set Point Plane points[i][0] = 1024 * random.random() / (RAND_MAX + 1.0) points[i][1] = 1024 * random.random() / (RAND_MAX + 1.0) points[i][2] = 0.1 * random.random() / (RAND_MAX + 1.0) points[i][3] = 255 << 16 | 255 << 8 | 255
seg.set_MaxIterations(10000) seg.set_optimize_coefficients("true") seg.set_method_type(6) inliers, coefficients = seg.segment() clc = tool0.extract(inliers, negative=False) outliers = tool0.extract(inliers, negative=True) points_list = [] for data in clc: points_list.append( [data[0], data[1], data[2], rgb_to_float([0, 255, 0])]) for data in outliers: points_list.append( [data[0], data[1], data[2], rgb_to_float([255, 0, 0])]) tool0_c = pcl.PointCloud_PointXYZRGB() tool0_c.from_list(points_list) pcl.save(tool0_c, "tool0_c%s.pcd" % (i + 1)) p_camera.append(coefficients[:3]) coefficients[3] p_camera_ = np.matrix( np.concatenate((np.array(p_camera).transpose() / 1000, np.ones([1, 4])), axis=0)) p_camera_reverse = p_camera_.getI() H = np.matmul(p_robot, p_camera_reverse) R = H[:3, :3] t = H[:3, 3]
def pcl_callback(pcl_msg): """Callback function for your Point Cloud Subscriber :param pcl_msg: ROS point cloud message """ # Convert ROS msg to PCL data (XYZRGB) cloud = ros_to_pcl(pcl_msg) # Voxel Grid Downsampling vox = cloud.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() ''' # Much like the previous filters, we start by creating a filter object: outlier_filter = cloud_filtered.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(50) # Set threshold scale factor x = 1.0 # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic cloud_filtered = outlier_filter.filter() ''' # PassThrough Filter to isolate the table and objects passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name('z') axis_min = 0.76 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # RANSAC Plane Segmentation to identify the table from the objects seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # Extract inliers (table surface) and outliers (objects) cloud_table = cloud_filtered.extract(inliers, negative=False) cloud_objects = cloud_filtered.extract(inliers, negative=True) # Convert the xyzrgb cloud to only xyz since k-d tree only needs xyz white_cloud = XYZRGB_to_XYZ(cloud_objects) # k-d tree decreases the computation of searching for neighboring points. # PCL's euclidian clustering algo only supports k-d trees tree = white_cloud.make_kdtree() # Euclidean clustering used to build individual point clouds for each # object. The Cluster-Mask point cloud allows each cluster to be # visualized separately. ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(100) ec.set_MaxClusterSize(3000) ec.set_SearchMethod(tree) # Search the k-d tree for clusters # Extract indices for each found cluster. This is a list of indices for # each cluster (list of lists) cluster_indices = ec.Extract() # Assign a random color to each isolated object in the scene cluster_color = get_color_list(len(cluster_indices)) # Store the detected objects and labels in these lists detected_objects_labels = [] detected_objects = [] color_cluster_point_list = [] # Iterate through each detected object cluster for object recognition for index, pts_list in enumerate(cluster_indices): object_cluster = [] # Create an individual cluster just for the object being processed for i, pts in enumerate(pts_list): # Retrieve cloud values for the x, y, z, rgb object object_cluster.append([cloud_objects[pts][0], cloud_objects[pts][1], cloud_objects[pts][2], cloud_objects[pts][3]]) # Retrieve cloud values for the x, y, z object, assigning a # preidentified color to all cloud values color_cluster_point_list.append([white_cloud[pts][0], white_cloud[pts][1], white_cloud[pts][2], rgb_to_float(cluster_color[index])]) # Convert list of point cloud features (x,y,z,rgb) into a point cloud pcl_cluster = pcl.PointCloud_PointXYZRGB() pcl_cluster.from_list(object_cluster) # Convert the cluster from pcl to ROS using helper function ros_cloud = pcl_to_ros(pcl_cluster) #pcl_objects_pub.publish(ros_cloud) # Extract histogram features (similar to capture_features.py) chists = compute_color_histograms(ros_cloud, nbins=64, using_hsv=True) normals = get_normals(ros_cloud) nhists = compute_normal_histograms(normals,nbins=64) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result and add it # to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cloud detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Create new cloud containing all clusters, each with a unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # Convert PCL data to ROS messages ros_cloud_cluster = pcl_to_ros(cluster_cloud) ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) # Publish ROS messages of the point clouds and detected objects pcl_objects_cloud_pub.publish(ros_cloud_cluster) pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) detected_objects_pub.publish(detected_objects) # detected object labels
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Statistical Outlier Filtering outlier_filter = cloud.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(20) # Set threshold scale factor x = 0.1 # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic cloud_outlier_filtered = outlier_filter.filter() # TODO: Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = cloud_outlier_filtered.make_voxel_grid_filter() 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 vox_filtered = vox.filter() # TODO: PassThrough Filtering # along the z-axis passthrough = vox_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.61 axis_max = 0.95 passthrough.set_filter_limits(axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. ptz_filtered = passthrough.filter() # Along the y-axis passthrough = ptz_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.4 axis_max = 0.4 passthrough.set_filter_limits(axis_min, axis_max) pt_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation # Create the segmentation object seg = pt_filtered.make_segmenter() # Set the model you wish 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 outliers (what we're interested in) cloud_objects = pt_filtered.extract(inliers, negative=True) # Euclidean Clustering # remove color data white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() # extract KD-tree from cloud # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(50) ec.set_MaxClusterSize(1000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() #Assign a color corresponding to each segmented object in scene 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_objects = pcl_to_ros(cloud_objects) ros_cluster_cloud = pcl_to_ros(cluster_cloud) ros_of = pcl_to_ros(cloud_outlier_filtered) ros_vox = pcl_to_ros(vox_filtered) ros_pt = pcl_to_ros(pt_filtered) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_objects) pcl_cluster_pub.publish(ros_cluster_cloud) pcl_of_pub.publish(ros_of) pcl_vox_pub.publish(ros_vox) pcl_pt_pub.publish(ros_pt) # Exercise-3 TODOs: detected_objects_labels = [] detected_objects = [] labeled_centroids = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function pcl_cluster_ros = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py color_bins = 32 normals_bins = 2 feature = extract_cloud_features(pcl_cluster_ros, color_bins, normals_bins) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = pcl_cluster_ros detected_objects.append(do) #rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) detected_objects_pub.publish(detected_objects) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() #################################### try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
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(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)) cone_clusters.append(y / len(indices)) #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), y=y / len(indices))) #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.data)) #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
#here we are gonna try to stich point cloud which going get from the lazer to generate full point cloud # so we are gonna load 2 pcl and stich it together import pcl from pcl import pcl_visualization import numpy as np <<<<<<< HEAD from pykdtree.kdtree import KDTree import matplotlib.pyplot as plt cloud5 = pcl.PointCloud_PointXYZRGB() ======= import matplotlib.pyplot as plt cloud5 = pcl.PointCloud_PointXYZRGB() >>>>>>> origin/master #hwe we are gonna load one point cloud cloud1 = pcl.load_XYZRGB('/media/ribin/DATA/addverb/binpicking/pcd files/object_pcd/soap_santoor/2.pcd') print(cloud1) #here we are gonna load the second point cloud cloud2 = pcl.load_XYZRGB('/media/ribin/DATA/addverb/binpicking/pcd files/object_pcd/colgate/2.pcd') array_size1 = cloud1.size array_size2 = cloud2.size array_size = array_size1 + array_size2 + 10
np_pc = np.array(list_pc, dtype=np.float32) print(np_pc.shape) # test # save numpy array to bin or pcd filename = str(lidar_t) print(filename) np_pc_bin = copy.deepcopy(np_pc) bin_path_ = save_path + '/bin' Path(bin_path_).mkdir(parents=True, exist_ok=True) bin_path = bin_path_ + "/" + filename + ".bin" np_pc_bin.astype(np.float32).tofile(bin_path) np_pc_pcd = copy.deepcopy(np_pc) pcd_path_ = save_path + '/pcd' Path(pcd_path_).mkdir(parents=True, exist_ok=True) pcd_path = pcd_path_ + "/" + filename + ".pcd" if args.is_ml == True: pc_rgb = pcl.PointCloud_PointXYZRGB() pc_rgb.from_list(test_list_pc) pcl.save(pc_rgb, pcd_path) else: pc_intensity = pcl.PointCloud_PointXYZI() pc_intensity.from_array(np_pc_pcd) pcl.save(pc_intensity, pcd_path) print('DONE')
def pcl_callback(pcl_msg): # TODO: Convert ROS msg to PCL data pcl_msg_new = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = pcl_msg_new.make_voxel_grid_filter() # 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 rsultant downsampled point cloud cloud_filtered = vox.filter() filename = 'voxel_downsampled.pcd' pcl.save(cloud_filtered, filename) # TODO: PassThrough Filter # Create a PassThrough filter object passthrough = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object filter_axis = 'z' passthrough.set_filter_field_name (filter_axis) axis_min = 0.757 axis_max = 1.1 passthrough.set_filter_limits (axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passthrough.filter() filename = 'pass_through_filtered.pcd' pcl.save(cloud_filtered, filename) # TODO: RANSAC Plane Segmentation # Create the segmentation object seg = cloud_filtered.make_segmenter() # Set the model you wish 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 # Experiment with different values for max_distance # for segmenting the table 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() # TODO: Extract table and objects cloud_table = cloud_filtered.extract(inliers, negative=False) filename = 'cloud_table.pcd' pcl.save(cloud_table, filename) cloud_objects = cloud_filtered.extract(inliers, negative=True) 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 a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(175) ec.set_MaxClusterSize(1200) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately #Assign a color corresponding to each segmented object in scene 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)
def pcl_callback(pcl_msg): # TODO: Convert ROS msg to PCL data pcl_data = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling vox = pcl_data.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() 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) cloud_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # TODO: Extract inliers and outliers cloud_table = cloud_filtered.extract(inliers, negative=False) cloud_objects = cloud_filtered.extract(inliers, negative=True) # Remove Noise - in particular the table edge from objects noise_filter = cloud_objects.make_statistical_outlier_filter() noise_filter.set_mean_k(50) x = 1.0 noise_filter.set_std_dev_mul_thresh(x) cloud_objects = noise_filter.filter() # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.04) ec.set_MinClusterSize(50) ec.set_MaxClusterSize(5000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, index in enumerate(indices): color_cluster_point_list.append([ white_cloud[index][0], white_cloud[index][1], white_cloud[index][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)
def circle_fitting(self, point_cloud_path, index=999, need_judge=True, save_interm=False): # Load point cloud from given path # FIXME: For debug # print(point_cloud_path) # cloud = pcl.load("/home/bionicdl-Mega/repos/01.ply") cloud = pcl.load(point_cloud_path) # Passthrough Filter passthrough = cloud.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 200 axis_max = 1000 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() print("Path Through for %s" % str(index)) # pcl.save(cloud_filtered, "%s.pcd"%(cloud_path+"pass"+str(index))) # Statistical Outlier Filter print("Outlier Filter :%s" % str(index)) outlier_filter = cloud_filtered.make_statistical_outlier_filter() outlier_filter.set_mean_k(50) outlier_filter.set_std_dev_mul_thresh(1.0) cloud_filtered = outlier_filter.filter() # pcl.save(cloud_filtered, "%s.pcd"%(cloud_path+"ol"+str(index))) # Euclidean Clustering print("Euclidean Clustering:%s" % str(index)) white_cloud = cloud_filtered tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.5) # Set tolerances for distance threshold ec.set_MinClusterSize(2000) ec.set_MaxClusterSize( 100000) # as well as minimum and maximum cluster size (in points) ec.set_SearchMethod(tree) cluster_indices = ec.Extract() tool_index = [] min_height = 10010 current_length = 0 # Pick out cluseter on the top for cluster in cluster_indices: cloud = white_cloud.extract(cluster) pcl.save(cloud, "/home/bionicdl-Mega/repos/test.pcd") cloud_array = np.array(cloud) length = cloud_array.shape[0] height = cloud_array[:, 2].min() if height < min_height: min_height = height tool_index = cluster tool0 = white_cloud.extract(tool_index) # pcl.save(tool0, "%s.pcd"%(cloud_path+"clustering"+str(index))) # RANSAC circle segmentation print("RANSAC :%s" % str(index)) seg = tool0.make_segmenter() seg.set_model_type(pcl.SACMODEL_CIRCLE3D) max_distance = 0.0002 seg.set_distance_threshold(max_distance) seg.set_MaxIterations(50000) seg.set_optimize_coefficients("true") seg.set_method_type(6) inliers, coefficients = seg.segment() clc = tool0.extract(inliers, negative=False) outliers = tool0.extract(inliers, negative=True) points_list = [] for data in clc: points_list.append([data[0], data[1], data[2], 0.5]) index_data = 0 for data in outliers: points_list.append([data[0], data[1], data[2], 255]) center = coefficients[:3] points_list.append([center[0], center[1], center[2], 100]) if need_judge in index: point_cloud_valid = self._feasibility_test(len(tool_index), len(inliers)) if not point_cloud_valid: return False tool0_c = pcl.PointCloud_PointXYZRGB() tool0_c.from_list(points_list) # pcl.save(tool0_c, "%s.pcd"%(cloud_path+"tool_c")) # TODO: remove temporal point cloud and rename return coefficients[:3]
def ransac_circle_fitting(self, point_cloud_path): """ Apply RANSAC circle fitting to find circle :param point_cloud_path: path of point cloud of flange :return: Coordinate of center of flange in camera frame """ p_robot_used = [] maxD = 0.3 R_FLANGE = 31.0 detR = 1 p_camera = [] # for i in range(len(index_list)): # index = index_list[i] try: # tool0 = pcl.load( # "/home/bionicdl/photoneo_data/calibration_images/data_ransac10000_valid/tool0_%s.pcd" % ( # index[:-1])) tool0 = pcl.load(point_cloud_path) except: print("PointCloud with path %s not found" % (point_cloud_path)) # p_robot_used.append(p_robot_list[i]) points = tool0.to_array() max_num_inliers = 0 # FIXME: For testing for k in range(10000): idx = np.random.randint(points.shape[0], size=3) A, B, C = points[idx, :] a = np.linalg.norm(C - B) b = np.linalg.norm(C - A) c = np.linalg.norm(B - A) s = (a + b + c) / 2 R = a * b * c / 4 / np.sqrt(s * (s - a) * (s - b) * (s - c)) if (R_FLANGE - R) > detR or R_FLANGE - R < 0: continue b1 = a * a * (b * b + c * c - a * a) b2 = b * b * (a * a + c * c - b * b) b3 = c * c * (a * a + b * b - c * c) P = np.column_stack((A, B, C)).dot(np.hstack((b1, b2, b3))) P /= b1 + b2 + b3 num_inliers = 0 inliers = [] outliers = [] for point in points: d = np.abs(np.linalg.norm(point - P) - R) if d < maxD: num_inliers += 1 inliers.append(point) else: outliers.append(point) if num_inliers > max_num_inliers: max_num_inliers = num_inliers max_A = A max_B = B max_C = C max_R = R max_P = P max_inliers = np.array(inliers) max_outliers = np.array(outliers) points_list = [] for data in max_inliers: points_list.append( [data[0], data[1], data[2], rgb_to_float([0, 255, 0])]) for data in max_outliers: points_list.append( [data[0], data[1], data[2], rgb_to_float([255, 0, 0])]) tool0_c = pcl.PointCloud_PointXYZRGB() tool0_c.from_list(points_list) output_path = point_cloud_path if "pcd" in output_path: output_path = output_path.replace(".pcd", "_valid.pcd") elif "ply" in output_path: output_path = output_path.replace(".ply", "_valid.ply") output_path = output_path.replace(".ply", ".pcd") pcl.save(tool0_c, output_path) output_path = output_path.replace(".pcd", ".ply") pcl.save(tool0_c, output_path) return np.array(max_P)