Vincentqyw
fix: roma
c74a070
raw
history blame
5.71 kB
import argparse
import cv2
import numpy as np
import os
import math
import subprocess
from tqdm import tqdm
def compute_essential(matched_kp1, matched_kp2, K):
pts1 = cv2.undistortPoints(
matched_kp1,
cameraMatrix=K,
distCoeffs=(-0.117918271740560, 0.075246403574314, 0, 0),
)
pts2 = cv2.undistortPoints(
matched_kp2,
cameraMatrix=K,
distCoeffs=(-0.117918271740560, 0.075246403574314, 0, 0),
)
K_1 = np.eye(3)
# Estimate the homography between the matches using RANSAC
ransac_model, ransac_inliers = cv2.findEssentialMat(
pts1, pts2, K_1, method=cv2.RANSAC, prob=0.999, threshold=0.001, maxIters=10000
)
if ransac_inliers is None or ransac_model.shape != (3, 3):
ransac_inliers = np.array([])
ransac_model = None
return ransac_model, ransac_inliers, pts1, pts2
def compute_error(R_GT, t_GT, E, pts1_norm, pts2_norm, inliers):
"""Compute the angular error between two rotation matrices and two translation vectors.
Keyword arguments:
R -- 2D numpy array containing an estimated rotation
gt_R -- 2D numpy array containing the corresponding ground truth rotation
t -- 2D numpy array containing an estimated translation as column
gt_t -- 2D numpy array containing the corresponding ground truth translation
"""
inliers = inliers.ravel()
R = np.eye(3)
t = np.zeros((3, 1))
sst = True
try:
_, R, t, _ = cv2.recoverPose(E, pts1_norm, pts2_norm, np.eye(3), inliers)
except:
sst = False
# calculate angle between provided rotations
#
if sst:
dR = np.matmul(R, np.transpose(R_GT))
dR = cv2.Rodrigues(dR)[0]
dR = np.linalg.norm(dR) * 180 / math.pi
# calculate angle between provided translations
dT = float(np.dot(t_GT.T, t))
dT /= float(np.linalg.norm(t_GT))
if dT > 1 or dT < -1:
print("Domain warning! dT:", dT)
dT = max(-1, min(1, dT))
dT = math.acos(dT) * 180 / math.pi
dT = np.minimum(dT, 180 - dT) # ambiguity of E estimation
else:
dR, dT = 180.0, 180.0
return dR, dT
def pose_evaluation(result_base_dir, dark_name1, dark_name2, enhancer, K, R_GT, t_GT):
try:
m_kp1 = np.load(result_base_dir + enhancer + "/DarkFeat/POINT_1/" + dark_name1)
m_kp2 = np.load(result_base_dir + enhancer + "/DarkFeat/POINT_2/" + dark_name2)
except:
return 180.0, 180.0
try:
E, inliers, pts1, pts2 = compute_essential(m_kp1, m_kp2, K)
except:
E, inliers, pts1, pts2 = np.zeros((3, 3)), np.array([]), None, None
dR, dT = compute_error(R_GT, t_GT, E, pts1, pts2, inliers)
return dR, dT
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--histeq", action="store_true")
parser.add_argument("--dataset_dir", type=str, default="/data/hyz/MID/")
opt = parser.parse_args()
sizer = (960, 640)
focallength_x = 4.504986436499113e03 / (6744 / sizer[0])
focallength_y = 4.513311442889859e03 / (4502 / sizer[1])
K = np.eye(3)
K[0, 0] = focallength_x
K[1, 1] = focallength_y
K[0, 2] = 3.363322177533149e03 / (6744 / sizer[0])
K[1, 2] = 2.291824660547715e03 / (4502 / sizer[1])
Kinv = np.linalg.inv(K)
Kinvt = np.transpose(Kinv)
PE_MT = np.zeros((6, 8))
enhancer = "None" if not opt.histeq else "HistEQ"
for scene in ["Indoor", "Outdoor"]:
dir_base = opt.dataset_dir + "/" + scene + "/"
base_save = "result_errors/" + scene + "/"
pair_list = sorted(os.listdir(dir_base))
os.makedirs(base_save, exist_ok=True)
for pair in tqdm(pair_list):
opention = 1
if scene == "Outdoor":
pass
else:
if int(pair[4::]) <= 17:
opention = 0
else:
pass
name = []
files = sorted(os.listdir(dir_base + pair))
for file_ in files:
if file_.endswith(".cr2"):
name.append(file_[0:9])
ISO = [
"00100",
"00200",
"00400",
"00800",
"01600",
"03200",
"06400",
"12800",
]
if opention == 1:
Shutter_speed = ["0.005", "0.01", "0.025", "0.05", "0.17", "0.5"]
else:
Shutter_speed = ["0.01", "0.02", "0.05", "0.1", "0.3", "1"]
E_GT = np.load(dir_base + pair + "/GT_Correspondence/" + "E_estimated.npy")
F_GT = np.dot(np.dot(Kinvt, E_GT), Kinv)
R_GT = np.load(dir_base + pair + "/GT_Correspondence/" + "R_GT.npy")
t_GT = np.load(dir_base + pair + "/GT_Correspondence/" + "T_GT.npy")
result_base_dir = "result/" + scene + "/" + pair + "/"
for iso in ISO:
for ex in Shutter_speed:
dark_name1 = name[0] + iso + "_" + ex + "_" + scene + ".npy"
dark_name2 = name[1] + iso + "_" + ex + "_" + scene + ".npy"
dr, dt = pose_evaluation(
result_base_dir, dark_name1, dark_name2, enhancer, K, R_GT, t_GT
)
PE_MT[Shutter_speed.index(ex), ISO.index(iso)] = max(dr, dt)
subprocess.check_output(
["mkdir", "-p", base_save + pair + f"/{enhancer}/"]
)
np.save(
base_save + pair + f"/{enhancer}/Pose_error_DarkFeat.npy", PE_MT
)