()
| 349 | |
| 350 | |
| 351 | def example_usage(): |
| 352 | optimizer = Sim3LoopOptimizer(solve_system_version="cpp") |
| 353 | |
| 354 | # Build rotating ring |
| 355 | sequential_transforms = create_ring_transforms(num_poses=20, radius=3.0) |
| 356 | |
| 357 | # Add loop closure constraint: from frame 5 back to frame 0 |
| 358 | loop_constraints = [ |
| 359 | (20, 0, (1.0, np.eye(3), np.zeros(3))) # Temporary unit loop for simulation |
| 360 | ] |
| 361 | |
| 362 | # Trajectory before/after optimization |
| 363 | input_abs_poses = optimizer.sequential_to_absolute_poses(sequential_transforms) |
| 364 | optimized_transforms = optimizer.optimize(sequential_transforms, loop_constraints) |
| 365 | optimized_abs_poses = optimizer.sequential_to_absolute_poses(optimized_transforms) |
| 366 | |
| 367 | def extract_xyz(pose_tensor): |
| 368 | poses = pose_tensor.cpu().numpy() |
| 369 | return poses[:, 0], poses[:, 1], poses[:, 2] |
| 370 | |
| 371 | x0, y0, z0 = extract_xyz(input_abs_poses) |
| 372 | x1, y1, z1 = extract_xyz(optimized_abs_poses) |
| 373 | |
| 374 | # Visualize trajectory |
| 375 | import matplotlib |
| 376 | import matplotlib.pyplot as plt |
| 377 | |
| 378 | matplotlib.use("Agg") |
| 379 | |
| 380 | plt.figure(figsize=(8, 6)) |
| 381 | plt.plot(x0, y0, "o--", label="Before Optimization") |
| 382 | plt.plot(x1, y1, "o-", label="After Optimization") |
| 383 | for i, j, _ in loop_constraints: |
| 384 | plt.plot([x0[i], x0[j]], [y0[i], y0[j]], "r--", label="Loop (Before)" if i == 5 else "") |
| 385 | plt.plot([x1[i], x1[j]], [y1[i], y1[j]], "g-", label="Loop (After)" if i == 5 else "") |
| 386 | plt.gca().set_aspect("equal") |
| 387 | plt.title("Sim3 Loop Closure Optimization (Rotating Ring)") |
| 388 | plt.xlabel("x") |
| 389 | plt.ylabel("y") |
| 390 | plt.legend() |
| 391 | plt.grid(True) |
| 392 | plt.axis("equal") |
| 393 | plt.show() |
| 394 | |
| 395 | return optimized_transforms |
| 396 | |
| 397 | |
| 398 | if __name__ == "__main__": |
no test coverage detected