(ini_track, opt_track, loop_track, output_dir)
| 113 | |
| 114 | |
| 115 | def visualize_loop(ini_track, opt_track, loop_track, output_dir): |
| 116 | def extract_xyz(c2ws): |
| 117 | c2ws = c2ws.cpu().numpy() |
| 118 | return c2ws[:, 0], c2ws[:, 1], c2ws[:, 2] |
| 119 | |
| 120 | x0, _, y0 = extract_xyz(ini_track) |
| 121 | x1, _, y1 = extract_xyz(opt_track) |
| 122 | |
| 123 | plt.figure(figsize=(8, 6)) |
| 124 | plt.plot(x0, y0, "o--", alpha=0.45, label="Before Optimization") |
| 125 | plt.plot(x1, y1, "o-", label="After Optimization") |
| 126 | for i, j, _ in loop_track: |
| 127 | plt.plot([x0[i], x0[j]], [y0[i], y0[j]], "r--", alpha=0.25, label="Loop (Before)" if i == 5 else "") |
| 128 | plt.plot([x1[i], x1[j]], [y1[i], y1[j]], "g-", alpha=0.35, label="Loop (After)" if i == 5 else "") |
| 129 | plt.gca().set_aspect("equal") |
| 130 | plt.title("Sim3 Loop Closure Optimization") |
| 131 | plt.xlabel("x") |
| 132 | plt.ylabel("z") |
| 133 | plt.legend() |
| 134 | plt.grid(True) |
| 135 | plt.axis("equal") |
| 136 | save_path = join(output_dir, "sim3_opt_result.png") |
| 137 | plt.savefig(save_path, dpi=300, bbox_inches="tight") |
| 138 | plt.close() |
no test coverage detected