Spaces:
Running
Running
File size: 2,723 Bytes
499e141 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 |
from typing import List, Tuple
import numpy as np
from transforms3d.quaternions import quat2mat
def project(pts: np.ndarray, K: np.ndarray, img_size: List[int] or Tuple[int] = None) -> np.ndarray:
"""Projects 3D points to image plane.
Args:
- pts [N, 3/4]: points in camera coordinates (homogeneous or non-homogeneous)
- K [3, 3]: intrinsic matrix
- img_size (width, height): optional, clamp projection to image borders
Outputs:
- uv [N, 2]: coordinates of projected points
"""
assert len(pts.shape) == 2, 'incorrect number of dimensions'
assert pts.shape[1] in [3, 4], 'invalid dimension size'
assert K.shape == (3, 3), 'incorrect intrinsic shape'
uv_h = (K @ pts[:, :3].T).T
uv = uv_h[:, :2] / uv_h[:, -1:]
if img_size is not None:
uv[:, 0] = np.clip(uv[:, 0], 0, img_size[0])
uv[:, 1] = np.clip(uv[:, 1], 0, img_size[1])
return uv
def get_grid_multipleheight() -> np.ndarray:
# create grid of points
ar_grid_step = 0.3
ar_grid_num_x = 7
ar_grid_num_y = 4
ar_grid_num_z = 7
ar_grid_z_offset = 1.8
ar_grid_y_offset = 0
ar_grid_x_pos = np.arange(0, ar_grid_num_x)-(ar_grid_num_x-1)/2
ar_grid_x_pos *= ar_grid_step
ar_grid_y_pos = np.arange(0, ar_grid_num_y)-(ar_grid_num_y-1)/2
ar_grid_y_pos *= ar_grid_step
ar_grid_y_pos += ar_grid_y_offset
ar_grid_z_pos = np.arange(0, ar_grid_num_z).astype(float)
ar_grid_z_pos *= ar_grid_step
ar_grid_z_pos += ar_grid_z_offset
xx, yy, zz = np.meshgrid(ar_grid_x_pos, ar_grid_y_pos, ar_grid_z_pos)
ones = np.ones(xx.shape[0]*xx.shape[1]*xx.shape[2])
eye_coords = np.concatenate([c.reshape(-1, 1)
for c in (xx, yy, zz, ones)], axis=-1)
return eye_coords
# global variable, avoids creating it again
eye_coords_glob = get_grid_multipleheight()
def reprojection_error(
q_est: np.ndarray, t_est: np.ndarray, q_gt: np.ndarray, t_gt: np.ndarray, K: np.ndarray,
W: int, H: int) -> float:
eye_coords = eye_coords_glob
# obtain ground-truth position of projected points
uv_gt = project(eye_coords, K, (W, H))
# residual transformation
cam2w_est = np.eye(4)
cam2w_est[:3, :3] = quat2mat(q_est)
cam2w_est[:3, -1] = t_est
cam2w_gt = np.eye(4)
cam2w_gt[:3, :3] = quat2mat(q_gt)
cam2w_gt[:3, -1] = t_gt
# residual reprojection
eyes_residual = (np.linalg.inv(cam2w_est) @ cam2w_gt @ eye_coords.T).T
uv_pred = project(eyes_residual, K, (W, H))
# get reprojection error
repr_err = np.linalg.norm(uv_gt - uv_pred, ord=2, axis=1)
mean_repr_err = float(repr_err.mean().item())
return mean_repr_err
|