def main(): # // Load input file into a PointCloud<T> with an appropriate type # pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ()); # // Load bun0.pcd -- should be available with the PCL archive in test # pcl::io::loadPCDFile ("bun0.pcd", *cloud); points = np.load("sample.npy").transpose(1, 0) filtered_neighbor_list, coordinate = MLS(points) cloud = pcl.load('pcd/origin.pcd') print('cloud(size) = ' + str(cloud.size)) # // Create a KD-Tree # pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); start = time() tree = cloud.make_kdtree() # tree = cloud.make_kdtree_flann() # blankCloud = pcl.PointCloud() # tree = blankCloud.make_kdtree() # // Output has the PointNormal type in order to store the normals calculated by MLS # pcl::PointCloud<pcl::PointNormal> mls_points; # mls_points = pcl.PointCloudNormal() # // Init object (second point type is for the normals, even if unused) # pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls; # mls.setComputeNormals (true); # # // Set parameters # mls.setInputCloud (cloud); # mls.setPolynomialFit (true); # mls.setSearchMethod (tree); # mls.setSearchRadius (0.03); # # // Reconstruct # mls.process (mls_points); mls = cloud.make_moving_least_squares() # print('make_moving_least_squares') mls.set_Compute_Normals(True) mls.set_polynomial_fit(True) mls.set_polynomial_order(3) mls.set_Search_Method(tree) mls.set_search_radius(0.1) print('set parameters') mls_points = mls.process() projected = np.asarray(mls_points) points2pcd(projected, "projected_mls") print("standard time is: ", time() - start) # Save output # pcl::io::savePCDFile ("bun0-mls.pcd", mls_points); pcl.save_PointNormal(mls_points, 'bun0-mls.pcd')
def main(): # // Load input file into a PointCloud<T> with an appropriate type # pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ()); # // Load bun0.pcd -- should be available with the PCL archive in test # pcl::io::loadPCDFile ("bun0.pcd", *cloud); cloud = pcl.load('./examples/official/Surface/bun0.pcd') print('cloud(size) = ' + str(cloud.size)) # // Create a KD-Tree # pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree = cloud.make_kdtree() # tree = cloud.make_kdtree_flann() # blankCloud = pcl.PointCloud() # tree = blankCloud.make_kdtree() # // Output has the PointNormal type in order to store the normals calculated by MLS # pcl::PointCloud<pcl::PointNormal> mls_points; # mls_points = pcl.PointCloudNormal() # // Init object (second point type is for the normals, even if unused) # pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls; # mls.setComputeNormals (true); # # // Set parameters # mls.setInputCloud (cloud); # mls.setPolynomialFit (true); # mls.setSearchMethod (tree); # mls.setSearchRadius (0.03); # # // Reconstruct # mls.process (mls_points); mls = cloud.make_moving_least_squares() # print('make_moving_least_squares') mls.set_Compute_Normals(True) mls.set_polynomial_fit(True) mls.set_Search_Method(tree) mls.set_search_radius(0.03) print('set parameters') mls_points = mls.process() # Save output # pcl::io::savePCDFile ("bun0-mls.pcd", mls_points); pcl.save_PointNormal(mls_points, 'bun0-mls.pcd')
# blankCloud = pcl.PointCloud() # tree = blankCloud.make_kdtree() # // Output has the PointNormal type in order to store the normals calculated by MLS # pcl::PointCloud<pcl::PointNormal> mls_points; # mls_points = pcl.PointCloudNormal() # // Init object (second point type is for the normals, even if unused) # pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls; # mls.setComputeNormals (true); # # // Set parameters # mls.setInputCloud (cloud); # mls.setPolynomialFit (true); # mls.setSearchMethod (tree); # mls.setSearchRadius (0.03); # # // Reconstruct # mls.process (mls_points); mls = cloud.make_moving_least_squares() # print('make_moving_least_squares') mls.set_Compute_Normals (True) mls.set_polynomial_fit (True) mls.set_Search_Method (tree) mls.set_search_radius (0.03) print('set parameters') mls_points = mls.process () # Save output # pcl::io::savePCDFile ("bun0-mls.pcd", mls_points); pcl.save_PointNormal(mls_points, 'bun0-mls.pcd')
def main(): # File i/o -- Folder name if (len(sys.argv) > 1): directory = sys.argv[1] else: print("Need a dirName argument... Exiting") exit() n_clouds = len([ len(r) for r in os.listdir(directory) if os.path.isfile(os.path.join(directory, r)) ]) # file handling if not os.path.exists("{}/smooth".format(directory)): print("Made Directory: {}/smooth".format(directory)) os.mkdir("{}/smooth".format(directory)) else: shutil.rmtree("{}/smooth".format(directory)) os.makedirs("{}/smooth".format(directory)) print("Re-made Directory: {}/smooth".format(directory)) print("Clouds to process: {}\n".format(n_clouds)) # process each cloud for x in range(n_clouds): f_name = "{}/inliers/{}.pcd".format(directory, x + 1) cloud = pcl.load(f_name) print('cloud(size) = ' + str(cloud.size)) tree = cloud.make_kdtree() mls = cloud.make_moving_least_squares() mls.set_Compute_Normals(True) mls.set_polynomial_fit(True) mls.set_Search_Method(tree) mls.set_search_radius(0.03) print('set parameters & processing...') mls_points = mls.process() print('cloud(size) = ' + str(mls_points.size)) f_name2 = "{}/smooth/{}.pcd".format(directory, x + 1) pcl.save_PointNormal(mls_points, f_name2) print("Processed and saved!\n") # display for last one if (x + 1 == n_clouds): normals = pcl.load(f_name2) viewer = pcl.pcl_visualization.PCLVisualizering('CLOUD') viewer2 = pcl.pcl_visualization.PCLVisualizering('RESAMPLED') pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom( cloud, 255, 255, 255) kpcolor = pcl.pcl_visualization.PointCloudColorHandleringCustom( normals, 255, 255, 255) viewer.AddPointCloud_ColorHandler(cloud, pccolor) viewer2.AddPointCloud_ColorHandler(normals, kpcolor, b'normals') v = True while v: v = not (viewer.WasStopped()) viewer.SpinOnce() v2 = not (viewer.WasStopped()) viewer2.SpinOnce()
def cal_grasp(msg, cam_pos_): """ 抓取姿态生成 :param msg: ROS点云消息 :param cam_pos_: 摄像头姿态 :return: """ points_ = pointclouds.pointcloud2_to_xyz_array(msg) points_ = points_.astype(np.float32) remove_white = False if remove_white: points_ = remove_white_pixel(msg, points_, vis=True) # begin voxel points n = n_voxel # parameter related to voxel method # gpg improvements, highlights: flexible n parameter for voxelizing. points_[:, 0] = points_[:, 0] + 0.025 # liang: as the kinect2 is not well calibrated, here is a work around points_[:, 2] = points_[:, 2] # + 0.018 # liang: as the kinect2 is not well calibrated, here is a work around points_voxel_ = get_voxel_fun(points_, n) # point cloud down sample # 点数过少, 调整降采样参数 if len(points_) < 2000: # should be a parameter while len(points_voxel_) < len(points_)-15: points_voxel_ = get_voxel_fun(points_, n) n = n + 100 rospy.loginfo("the voxel has {} points, we want get {} points".format(len(points_voxel_), len(points_))) rospy.loginfo("the voxel has {} points, we want get {} points".format(len(points_voxel_), len(points_))) points_ = points_voxel_ # 降采样之后的点云 remove_points = False if remove_points: points_ = remove_table_points(points_, vis=True) point_cloud = pcl.PointCloud(points_) # 传入pcl处理 # 计算表面法线 norm = point_cloud.make_NormalEstimation() pcl.save_PointNormal() norm.set_KSearch(30) # critical parameter when calculating the norms normals = norm.compute() surface_normal = normals.to_array() surface_normal = surface_normal[:, 0:3] # 处理法线方向 vector_p2cam = cam_pos_ - points_ vector_p2cam = vector_p2cam / np.linalg.norm(vector_p2cam, axis=1).reshape(-1, 1) tmp = np.dot(vector_p2cam, surface_normal.T).diagonal() angel = np.arccos(np.clip(tmp, -1.0, 1.0)) wrong_dir_norm = np.where(angel > np.pi * 0.5)[0] tmp = np.ones([len(angel), 3]) tmp[wrong_dir_norm, :] = -1 surface_normal = surface_normal * tmp select_point_above_table = 0.010 # modify of gpg: make it as a parameter. avoid select points near the table. points_for_sample = points_[np.where(points_[:, 2] > select_point_above_table)[0]] if len(points_for_sample) == 0: rospy.loginfo("Can not select point, maybe the point cloud is too low?") return [], points_, surface_normal yaml_config['metrics']['robust_ferrari_canny']['friction_coef'] = value_fc if not using_mp: rospy.loginfo("Begin cal grasps using single thread, slow!") grasps_together_ = ags.sample_grasps(point_cloud, points_for_sample, surface_normal, num_grasps, max_num_samples=max_num_samples, show_final_grasp=show_final_grasp) else: # begin parallel grasp: rospy.loginfo("Begin cal grasps using parallel!") def grasp_task(num_grasps_, ags_, queue_): ret = ags_.sample_grasps(point_cloud, points_for_sample, surface_normal, num_grasps_, max_num_samples=max_num_samples, show_final_grasp=show_final_grasp) queue_.put(ret) queue = mp.Queue() num_grasps_p_worker = int(num_grasps/num_workers) workers = [mp.Process(target=grasp_task, args=(num_grasps_p_worker, ags, queue)) for _ in range(num_workers)] [i.start() for i in workers] grasps_together_ = [] for i in range(num_workers): grasps_together_ = grasps_together_ + queue.get() rospy.loginfo("Finish mp processing!") rospy.loginfo("Grasp sampler finish, generated {} grasps.".format(len(grasps_together_))) return grasps_together_, points_, surface_normal