Spaces:
Running
Running
File size: 3,527 Bytes
13760e8 |
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 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 |
import numpy as np
import torch
import poselib
def relative_pose_error(T_0to1, R, t, ignore_gt_t_thr=0.0):
# angle error between 2 vectors
t_gt = T_0to1[:3, 3]
n = np.linalg.norm(t) * np.linalg.norm(t_gt)
t_err = np.rad2deg(np.arccos(np.clip(np.dot(t, t_gt) / n, -1.0, 1.0)))
t_err = np.minimum(t_err, 180 - t_err) # handle E ambiguity
if np.linalg.norm(t_gt) < ignore_gt_t_thr: # pure rotation is challenging
t_err = 0
# angle error between 2 rotation matrices
R_gt = T_0to1[:3, :3]
cos = (np.trace(np.dot(R.T, R_gt)) - 1) / 2
cos = np.clip(cos, -1.0, 1.0) # handle numercial errors
R_err = np.rad2deg(np.abs(np.arccos(cos)))
return t_err, R_err
def intrinsics_to_camera(K):
px, py = K[0, 2], K[1, 2]
fx, fy = K[0, 0], K[1, 1]
return {
"model": "PINHOLE",
"width": int(2 * px),
"height": int(2 * py),
"params": [fx, fy, px, py],
}
def estimate_pose(kpts0, kpts1, K0, K1, thresh, conf=0.99999):
M, info = poselib.estimate_relative_pose(
kpts0, kpts1,
intrinsics_to_camera(K0),
intrinsics_to_camera(K1),
{"max_epipolar_error": thresh,
"success_prob": conf,
"min_iterations": 20,
"max_iterations": 1_000},
)
R, t, inl = M.R, M.t, info["inliers"]
inl = np.array(inl)
ret = (R, t, inl)
return ret
def tensor2bgr(t):
return (t.cpu()[0].permute(1,2,0).numpy()*255).astype(np.uint8)
def compute_pose_error(match_fn,data):
result = {}
with torch.no_grad():
mkpts0,mkpts1=match_fn(tensor2bgr(data["image0"]),tensor2bgr(data["image1"]))
mkpts0=mkpts0 * data["scale0"].numpy()
mkpts1=mkpts1 * data["scale1"].numpy()
K0, K1 = data["K0"][0].numpy(), data["K1"][0].numpy()
T_0to1 = data["T_0to1"][0].numpy()
T_1to0 = data["T_1to0"][0].numpy()
result={}
conf = 0.99999
ret = estimate_pose(mkpts0,mkpts1,K0,K1,4.0,conf)
if ret is not None:
R, t, inliers = ret
t_err, R_err = relative_pose_error(T_0to1, R, t, ignore_gt_t_thr=0.0)
result['R_err'] = R_err
result['t_err'] = t_err
return result
def error_auc(errors, thresholds=[5, 10, 20]):
"""
Args:
errors (list): [N,]
thresholds (list)
"""
errors = [0] + sorted(list(errors))
recall = list(np.linspace(0, 1, len(errors)))
aucs = []
for thr in thresholds:
last_index = np.searchsorted(errors, thr)
y = recall[:last_index] + [recall[last_index-1]]
x = errors[:last_index] + [thr]
aucs.append(np.trapz(y, x) / thr)
return {f'auc@{t}': auc for t, auc in zip(thresholds, aucs)}
def compute_maa(pairs, thresholds=[5, 10, 20]):
# print("auc / mAcc on %d pairs" % (len(pairs)))
errors = []
for p in pairs:
et = p['t_err']
er = p['R_err']
errors.append(max(et, er))
d_err_auc = error_auc(errors)
# for k,v in d_err_auc.items():
# print(k, ': ', '%.1f'%(v*100))
errors = np.array(errors)
for t in thresholds:
acc = (errors <= t).sum() / len(errors)
# print("mAcc@%d: %.1f "%(t, acc*100))
return d_err_auc,errors
def homo_trans(coord, H):
kpt_num = coord.shape[0]
homo_coord = np.concatenate((coord, np.ones((kpt_num, 1))), axis=-1)
proj_coord = np.matmul(H, homo_coord.T).T
proj_coord = proj_coord / proj_coord[:, 2][..., None]
proj_coord = proj_coord[:, 0:2]
return proj_coord
|