(pose, dz)
| 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)]) |
no outgoing calls
no test coverage detected