def ransac_render(cloud): pointCloud = vpc.VtkPointCloud() model_p = pcl.SampleConsensusModelPlane(cloud) ransac = pcl.RandomSampleConsensus(model_p) ransac.set_DistanceThreshold(1) ransac.computeModel() plane = ransac.get_Inliers() print(len(plane)) find_plane = 0 ## index of inlier inside cloud cnt = 0 for i in range(0, cloud.size): plane_index = plane[find_plane] if plane_index == i: if find_plane < len(plane) - 1: find_plane += 1 else: pointCloud.addPoint(cloud[i], -1) cnt += 1 print(cnt) print(cloud.size) renderer = vtk.vtkRenderer() renderWindow = vtk.vtkRenderWindow() renderWindow.AddRenderer(renderer) renderWindowInteractor = vtk.vtkRenderWindowInteractor() renderWindowInteractor.SetRenderWindow(renderWindow) renderer.AddActor(pointCloud.point_vtkActor) renderer.SetBackground(0.0, 0.0, 0.0) renderWindow.Render() renderWindowInteractor.Initialize() renderWindowInteractor.Start()
def test_SampleConsensusModelLine(self): model_line = pcl.SampleConsensusModelLine(self.p) ransac = pcl.RandomSampleConsensus(model_line) ransac.set_DistanceThreshold(.01) ransac.computeModel() inliers = ransac.get_Inliers() # print(str(len(inliers))) self.assertNotEqual(len(inliers), 0)
def ransac(self, point_cloud): model = pcl.SampleConsensusModelLine(point_cloud) ransac = pcl.RandomSampleConsensus(model) ransac.set_DistanceThreshold(0.01) ransac.computeModel() inliers = ransac.get_Inliers() len_inliers = len(inliers) if len_inliers != 0: finalpoints = np.zeros((len_inliers, 3), dtype=np.float32) for i in range(0, len_inliers): finalpoints[i][0] = point_cloud[inliers[i]][0] finalpoints[i][1] = point_cloud[inliers[i]][1] finalpoints[i][2] = point_cloud[inliers[i]][2] final = pcl.PointCloud() final.from_array(finalpoints) return final
def ransac(cloud): model_p = pcl.SampleConsensusModelPlane(cloud) ransac = pcl.RandomSampleConsensus(model_p) ransac.set_DistanceThreshold(0.1) ransac.computeModel() plane = ransac.get_Inliers() final = [] find_plane = 0 ## index of inlier inside cloud for i in range(0, cloud.size): plane_index = plane[find_plane] if plane_index == i: if find_plane < len(plane) - 1: find_plane += 1 else: final.append(cloud[i]) return pcl.PointCloud(final)
def register_ransac(cam_points, log_points): cam_pc = pcl.PointCloud(cam_points.astype(np.float32)) log_pc = pcl.PointCloud(log_points.astype(np.float32)) model = pcl.SampleConsensusModelRegistration(cam_pc) ransac = pcl.RandomSampleConsensus(model) ransac.set_DistanceThreshold(0.01) ransac.computeModel() inliers = ransac.get_Inliers() if len(inliers) != 0: finalpoints = np.zeros((len(inliers), 3), dtype=np.float32) for i in range(0, len(inliers)): finalpoints[i][0] = cloud[inliers[i]][0] finalpoints[i][1] = cloud[inliers[i]][1] finalpoints[i][2] = cloud[inliers[i]][2] final.from_array(finalpoints)
def test_ModelSphere(self): model_s = pcl.SampleConsensusModelSphere(self.p) ransac = pcl.RandomSampleConsensus(model_s) ransac.set_DistanceThreshold(.01) ransac.computeModel() inliers = ransac.get_Inliers() # print(str(len(inliers))) self.assertNotEqual(len(inliers), 0) final = pcl.PointCloud() if len(inliers) != 0: finalpoints = np.zeros((len(inliers), 3), dtype=np.float32) for i in range(0, len(inliers)): finalpoints[i][0] = self.p[inliers[i]][0] finalpoints[i][1] = self.p[inliers[i]][1] finalpoints[i][2] = self.p[inliers[i]][2] final.from_array(finalpoints) self.assertNotEqual(final.size, 0) pass
def ModelPlane(cloud_filter, threshold=0.1, ext=True): modle_p = pcl.SampleConsensusModelPlane(cloud_filter) ransac = pcl.RandomSampleConsensus(modle_p) ransac.set_DistanceThreshold(threshold) ransac.computeModel() # ransac.setNegative(True) inliers = ransac.get_Inliers() # final.extract() final = pcl.PointCloud() finalpoints = np.zeros((len(inliers), 3), dtype=np.float32) for i in range(0, len(inliers)): finalpoints[i][0] = cloud_filter[inliers[i]][0] finalpoints[i][1] = cloud_filter[inliers[i]][1] finalpoints[i][2] = cloud_filter[inliers[i]][2] # finalpoints[1][0] = cloud_filter[inliers[1]][0] # finalpoints[1][1] = cloud_filter[inliers[1]][1] # finalpoints[1][2] = cloud_filter[inliers[1]][2] final = pcl.PointCloud() final.from_array(finalpoints) if ext == True: return cloud_filter.extract(inliers, True) else: return final
def extract_points_3D(self, pointcloud_msg, now, proc_results, calibrator_instance): # print("lidar_points:", len(lidar_points), ":", [x.shape for x in lidar_points]) # Log PID rospy.loginfo('3D Picker PID: [%d]' % os.getpid()) # Extract points data points = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud_msg) points = np.asarray(points.tolist()) if (len(points.shape)>2): points = points.reshape(-1, points.shape[-1]) points = points[~np.isnan(points).any(axis=1), :] points2pcd(points, os.path.join(PKG_PATH, CALIB_PATH, self.camera_name+"_pcd", str(self.lidar_points_num)+".pcd")) if (self.args.transform_pointcloud): points = (self.lidar_rotation_matrix.dot(points[:,:3].T) + self.lidar_translation_vector.reshape(-1,1)).T # Select points within chessboard range inrange = np.where((np.abs(points[:, 0]) < 7) & (points[:, 1] < 25) & (points[:, 1] > 0) & # (points[:, 2] < 4) & (points[:, 2] > 0.2)) points = points[inrange[0]] if points.shape[0] < 10: rospy.logwarn('Too few PCL points available in range') return # pptk viewer viewer = pptk.viewer(points[:, :3]) viewer.set(lookat=(0,0,4)) viewer.set(phi=-1.57) viewer.set(theta=0.4) viewer.set(r=4) viewer.set(floor_level=0) viewer.set(point_size=0.02) rospy.loginfo("Press [Ctrl + LeftMouseClick] to select a chessboard point. Press [Enter] on viewer to finish selection.") pcl_pointcloud = pcl.PointCloud(points[:,:3].astype(np.float32)) region_growing = pcl_pointcloud.make_RegionGrowing(searchRadius=self.chessboard_diagonal/5.0) indices = [] viewer.wait() indices = viewer.get('selected') if (len(indices) < 1): rospy.logwarn("No point selected!") # self.terminate_all() viewer.close() return rg_indices = region_growing.get_SegmentFromPoint(indices[0]) points_color = np.zeros(len(points)) points_color[rg_indices] = 1 viewer.attributes(points_color) # viewer.wait() chessboard_pointcloud = pcl_pointcloud.extract(rg_indices) plane_model = pcl.SampleConsensusModelPlane(chessboard_pointcloud) pcl_RANSAC = pcl.RandomSampleConsensus(plane_model) pcl_RANSAC.set_DistanceThreshold(0.01) pcl_RANSAC.computeModel() inliers = pcl_RANSAC.get_Inliers() # random select a fixed number of lidar points to reduce computation time inliers = np.random.choice(inliers, self.args.lidar_points) chessboard_points = np.asarray(chessboard_pointcloud) proc_results.put([np.asarray(chessboard_points[inliers, :3])]) points_color = np.zeros(len(chessboard_points)) points_color[inliers] = 1 viewer.load(chessboard_points[:,:3]) viewer.set(lookat=(0,0,4)) viewer.set(phi=-1.57) viewer.set(theta=0.4) viewer.set(r=2) viewer.set(floor_level=0) viewer.set(point_size=0.02) viewer.attributes(points_color) viewer.attributes(points_color) rospy.loginfo("Check the chessboard segmentation in the viewer.") viewer.wait() viewer.close()
# ransac.getInliers(inliers); # } # else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 ) # { # pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s); # ransac.setDistanceThreshold (.01); # ransac.computeModel(); # ransac.getInliers(inliers); # } ### # inliers = vector[int] model_s = pcl.SampleConsensusModelSphere(cloud) model_p = pcl.SampleConsensusModelPlane(cloud) if argc > 1: if argvs == "-f": ransac = pcl.RandomSampleConsensus(model_p) ransac.set_DistanceThreshold(.01) ransac.computeModel() inliers = ransac.get_Inliers() elif argvs == "-sf": ransac = pcl.RandomSampleConsensus(model_s) ransac.set_DistanceThreshold(.01) ransac.computeModel() inliers = ransac.get_Inliers() else: inliers = [] else: inliers = [] # // copies all inliers of the model computed to another PointCloud # pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
def extract_points_3D(self, pcd_file, proc_results): # print("lidar_points:", len(lidar_points), ":", [x.shape for x in lidar_points]) # Log PID rospy.loginfo('3D Picker PID: [%d] file name: %s' % (os.getpid(), os.path.basename(pcd_file))) # Extract points data points = pcl.load(pcd_file).to_array() # Select points within chessboard range inrange = np.where((np.abs(points[:, 0]) < 8) & (points[:, 1] < 25) & (points[:, 1] > 0) & # (points[:, 2] < 4) & (points[:, 2] > 0.2)) points = points[inrange[0]] if points.shape[0] < 10: rospy.logwarn('Too few PCL points available in range') return # pptk viewer viewer = pptk.viewer(points[:, :3]) viewer.set(lookat=(0, 0, 4)) viewer.set(phi=-1.57) viewer.set(theta=0.4) viewer.set(r=4) viewer.set(floor_level=0) viewer.set(point_size=0.02) rospy.loginfo( "Press [Ctrl + LeftMouseClick] to select a chessboard point. Press [Enter] on viewer to finish selection." ) pcl_pointcloud = pcl.PointCloud(points[:, :3].astype(np.float32)) region_growing = pcl_pointcloud.make_RegionGrowing( searchRadius=self.chessboard_diagonal / 5.0) indices = [] viewer.wait() indices = viewer.get('selected') if (len(indices) < 1): rospy.logwarn("No point selected! Skip this frame.") viewer.close() return rg_indices = region_growing.get_SegmentFromPoint(indices[0]) points_color = np.zeros(len(points)) points_color[rg_indices] = 1 viewer.attributes(points_color) # viewer.wait() chessboard_pointcloud = pcl_pointcloud.extract(rg_indices) plane_model = pcl.SampleConsensusModelPlane(chessboard_pointcloud) pcl_RANSAC = pcl.RandomSampleConsensus(plane_model) pcl_RANSAC.set_DistanceThreshold(0.01) pcl_RANSAC.computeModel() inliers = pcl_RANSAC.get_Inliers() inliers = np.random.choice(inliers, self.args.lidar_points) chessboard_points = np.asarray(chessboard_pointcloud) proc_results.put([np.asarray(chessboard_points[inliers, :3])]) points_color = np.zeros(len(chessboard_points)) points_color[inliers] = 1 viewer.load(chessboard_points[:, :3]) viewer.set(lookat=(0, 0, 4)) viewer.set(phi=-1.57) viewer.set(theta=0.4) viewer.set(r=2) viewer.set(floor_level=0) viewer.set(point_size=0.02) viewer.attributes(points_color) rospy.loginfo("Check the chessboard segmentation in the viewer.") viewer.wait() viewer.close()
def calibrate(points3D=None): # Load corresponding points folder = os.path.join(PKG_PATH, CALIB_PATH) if points3D is None and os.path.exists( os.path.join(folder, 'lidar_points.txt')): points3D = np.loadtxt(os.path.join(folder, 'lidar_points.txt')) # print("points3D:", points3D.shape) # Estimate extrinsics # best-fit linear plane # method 1: PCA method # pca = PCA() # pca.fit(points3D[:,:3]) # normal_vector = pca.components_[2] # method 2: lstsq method # A = np.c_[points3D[:,0], points3D[:,1], np.ones(points3D.shape[0])] # C,_,_,_ = scipy.linalg.lstsq(A, points3D[:,2]) # coefficients # # evaluate it on grid # # Z = C[0]*X + C[1]*Y + C[2] # # or expressed using matrix/vector product # #Z = np.dot(np.c_[XX, YY, np.ones(XX.shape)], C).reshape(X.shape) # # normal vector # # print("C:", C) # normal_vector = np.array([C[0], C[1], -1]) # normal_vector = np.array(normal_vector) / np.linalg.norm(normal_vector) # translation_vector = np.array([[0],[0],[C[2]]]) # shpae: (3,1) # method 3: PCL RANSAC fit plane pcl_pointcloud = pcl.PointCloud(points3D[:, :3].astype(np.float32)) plane_model = pcl.SampleConsensusModelPlane(pcl_pointcloud) pcl_RANSAC = pcl.RandomSampleConsensus(plane_model) pcl_RANSAC.set_DistanceThreshold(0.03) pcl_RANSAC.computeModel() inliers = pcl_RANSAC.get_Inliers() points3D = points3D[inliers, :3] pca = PCA() pca.fit(points3D) normal_vector = pca.components_[2] translation_vector = np.zeros((3, 1)) if (normal_vector[2] < 0): normal_vector = -normal_vector # print("normal vector:", normal_vector) # rotation_angle = np.arccos(normal_vector[2]) # rotation_vector_temp = np.cross(normal_vector, np.array([0,0,1])) # rotation_vector = rotation_angle * rotation_vector_temp / np.linalg.norm(rotation_vector_temp) # Convert rotation vector rotation_matrix = rotation_matrix_from_vectors(normal_vector, np.array([0, 0, 1])) euler = euler_from_matrix(rotation_matrix) # use reverse transform because of issues in the BIT-IVRC sensor_driver package reverse_euler_angles = np.zeros((1, 3)) reverse_rotation_matrix = rotation_matrix_from_vectors( np.array([0, 0, 1]), normal_vector) reverse_euler_angles = euler_from_matrix(reverse_rotation_matrix) alfa, beta, gama = reverse_euler_angles alfa = -alfa # x_offset, y_offset, z_offset = C transform_matrix_calibration = np.zeros((3, 3)) transform_matrix_calibration[0, 0] = np.cos(beta) * np.cos(gama) - np.sin( beta) * np.sin(alfa) * np.sin(gama) transform_matrix_calibration[1, 0] = np.cos(beta) * np.sin(gama) + np.sin( beta) * np.sin(alfa) * np.cos(gama) transform_matrix_calibration[2, 0] = -np.cos(alfa) * np.sin(beta) transform_matrix_calibration[0, 1] = -np.cos(alfa) * np.sin(gama) transform_matrix_calibration[1, 1] = np.cos(alfa) * np.cos(gama) transform_matrix_calibration[2, 1] = np.sin(alfa) transform_matrix_calibration[0, 2] = np.sin(beta) * np.cos(gama) + np.cos( beta) * np.sin(alfa) * np.sin(gama) transform_matrix_calibration[1, 2] = np.sin(beta) * np.sin(gama) - np.cos( beta) * np.sin(alfa) * np.cos(gama) transform_matrix_calibration[2, 2] = np.cos(alfa) * np.cos(beta) # points3D = np.matmul(points3D[:,:3], transform_matrix_calibration) points3D = transform_matrix_calibration.dot(points3D[:, :3].T).T z_mean = np.mean(points3D[:, 2]) # print("Z statitics:", z_mean, np.std(points3D[:,2])) # viewer = pptk.viewer(points3D[:,:3]) # viewer.wait() translation_vector[2] = -z_mean rospy.loginfo( "The following values are used for tf static_transform_publisher") # print("rotation_matrix * normal_vector:", np.dot(rotation_matrix, normal_vector)) # print("rotation_matrix * [0,0,1]:", np.dot(rotation_matrix, np.array([0,0,1]))) # Display results print('Euler angles (RPY):', euler) print('Rotation Matrix:', rotation_matrix) print('Translation Offsets:', translation_vector) # Save extrinsics if (not os.path.exists(folder)): os.makedirs(folder) np.savez(os.path.join(folder, args.lidar_name + '_extrinsics.npz'), euler=euler, R=rotation_matrix, T=translation_vector) # with open(os.path.join(folder, args.lidar_name + '_extrinsics.yaml'),'w') as f: # yaml.dump({'euler':list(euler), 'R':rotation_matrix.tolist(), 'T':translation_vector.tolist()},f) calibration_string = lidarCalibrationYamlBuf(rotation_matrix, translation_vector, euler, name=args.lidar_name) if (not os.path.exists(folder)): os.mkdir(folder) with open(os.path.join(folder, args.lidar_name + '_extrinsics.yaml'), 'w') as f: f.write(calibration_string) reverse_euler_angles = np.array(reverse_euler_angles) reverse_euler_angles *= 180. / np.pi rospy.loginfo( "Calibration finished. Copy the following offsets and angles to the lua file in sensor_driver package." ) rospy.loginfo( "x_offset: %f, y_offset: %f, z_offset: %f" % (translation_vector[0], translation_vector[1], translation_vector[2])) rospy.loginfo("x_angle: %f, y_angle: %f, z_angle: %f" % (-reverse_euler_angles[0], reverse_euler_angles[1], reverse_euler_angles[2])) return reverse_rotation_matrix, transform_matrix_calibration, translation_vector