File size: 1,074 Bytes
9223079 |
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 |
import numpy as np
import pycolmap
def to_homogeneous(p):
return np.pad(p, ((0, 0),) * (p.ndim - 1) + ((0, 1),), constant_values=1)
def vector_to_cross_product_matrix(v):
return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
def compute_epipolar_errors(qvec_r2t, tvec_r2t, p2d_r, p2d_t):
T_r2t = pose_matrix_from_qvec_tvec(qvec_r2t, tvec_r2t)
# Compute errors in normalized plane to avoid distortion.
E = vector_to_cross_product_matrix(T_r2t[:3, -1]) @ T_r2t[:3, :3]
l2d_r2t = (E @ to_homogeneous(p2d_r).T).T
l2d_t2r = (E.T @ to_homogeneous(p2d_t).T).T
errors_r = np.abs(np.sum(to_homogeneous(p2d_r) * l2d_t2r, axis=1)) / np.linalg.norm(
l2d_t2r[:, :2], axis=1
)
errors_t = np.abs(np.sum(to_homogeneous(p2d_t) * l2d_r2t, axis=1)) / np.linalg.norm(
l2d_r2t[:, :2], axis=1
)
return E, errors_r, errors_t
def pose_matrix_from_qvec_tvec(qvec, tvec):
pose = np.zeros((4, 4))
pose[:3, :3] = pycolmap.qvec_to_rotmat(qvec)
pose[:3, -1] = tvec
pose[-1, -1] = 1
return pose
|