def extract_points_to_pcd_file(filename, by_selection=False, name_starts_with="", name_ends_with=""): """ Extract 3D coordinates of vertices of meshes to a PointCloud (.pcd) file. The resulting vertices are sorted by object-name. "filename" : .pcd-file to save to "by_selection", "name_starts_with", "name_ends_with" : see documentation of "get_objects()" Note: currently, colors are not exported. """ verts = [] for ob_name, ob in get_objects(by_selection, name_starts_with, name_ends_with): verts += [tuple(ob.matrix_world * vertex.co) for vertex in ob.data.vertices] verts = np.array(verts) dataset_tools.save_3D_points_to_pcd_file(filename, verts)
def main(): traj_to_file, traj_from_file, traj_input_files, map_input_files, at_frame, offset_time = parse_cmd_args( ) print("Calculating transformation...") cam_trajectory_from = dataset_tools.load_cam_trajectory_TUM(traj_from_file) cam_trajectory_to = dataset_tools.load_cam_trajectory_TUM(traj_to_file) transformation = dataset_tools.transform_between_cam_trajectories( cam_trajectory_from, cam_trajectory_to, at_frame=at_frame, offset_time=offset_time) print("Results:") delta_quaternion, delta_scale, delta_location = transformation print("delta_quaternion:") print("\t %s" % delta_quaternion) print("delta_scale:") print("\t %s" % delta_scale) print("delta_location:") print("\t %s" % delta_location) print() for traj_input_file in traj_input_files: print('Transforming traj "%s"...' % traj_input_file) dataset_tools.save_cam_trajectory_TUM( "%s-trfm%s" % tuple(os.path.splitext(traj_input_file)), dataset_tools.transformed_cam_trajectory( dataset_tools.load_cam_trajectory_TUM(traj_input_file), transformation)) for map_input_file in map_input_files: print('Transforming map "%s"...' % map_input_file) points, colors, _ = dataset_tools.load_3D_points_from_pcd_file( map_input_file, use_alpha=True) dataset_tools.save_3D_points_to_pcd_file( "%s-trfm%s" % tuple(os.path.splitext(map_input_file)), dataset_tools.transformed_points(points, transformation), colors) print("Done.")
def extract_points_to_pcd_file(filename, by_selection=False, name_starts_with="", name_ends_with=""): """ Extract 3D coordinates of vertices of meshes to a PointCloud (.pcd) file. The resulting vertices are sorted by object-name. "filename" : .pcd-file to save to "by_selection", "name_starts_with", "name_ends_with" : see documentation of "get_objects()" Note: currently, colors are not exported. """ verts = [] for ob_name, ob in get_objects(by_selection, name_starts_with, name_ends_with): verts += [ tuple(ob.matrix_world * vertex.co) for vertex in ob.data.vertices ] verts = np.array(verts) dataset_tools.save_3D_points_to_pcd_file(filename, verts)
def main(): traj_to_file, traj_from_file, traj_input_files, map_input_files, at_frame, offset_time = parse_cmd_args() print "Calculating transformation..." cam_trajectory_from = dataset_tools.load_cam_trajectory_TUM(traj_from_file) cam_trajectory_to = dataset_tools.load_cam_trajectory_TUM(traj_to_file) transformation = dataset_tools.transform_between_cam_trajectories( cam_trajectory_from, cam_trajectory_to, at_frame=at_frame, offset_time=offset_time ) print "Results:" delta_quaternion, delta_scale, delta_location = transformation print "delta_quaternion:" print "\t %s" % delta_quaternion print "delta_scale:" print "\t %s" % delta_scale print "delta_location:" print "\t %s" % delta_location print for traj_input_file in traj_input_files: print 'Transforming traj "%s"...' % traj_input_file dataset_tools.save_cam_trajectory_TUM( "%s-trfm%s" % tuple(os.path.splitext(traj_input_file)), dataset_tools.transformed_cam_trajectory( dataset_tools.load_cam_trajectory_TUM(traj_input_file), transformation ) ) for map_input_file in map_input_files: print 'Transforming map "%s"...' % map_input_file points, colors, _ = dataset_tools.load_3D_points_from_pcd_file(map_input_file, use_alpha=True) dataset_tools.save_3D_points_to_pcd_file( "%s-trfm%s" % tuple(os.path.splitext(map_input_file)), dataset_tools.transformed_points(points, transformation), colors ) print "Done."
def main(): """ A file with the initial pose and 3D points of the SVO dataset, will be created. """ num_features = 100 num_iterations = 53 #corner_quality_level = 0.4805294789864505 print "Searching for features ( max", num_features, ")..." # Load first image img = cv2.cvtColor( cv2.imread(join_path("sin2_tex2_h1_v8_d", "img", "frame_000002_0.png")), cv2.COLOR_BGR2GRAY ) # Find just enough features, iterate using bisection to find the best "corner_quality_level" value corner_min_dist = 0. lower = 0. upper = 1. for i in range(num_iterations): corner_quality_level = (lower + upper) / 2 imgp = cv2.goodFeaturesToTrack(img, num_features, corner_quality_level, corner_min_dist) if imgp == None or len(imgp) < num_features: upper = corner_quality_level else: lower = corner_quality_level corner_quality_level = lower if lower else corner_quality_level imgp = cv2.goodFeaturesToTrack(img, num_features, corner_quality_level, corner_min_dist).reshape((-1, 2)) print len(imgp), "features found, corner_quality_level:", corner_quality_level # Load camera intrinsics cameraMatrix, distCoeffs, imageSize = calibration_tools.load_camera_intrinsics( join_path("camera_intrinsics.txt") ) # Load and save first pose timestps, locations, quaternions = dataset_tools.load_cam_trajectory_TUM( join_path("sin2_tex2_h1_v8_d", "traj_groundtruth.txt") ) P = trfm.P_from_pose_TUM(quaternions[0], locations[0]) np.savetxt(join_path("sin2_tex2_h1_v8_d", "init_pose.txt"), P) # Generate 3D points, knowing that they lay at the plane z=0: # for each 2D point, the following system of equations is solved for X: # A * X = b # where: # X = [x, y, s]^T # [x, y, 0] the 3D point's coords # s is an unknown scalefactor of the normalized 2D point # A = [[ 1 0 | ], # [ 0 1 | Pu ], # [ 0 0 | ]] # Pu = - R^(-1) * p # p = [u, v, 1]^T the 2D point's homogeneous coords # b = - R^(-1) * t # R, t = 3x3 rotation matrix and 3x1 translation vector of 3x4 pose matrix P # The system of equations is solved in bulk for all points using broadcasted backsubstitution objp = np.empty((len(imgp), 3)).T objp[2:3, :] = 0. # z = 0 imgp_nrml = cv2.undistortPoints(np.array([imgp]), cameraMatrix, distCoeffs)[0] imgp_nrml = np.concatenate((imgp_nrml, np.ones((len(imgp), 1))), axis=1) # to homogeneous coords P_inv = trfm.P_inv(P) Pu = P_inv[0:3, 0:3].dot(imgp_nrml.T) scale = -P_inv[2:3, 3:4] / Pu[2:3, :] objp[0:2, :] = P_inv[0:2, 3:4] + scale * Pu[0:2, :] # x and y components objp = objp.T # Save 3D points dataset_tools.save_3D_points_to_pcd_file( join_path("sin2_tex2_h1_v8_d", "init_points.pcd"), objp ) print "Done."
def main(): """ A file with the initial pose and 3D points of the SVO dataset, will be created. """ num_features = 100 num_iterations = 53 #corner_quality_level = 0.4805294789864505 print("Searching for features ( max", num_features, ")...") # Load first image img = cv2.cvtColor( cv2.imread(join_path("sin2_tex2_h1_v8_d", "img", "frame_000002_0.png")), cv2.COLOR_BGR2GRAY) # Find just enough features, iterate using bisection to find the best "corner_quality_level" value corner_min_dist = 0. lower = 0. upper = 1. for i in range(num_iterations): corner_quality_level = (lower + upper) / 2 imgp = cv2.goodFeaturesToTrack(img, num_features, corner_quality_level, corner_min_dist) if imgp == None or len(imgp) < num_features: upper = corner_quality_level else: lower = corner_quality_level corner_quality_level = lower if lower else corner_quality_level imgp = cv2.goodFeaturesToTrack(img, num_features, corner_quality_level, corner_min_dist).reshape((-1, 2)) print(len(imgp), "features found, corner_quality_level:", corner_quality_level) # Load camera intrinsics cameraMatrix, distCoeffs, imageSize = calibration_tools.load_camera_intrinsics( join_path("camera_intrinsics.txt")) # Load and save first pose timestps, locations, quaternions = dataset_tools.load_cam_trajectory_TUM( join_path("sin2_tex2_h1_v8_d", "traj_groundtruth.txt")) P = trfm.P_from_pose_TUM(quaternions[0], locations[0]) np.savetxt(join_path("sin2_tex2_h1_v8_d", "init_pose.txt"), P) # Generate 3D points, knowing that they lay at the plane z=0: # for each 2D point, the following system of equations is solved for X: # A * X = b # where: # X = [x, y, s]^T # [x, y, 0] the 3D point's coords # s is an unknown scalefactor of the normalized 2D point # A = [[ 1 0 | ], # [ 0 1 | Pu ], # [ 0 0 | ]] # Pu = - R^(-1) * p # p = [u, v, 1]^T the 2D point's homogeneous coords # b = - R^(-1) * t # R, t = 3x3 rotation matrix and 3x1 translation vector of 3x4 pose matrix P # The system of equations is solved in bulk for all points using broadcasted backsubstitution objp = np.empty((len(imgp), 3)).T objp[2:3, :] = 0. # z = 0 imgp_nrml = cv2.undistortPoints(np.array([imgp]), cameraMatrix, distCoeffs)[0] imgp_nrml = np.concatenate((imgp_nrml, np.ones((len(imgp), 1))), axis=1) # to homogeneous coords P_inv = trfm.P_inv(P) Pu = P_inv[0:3, 0:3].dot(imgp_nrml.T) scale = -P_inv[2:3, 3:4] / Pu[2:3, :] objp[0:2, :] = P_inv[0:2, 3:4] + scale * Pu[0:2, :] # x and y components objp = objp.T # Save 3D points dataset_tools.save_3D_points_to_pcd_file( join_path("sin2_tex2_h1_v8_d", "init_points.pcd"), objp) print("Done.")