Transform points using an SE(3) matrix. Args: matrix: 4x4 SE(3) matrix points: Nx3 array of points Returns: Nx3 array of transformed points
(matrix, points)
| 98 | |
| 99 | |
| 100 | def transform_points(matrix, points): |
| 101 | """Transform points using an SE(3) matrix. |
| 102 | |
| 103 | Args: |
| 104 | matrix: 4x4 SE(3) matrix |
| 105 | points: Nx3 array of points |
| 106 | |
| 107 | Returns: |
| 108 | Nx3 array of transformed points |
| 109 | """ |
| 110 | assert matrix.shape == (4, 4) |
| 111 | assert len(points.shape) == 2 and points.shape[1] == 3 |
| 112 | return points @ matrix[:3, :3].T + matrix[:3, 3] |
| 113 | |
| 114 | |
| 115 | def transform_cameras(matrix, camtoworlds): |