Insert extra poses at the start and end of the path.
(poses, n_buffer)
| 485 | return np.array(poses) |
| 486 | |
| 487 | def insert_buffer_poses(poses, n_buffer): |
| 488 | """Insert extra poses at the start and end of the path.""" |
| 489 | |
| 490 | def average_distance(points): |
| 491 | distances = np.linalg.norm(points[1:] - points[0:-1], axis=-1) |
| 492 | return np.mean(distances) |
| 493 | |
| 494 | def shift(pose, dz): |
| 495 | result = np.copy(pose) |
| 496 | z = result[:3, 2] |
| 497 | z /= np.linalg.norm(z) |
| 498 | # Move along forward-backward axis. -z is forward. |
| 499 | result[:3, 3] += z * dz |
| 500 | return result |
| 501 | |
| 502 | dz = average_distance(poses[:, :3, 3]) |
| 503 | prefix = np.stack([shift(poses[0], (i + 1) * dz) for i in range(n_buffer)]) |
| 504 | prefix = prefix[::-1] # reverse order |
| 505 | suffix = np.stack( |
| 506 | [shift(poses[-1], -(i + 1) * dz) for i in range(n_buffer)] |
| 507 | ) |
| 508 | result = np.concatenate([prefix, poses, suffix]) |
| 509 | return result |
| 510 | |
| 511 | def remove_buffer_poses(poses, u, n_frames, u_keyframes, n_buffer): |
| 512 | u_keyframes = u_keyframes[n_buffer:-n_buffer] |
no test coverage detected