Spaces:
Running
Running
File size: 2,343 Bytes
63f3cf2 |
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 |
# -*- coding: UTF-8 -*-
'''=================================================
@Project -> File pram -> utils
@IDE PyCharm
@Author fx221@cam.ac.uk
@Date 07/02/2024 15:27
=================================================='''
import numpy as np
from colmap_utils.read_write_model import qvec2rotmat
def read_query_info(query_fn: str, name_prefix='') -> dict:
results = {}
with open(query_fn, 'r') as f:
lines = f.readlines()
for l in lines:
l = l.strip().split()
name, camera_model, width, height = l[:4]
params = np.array(l[4:], float)
info = (camera_model, int(width), int(height), params)
results[name_prefix + name] = info
print('Load {} query images'.format(len(results.keys())))
return results
def quaternion_angular_error(q1, q2):
"""
angular error between two quaternions
:param q1: (4, )
:param q2: (4, )
:return:
"""
d = abs(np.dot(q1, q2))
d = min(1.0, max(-1.0, d))
theta = 2 * np.arccos(d) * 180 / np.pi
return theta
def compute_pose_error(pred_qcw, pred_tcw, gt_qcw, gt_tcw):
pred_Rcw = qvec2rotmat(qvec=pred_qcw)
pred_tcw = np.array(pred_tcw, float).reshape(3, 1)
pred_twc = -pred_Rcw.transpose() @ pred_tcw
gt_Rcw = qvec2rotmat(gt_qcw)
gt_tcw = np.array(gt_tcw, float).reshape(3, 1)
gt_twc = -gt_Rcw.transpose() @ gt_tcw
t_error_xyz = pred_twc - gt_twc
t_error = np.sqrt(np.sum(t_error_xyz ** 2))
q_error = quaternion_angular_error(q1=pred_qcw, q2=gt_qcw)
return q_error, t_error
def read_retrieval_results(path):
output = {}
with open(path, "r") as f:
lines = f.readlines()
for p in lines:
p = p.strip("\n").split(" ")
if p[1] == "no_match":
continue
if p[0] in output.keys():
output[p[0]].append(p[1])
else:
output[p[0]] = [p[1]]
return output
def read_gt_pose(path):
gt_poses = {}
with open(path, 'r') as f:
lines = f.readlines()
for l in lines:
l = l.strip().split(' ')
gt_poses[l[0]] = {
'qvec': np.array([float(v) for v in l[1:5]], float),
'tvec': np.array([float(v) for v in l[5:]], float),
}
return gt_poses
|