| 105 | |
| 106 | |
| 107 | def load_replica_traj(filename: str) -> tuple: |
| 108 | traj_w_c = np.loadtxt(filename) |
| 109 | assert traj_w_c.shape[1] == 12 or traj_w_c.shape[1] == 16 |
| 110 | poses = [ |
| 111 | np.array( |
| 112 | [ |
| 113 | [r[0], r[1], r[2], r[3]], |
| 114 | [r[4], r[5], r[6], r[7]], |
| 115 | [r[8], r[9], r[10], r[11]], |
| 116 | [0, 0, 0, 1], |
| 117 | ] |
| 118 | ) |
| 119 | for r in traj_w_c |
| 120 | ] |
| 121 | |
| 122 | pose_path = PosePath3D(poses_se3=poses) |
| 123 | timestamps_mat = np.arange(traj_w_c.shape[0]).astype(float) |
| 124 | |
| 125 | traj = PoseTrajectory3D(poses_se3=pose_path.poses_se3, timestamps=timestamps_mat) |
| 126 | xyz = traj.positions_xyz |
| 127 | # shift -1 column -> w in back column |
| 128 | # quat = np.roll(traj.orientations_quat_wxyz, -1, axis=1) |
| 129 | # uncomment this line if the quaternion is in scalar-first format |
| 130 | quat = traj.orientations_quat_wxyz |
| 131 | |
| 132 | traj_tum = np.column_stack((xyz, quat)) |
| 133 | return (traj_tum, timestamps_mat) |
| 134 | |
| 135 | |
| 136 | def load_sintel_traj(gt_file): # './data/sintel/training/camdata_left/alley_2' |