Spaces:
Running
Running
File size: 1,122 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 |
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
|