File size: 2,013 Bytes
404d2af 8b973ee 404d2af 8b973ee 404d2af 8b973ee 404d2af 8b973ee 404d2af 8b973ee 404d2af 8b973ee 404d2af 8b973ee 404d2af 8b973ee 404d2af 8b973ee |
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 |
from .transformations import quaternion_from_matrix
import numpy as np
import os
import sys
def evaluate_R_t(R_gt, t_gt, R, t):
t = t.flatten()
t_gt = t_gt.flatten()
eps = 1e-15
q_gt = quaternion_from_matrix(R_gt)
q = quaternion_from_matrix(R)
q = q / (np.linalg.norm(q) + eps)
q_gt = q_gt / (np.linalg.norm(q_gt) + eps)
loss_q = np.maximum(eps, (1.0 - np.sum(q * q_gt) ** 2))
err_q = np.arccos(1 - 2 * loss_q)
t = t / (np.linalg.norm(t) + eps)
t_gt = t_gt / (np.linalg.norm(t_gt) + eps)
loss_t = np.maximum(eps, (1.0 - np.sum(t * t_gt) ** 2))
err_t = np.arccos(np.sqrt(1 - loss_t))
return np.rad2deg(err_q), np.rad2deg(err_t)
def pose_auc(errors, thresholds):
sort_idx = np.argsort(errors)
errors = np.array(errors.copy())[sort_idx]
recall = (np.arange(len(errors)) + 1) / len(errors)
errors = np.r_[0.0, errors]
recall = np.r_[0.0, recall]
aucs = []
for t in thresholds[1:]:
last_index = np.searchsorted(errors, t)
r = np.r_[recall[:last_index], recall[last_index - 1]]
e = np.r_[errors[:last_index], t]
aucs.append(np.trapz(r, x=e) / t)
return aucs
def approx_pose_auc(errors, thresholds):
qt_acc_hist, _ = np.histogram(errors, thresholds)
num_pair = float(len(errors))
qt_acc_hist = qt_acc_hist.astype(float) / num_pair
qt_acc = np.cumsum(qt_acc_hist)
approx_aucs = [np.mean(qt_acc[:i]) for i in range(1, len(thresholds))]
return approx_aucs
def compute_epi_inlier(x1, x2, E, inlier_th):
num_pts1, num_pts2 = x1.shape[0], x2.shape[0]
x1_h = np.concatenate([x1, np.ones([num_pts1, 1])], -1)
x2_h = np.concatenate([x2, np.ones([num_pts2, 1])], -1)
ep_line1 = x1_h @ E.T
ep_line2 = x2_h @ E
norm_factor = (
1 / np.sqrt((ep_line1[:, :2] ** 2).sum(1))
+ 1 / np.sqrt((ep_line2[:, :2] ** 2).sum(1))
) / 2
dis = abs((ep_line1 * x2_h).sum(-1)) * norm_factor
inlier_mask = dis < inlier_th
return inlier_mask
|