def load_and_create_cam_trajectory(filename, name_prefix="", strip_file_extension=True, start_frame=1, start_time=None, fps="data", no_keyframe_highlighting=True, goto_last_keyframe=False): """ Load a camera trajectory (in the TUM format, see "dataset_tools" module for more info) with filename "filename", and create it in Blender. "name_prefix", "strip_file_extension" : see documentation of "object_name_from_filename()" "start_frame" : Blender's start frame of the trajectory "start_time" : if not None, this float will be used as trajectory's timestamp offset to start from "fps" : should be one of the following: - "blender" : infer the fps from Blender's scene "fps" property - "data" : infer the fps from the data (using minimum delta time), Blender's "fps" will be adjusted - an integer indicating the data's fps "no_keyframe_highlighting" : if True, don't show framenumbers for each keyframe along trajectory "goto_last_keyframe" : go to the last keyframe of the generated trajectory Note: to synchronize multiple trajectories in time, it is advised to set: - "start_frame" to 0 - "start_time" to 0. - "fps" to the exact fps integer """ timestps, locations, quaternions = dataset_tools.load_cam_trajectory_TUM( filename) if len(timestps) == 0: return elif len(timestps) == 1: framenrs = [start_frame] else: if fps == "blender": fps = bpy.context.scene.render.fps elif fps == "data": fps = 1. / np.min(timestps[1:] - timestps[:-1]) bpy.context.scene.render.fps = int(round(fps)) if start_time == None: start_time = timestps[0] framenrs = np.rint(start_frame + (timestps - start_time) * float(fps)).astype(int) create_cam_trajectory(object_name_from_filename(filename, name_prefix, strip_file_extension), locations, quaternions, start_frame, framenrs, no_keyframe_highlighting, goto_last_keyframe=goto_last_keyframe)
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(): """ The quaternions of the groundtruth will be normalized, and saved to another filename. """ print("Normalizing quaternions of groundtruth camera trajectory...") dataset_tools.save_cam_trajectory_TUM( join_path("sin2_tex2_h1_v8_d", "traj_groundtruth.txt"), dataset_tools.load_cam_trajectory_TUM(join_path("sin2_tex2_h1_v8_d", "trajectory_nominal.txt")), ) print("Done.")
def repair_ICL_NUIM_cam_trajectory(filename_in, filename_out, initial_location=None, rebuild_timestamps=True, delta_timestamp=0., fps=30): """ After this repair, you will be able to use the output in conjunction with the original (i.e. non-mirrored) 3D scene of the dataset, provided that you don't apply the negative Y-factor of the camera calibration matrix. "filename_in" : filename of the input camera trajectory "filename_out" : filename of the repaired output camera trajectory "initial_location" : location of the first camera pose Note that all ICL NUIM camera trajectories are undetermined up to an unknown translation. To find the correct "initial_location", you can do the following: Ps = load_cam_poses_POV("traj1.posesRenderingCommands.sh") _, locs_exact, _ = dataset_tools.convert_cam_poses_to_cam_trajectory_TUM(Ps) initial_location = [-locs_exact[0, 0], locs_exact[0, 1], locs_exact[0, 2]] "rebuild_timestamps" : if True, regenerate timestamps starting from start-time "delta_timestamp", at a rate of "fps" The new "timestps", "locations", "quaternions" will be returned. Note: this is not the recommended way to go, for accuracy, consider using "load_cam_poses_POV()" instead. """ timestps, locations, quaternions = dataset_tools.load_cam_trajectory_TUM( filename_in) if initial_location != None: delta_location = initial_location - locations[0] else: delta_location = np.zeros(3) if rebuild_timestamps: timestps = delta_timestamp + (1 + np.arange(len(timestps))) / float(fps) for i, (location, quaternion) in enumerate(zip(locations, quaternions)): lx, ly, lz = location locations[i] = np.array([lx, ly, -lz]) + delta_location qx, qy, qz, qw = quaternion quaternions[i] = np.array([qw, qz, qy, -qx]) dataset_tools.save_cam_trajectory_TUM(filename_out, (timestps, locations, quaternions)) return timestps, locations, quaternions
def main(): """ The quaternions of the groundtruth will be normalized, and saved to another filename. """ print ("Normalizing quaternions of groundtruth camera trajectory...") dataset_tools.save_cam_trajectory_TUM( join_path("sin2_tex2_h1_v8_d", "traj_groundtruth.txt"), dataset_tools.load_cam_trajectory_TUM( join_path("sin2_tex2_h1_v8_d", "trajectory_nominal.txt") ) ) print ("Done.")
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 repair_ICL_NUIM_cam_trajectory(filename_in, filename_out, initial_location=None, rebuild_timestamps=True, delta_timestamp=0., fps=30): """ After this repair, you will be able to use the output in conjunction with the original (i.e. non-mirrored) 3D scene of the dataset, provided that you don't apply the negative Y-factor of the camera calibration matrix. "filename_in" : filename of the input camera trajectory "filename_out" : filename of the repaired output camera trajectory "initial_location" : location of the first camera pose Note that all ICL NUIM camera trajectories are undetermined up to an unknown translation. To find the correct "initial_location", you can do the following: Ps = load_cam_poses_POV("traj1.posesRenderingCommands.sh") _, locs_exact, _ = dataset_tools.convert_cam_poses_to_cam_trajectory_TUM(Ps) initial_location = [-locs_exact[0, 0], locs_exact[0, 1], locs_exact[0, 2]] "rebuild_timestamps" : if True, regenerate timestamps starting from start-time "delta_timestamp", at a rate of "fps" The new "timestps", "locations", "quaternions" will be returned. Note: this is not the recommended way to go, for accuracy, consider using "load_cam_poses_POV()" instead. """ timestps, locations, quaternions = dataset_tools.load_cam_trajectory_TUM(filename_in) if initial_location != None: delta_location = initial_location - locations[0] else: delta_location = np.zeros(3) if rebuild_timestamps: timestps = delta_timestamp + (1 + np.arange(len(timestps))) / float(fps) for i, (location, quaternion) in enumerate(zip(locations, quaternions)): lx, ly, lz = location locations[i] = np.array([lx, ly, -lz]) + delta_location qx, qy, qz, qw = quaternion quaternions[i] = np.array([qw, qz, qy, -qx]) dataset_tools.save_cam_trajectory_TUM(filename_out, (timestps, locations, quaternions)) return timestps, locations, quaternions
def load_and_create_cam_trajectory(filename, name_prefix="", strip_file_extension=True, start_frame=1, start_time=None, fps="data", no_keyframe_highlighting=True, goto_last_keyframe=False): """ Load a camera trajectory (in the TUM format, see "dataset_tools" module for more info) with filename "filename", and create it in Blender. "name_prefix", "strip_file_extension" : see documentation of "object_name_from_filename()" "start_frame" : Blender's start frame of the trajectory "start_time" : if not None, this float will be used as trajectory's timestamp offset to start from "fps" : should be one of the following: - "blender" : infer the fps from Blender's scene "fps" property - "data" : infer the fps from the data (using minimum delta time), Blender's "fps" will be adjusted - an integer indicating the data's fps "no_keyframe_highlighting" : if True, don't show framenumbers for each keyframe along trajectory "goto_last_keyframe" : go to the last keyframe of the generated trajectory Note: to synchronize multiple trajectories in time, it is advised to set: - "start_frame" to 0 - "start_time" to 0. - "fps" to the exact fps integer """ timestps, locations, quaternions = dataset_tools.load_cam_trajectory_TUM(filename) if len(timestps) == 0: return elif len(timestps) == 1: framenrs = [start_frame] else: if fps == "blender": fps = bpy.context.scene.render.fps elif fps == "data": fps = 1. / np.min(timestps[1:] - timestps[:-1]) bpy.context.scene.render.fps = int(round(fps)) if start_time == None: start_time = timestps[0] framenrs = np.rint(start_frame + (timestps - start_time) * float(fps)).astype(int) create_cam_trajectory( object_name_from_filename(filename, name_prefix, strip_file_extension), locations, quaternions, start_frame, framenrs, no_keyframe_highlighting, goto_last_keyframe=goto_last_keyframe )
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.")