Spaces:
Running
Running
File size: 8,162 Bytes
9223079 |
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 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 |
import os
import numpy as np
import logging
from pathlib import Path
from ...utils.read_write_model import qvec2rotmat, rotmat2qvec
from ...utils.read_write_model import Image, write_model, Camera
from ...utils.parsers import parse_retrieval
logger = logging.getLogger(__name__)
def get_timestamps(files, idx):
"""Extract timestamps from a pose or relocalization file."""
lines = []
for p in files.parent.glob(files.name):
with open(p) as f:
lines += f.readlines()
timestamps = set()
for line in lines:
line = line.rstrip("\n")
if line[0] == "#" or line == "":
continue
ts = line.replace(",", " ").split()[idx]
timestamps.add(ts)
return timestamps
def delete_unused_images(root, timestamps):
"""Delete all images in root if they are not contained in timestamps."""
images = list(root.glob("**/*.png"))
deleted = 0
for image in images:
ts = image.stem
if ts not in timestamps:
os.remove(image)
deleted += 1
logger.info(f"Deleted {deleted} images in {root}.")
def camera_from_calibration_file(id_, path):
"""Create a COLMAP camera from an MLAD calibration file."""
with open(path, "r") as f:
data = f.readlines()
model, fx, fy, cx, cy = data[0].split()[:5]
width, height = data[1].split()
assert model == "Pinhole"
model_name = "PINHOLE"
params = [float(i) for i in [fx, fy, cx, cy]]
camera = Camera(
id=id_, model=model_name, width=int(width), height=int(height), params=params
)
return camera
def parse_poses(path, colmap=False):
"""Parse a list of poses in COLMAP or MLAD quaternion convention."""
poses = []
with open(path) as f:
for line in f.readlines():
line = line.rstrip("\n")
if line[0] == "#" or line == "":
continue
data = line.replace(",", " ").split()
ts, p = data[0], np.array(data[1:], float)
if colmap:
q, t = np.split(p, [4])
else:
t, q = np.split(p, [3])
q = q[[3, 0, 1, 2]] # xyzw to wxyz
R = qvec2rotmat(q)
poses.append((ts, R, t))
return poses
def parse_relocalization(path, has_poses=False):
"""Parse a relocalization file, possibly with poses."""
reloc = []
with open(path) as f:
for line in f.readlines():
line = line.rstrip("\n")
if line[0] == "#" or line == "":
continue
data = line.replace(",", " ").split()
out = data[:2] # ref_ts, q_ts
if has_poses:
assert len(data) == 9
t, q = np.split(np.array(data[2:], float), [3])
q = q[[3, 0, 1, 2]] # xyzw to wxyz
R = qvec2rotmat(q)
out += [R, t]
reloc.append(out)
return reloc
def build_empty_colmap_model(root, sfm_dir):
"""Build a COLMAP model with images and cameras only."""
calibration = "Calibration/undistorted_calib_{}.txt"
cam0 = camera_from_calibration_file(0, root / calibration.format(0))
cam1 = camera_from_calibration_file(1, root / calibration.format(1))
cameras = {0: cam0, 1: cam1}
T_0to1 = np.loadtxt(root / "Calibration/undistorted_calib_stereo.txt")
poses = parse_poses(root / "poses.txt")
images = {}
id_ = 0
for ts, R_cam0_to_w, t_cam0_to_w in poses:
R_w_to_cam0 = R_cam0_to_w.T
t_w_to_cam0 = -(R_w_to_cam0 @ t_cam0_to_w)
R_w_to_cam1 = T_0to1[:3, :3] @ R_w_to_cam0
t_w_to_cam1 = T_0to1[:3, :3] @ t_w_to_cam0 + T_0to1[:3, 3]
for idx, (R_w_to_cam, t_w_to_cam) in enumerate(
zip([R_w_to_cam0, R_w_to_cam1], [t_w_to_cam0, t_w_to_cam1])
):
image = Image(
id=id_,
qvec=rotmat2qvec(R_w_to_cam),
tvec=t_w_to_cam,
camera_id=idx,
name=f"cam{idx}/{ts}.png",
xys=np.zeros((0, 2), float),
point3D_ids=np.full(0, -1, int),
)
images[id_] = image
id_ += 1
sfm_dir.mkdir(exist_ok=True, parents=True)
write_model(cameras, images, {}, path=str(sfm_dir), ext=".bin")
def generate_query_lists(timestamps, seq_dir, out_path):
"""Create a list of query images with intrinsics from timestamps."""
cam0 = camera_from_calibration_file(
0, seq_dir / "Calibration/undistorted_calib_0.txt"
)
intrinsics = [cam0.model, cam0.width, cam0.height] + cam0.params
intrinsics = [str(p) for p in intrinsics]
data = map(lambda ts: " ".join([f"cam0/{ts}.png"] + intrinsics), timestamps)
with open(out_path, "w") as f:
f.write("\n".join(data))
def generate_localization_pairs(sequence, reloc, num, ref_pairs, out_path):
"""Create the matching pairs for the localization.
We simply lookup the corresponding reference frame
and extract its `num` closest frames from the existing pair list.
"""
if "test" in sequence:
# hard pairs will be overwritten by easy ones if available
relocs = [str(reloc).replace("*", d) for d in ["hard", "moderate", "easy"]]
else:
relocs = [reloc]
query_to_ref_ts = {}
for reloc in relocs:
with open(reloc, "r") as f:
for line in f.readlines():
line = line.rstrip("\n")
if line[0] == "#" or line == "":
continue
ref_ts, q_ts = line.split()[:2]
query_to_ref_ts[q_ts] = ref_ts
ts_to_name = "cam0/{}.png".format
ref_pairs = parse_retrieval(ref_pairs)
loc_pairs = []
for q_ts, ref_ts in query_to_ref_ts.items():
ref_name = ts_to_name(ref_ts)
selected = [ref_name] + ref_pairs[ref_name][: num - 1]
loc_pairs.extend([" ".join((ts_to_name(q_ts), s)) for s in selected])
with open(out_path, "w") as f:
f.write("\n".join(loc_pairs))
def prepare_submission(results, relocs, poses_path, out_dir):
"""Obtain relative poses from estimated absolute and reference poses."""
gt_poses = parse_poses(poses_path)
all_T_ref0_to_w = {ts: (R, t) for ts, R, t in gt_poses}
pred_poses = parse_poses(results, colmap=True)
all_T_w_to_q0 = {Path(name).stem: (R, t) for name, R, t in pred_poses}
for reloc in relocs.parent.glob(relocs.name):
relative_poses = []
reloc_ts = parse_relocalization(reloc)
for ref_ts, q_ts in reloc_ts:
R_w_to_q0, t_w_to_q0 = all_T_w_to_q0[q_ts]
R_ref0_to_w, t_ref0_to_w = all_T_ref0_to_w[ref_ts]
R_ref0_to_q0 = R_w_to_q0 @ R_ref0_to_w
t_ref0_to_q0 = R_w_to_q0 @ t_ref0_to_w + t_w_to_q0
tvec = t_ref0_to_q0.tolist()
qvec = rotmat2qvec(R_ref0_to_q0)[[1, 2, 3, 0]] # wxyz to xyzw
out = [ref_ts, q_ts] + list(map(str, tvec)) + list(map(str, qvec))
relative_poses.append(" ".join(out))
out_path = out_dir / reloc.name
with open(out_path, "w") as f:
f.write("\n".join(relative_poses))
logger.info(f"Submission file written to {out_path}.")
def evaluate_submission(submission_dir, relocs, ths=[0.1, 0.2, 0.5]):
"""Compute the relocalization recall from predicted and ground truth poses."""
for reloc in relocs.parent.glob(relocs.name):
poses_gt = parse_relocalization(reloc, has_poses=True)
poses_pred = parse_relocalization(submission_dir / reloc.name, has_poses=True)
poses_pred = {(ref_ts, q_ts): (R, t) for ref_ts, q_ts, R, t in poses_pred}
error = []
for ref_ts, q_ts, R_gt, t_gt in poses_gt:
R, t = poses_pred[(ref_ts, q_ts)]
e = np.linalg.norm(t - t_gt)
error.append(e)
error = np.array(error)
recall = [np.mean(error <= th) for th in ths]
s = f"Relocalization evaluation {submission_dir.name}/{reloc.name}\n"
s += " / ".join([f"{th:>7}m" for th in ths]) + "\n"
s += " / ".join([f"{100*r:>7.3f}%" for r in recall])
logger.info(s)
|