LiftFeat/evaluation/eval_utils.py at main · Paar-Autonomy/LiftFeat · GitHub

Repository navigation

History

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

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

LiftFeat/evaluation/eval_utils.py at main · Paar-Autonomy/LiftFeat · GitHub

Repository navigation

Expand file tree

Breadcrumbs

eval_utils.py

Latest commit

History

Breadcrumbs

eval_utils.py

File metadata and controls