| 175 | |
| 176 | |
| 177 | def plot_pose(ref_poses, est_poses, output_path, args, vid=False): |
| 178 | ref_poses = [pose for pose in ref_poses] |
| 179 | if isinstance(est_poses, dict): |
| 180 | est_poses = [pose for k, pose in est_poses.items()] |
| 181 | else: |
| 182 | est_poses = [pose for pose in est_poses] |
| 183 | traj_ref = PosePath3D(poses_se3=ref_poses) |
| 184 | traj_est = PosePath3D(poses_se3=est_poses) |
| 185 | traj_est_aligned = copy.deepcopy(traj_est) |
| 186 | traj_est_aligned.align(traj_ref, correct_scale=True, |
| 187 | correct_only_scale=False) |
| 188 | if vid: |
| 189 | for p_idx in range(len(ref_poses)): |
| 190 | fig = plt.figure() |
| 191 | current_est_aligned = traj_est_aligned.poses_se3[:p_idx+1] |
| 192 | current_ref = traj_ref.poses_se3[:p_idx+1] |
| 193 | current_est_aligned = PosePath3D(poses_se3=current_est_aligned) |
| 194 | current_ref = PosePath3D(poses_se3=current_ref) |
| 195 | traj_by_label = { |
| 196 | # "estimate (not aligned)": traj_est, |
| 197 | "Ours (aligned)": current_est_aligned, |
| 198 | "Ground-truth": current_ref |
| 199 | } |
| 200 | plot_mode = plot.PlotMode.xyz |
| 201 | # ax = plot.prepare_axis(fig, plot_mode, 111) |
| 202 | ax = fig.add_subplot(111, projection="3d") |
| 203 | ax.xaxis.set_tick_params(labelbottom=False) |
| 204 | ax.yaxis.set_tick_params(labelleft=False) |
| 205 | ax.zaxis.set_tick_params(labelleft=False) |
| 206 | colors = ['r', 'b'] |
| 207 | styles = ['-', '--'] |
| 208 | |
| 209 | for idx, (label, traj) in enumerate(traj_by_label.items()): |
| 210 | plot.traj(ax, plot_mode, traj, |
| 211 | styles[idx], colors[idx], label) |
| 212 | # break |
| 213 | # plot.trajectories(fig, traj_by_label, plot.PlotMode.xyz) |
| 214 | ax.view_init(elev=10., azim=45) |
| 215 | plt.tight_layout() |
| 216 | os.makedirs(os.path.join(os.path.dirname( |
| 217 | output_path), 'pose_vid'), exist_ok=True) |
| 218 | pose_vis_path = os.path.join(os.path.dirname( |
| 219 | output_path), 'pose_vid', 'pose_vis_{:03d}.png'.format(p_idx)) |
| 220 | print(pose_vis_path) |
| 221 | fig.savefig(pose_vis_path) |
| 222 | |
| 223 | |
| 224 | fig = plt.figure() |
| 225 | fig.patch.set_facecolor('white') # Set background to pure white |
| 226 | traj_by_label = { |
| 227 | # "estimate (not aligned)": traj_est, |
| 228 | "Ours (aligned)": traj_est_aligned, |
| 229 | "COLMAP (GT)": traj_ref |
| 230 | } |
| 231 | plot_mode = plot.PlotMode.xyz |
| 232 | # ax = plot.prepare_axis(fig, plot_mode, 111) |
| 233 | ax = fig.add_subplot(111, projection="3d") |
| 234 | ax.set_facecolor('white') # Set subplot to pure white |