lomatch 1.0.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- loma/__init__.py +7 -0
- loma/benchmarks/__init__.py +4 -0
- loma/benchmarks/mega.py +196 -0
- loma/benchmarks/rubik.py +310 -0
- loma/benchmarks/scannet.py +121 -0
- loma/benchmarks/wxbs.py +76 -0
- loma/descriptor/__init__.py +1 -0
- loma/descriptor/dedode.py +407 -0
- loma/descriptor/transformer/__init__.py +1 -0
- loma/descriptor/transformer/dinov2.py +397 -0
- loma/descriptor/transformer/layers/__init__.py +10 -0
- loma/descriptor/transformer/layers/attention.py +59 -0
- loma/descriptor/transformer/layers/block.py +144 -0
- loma/descriptor/transformer/layers/drop_path.py +37 -0
- loma/descriptor/transformer/layers/layer_scale.py +28 -0
- loma/descriptor/transformer/layers/mlp.py +41 -0
- loma/descriptor/transformer/layers/patch_embed.py +101 -0
- loma/detector/__init__.py +1 -0
- loma/detector/dad.py +425 -0
- loma/detector/utils.py +213 -0
- loma/device.py +13 -0
- loma/features.py +52 -0
- loma/geometry.py +849 -0
- loma/io.py +30 -0
- loma/loma.py +498 -0
- loma/loss.py +102 -0
- loma/normalizers.py +17 -0
- loma/random.py +20 -0
- loma/types.py +125 -0
- lomatch-1.0.0.dist-info/METADATA +114 -0
- lomatch-1.0.0.dist-info/RECORD +32 -0
- lomatch-1.0.0.dist-info/WHEEL +4 -0
loma/__init__.py
ADDED
loma/benchmarks/mega.py
ADDED
|
@@ -0,0 +1,196 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
from tqdm import tqdm
|
|
3
|
+
import logging
|
|
4
|
+
|
|
5
|
+
from loma.loma import LoMa
|
|
6
|
+
from loma.geometry import (
|
|
7
|
+
compute_pose_error,
|
|
8
|
+
compute_relative_pose,
|
|
9
|
+
estimate_pose_cv2_ransac,
|
|
10
|
+
pose_auc,
|
|
11
|
+
)
|
|
12
|
+
|
|
13
|
+
logger = logging.getLogger(__name__)
|
|
14
|
+
|
|
15
|
+
|
|
16
|
+
class MegaDepthPoseEstimationBenchmark:
|
|
17
|
+
def __init__(
|
|
18
|
+
self,
|
|
19
|
+
data_root="data/megadepth",
|
|
20
|
+
num_keypoints: int = 4096,
|
|
21
|
+
) -> None:
|
|
22
|
+
self.data_root = data_root
|
|
23
|
+
self.num_keypoints = num_keypoints
|
|
24
|
+
self._post_init()
|
|
25
|
+
|
|
26
|
+
def _post_init(self):
|
|
27
|
+
raise NotImplementedError(
|
|
28
|
+
"Add scene names and benchmark name in derived class _post_init"
|
|
29
|
+
)
|
|
30
|
+
|
|
31
|
+
def benchmark(
|
|
32
|
+
self,
|
|
33
|
+
model: LoMa,
|
|
34
|
+
num_ransac_runs: int = 5,
|
|
35
|
+
sample_every: int = 1,
|
|
36
|
+
):
|
|
37
|
+
self.scenes = [
|
|
38
|
+
np.load(f"{self.data_root}/{scene}", allow_pickle=True)
|
|
39
|
+
for scene in self.scene_names
|
|
40
|
+
]
|
|
41
|
+
|
|
42
|
+
data_root = self.data_root
|
|
43
|
+
thresholds = [5, 10, 20]
|
|
44
|
+
tot_e_t, tot_e_R, tot_e_pose = [], [], []
|
|
45
|
+
for scene_ind in range(len(self.scenes)):
|
|
46
|
+
scene = self.scenes[scene_ind]
|
|
47
|
+
pairs = scene["pair_infos"]
|
|
48
|
+
intrinsics = scene["intrinsics"]
|
|
49
|
+
poses = scene["poses"]
|
|
50
|
+
im_paths = scene["image_paths"]
|
|
51
|
+
pair_inds = range(len(pairs))
|
|
52
|
+
for pairind in (
|
|
53
|
+
pbar := tqdm(
|
|
54
|
+
pair_inds[::sample_every],
|
|
55
|
+
desc="Current AUC: ?",
|
|
56
|
+
mininterval=10,
|
|
57
|
+
)
|
|
58
|
+
):
|
|
59
|
+
idx1, idx2 = pairs[pairind][0]
|
|
60
|
+
K1 = intrinsics[idx1].copy()
|
|
61
|
+
T1 = poses[idx1].copy()
|
|
62
|
+
R1, t1 = T1[:3, :3], T1[:3, 3]
|
|
63
|
+
K2 = intrinsics[idx2].copy()
|
|
64
|
+
T2 = poses[idx2].copy()
|
|
65
|
+
R2, t2 = T2[:3, :3], T2[:3, 3]
|
|
66
|
+
R, t = compute_relative_pose(R1, t1, R2, t2)
|
|
67
|
+
im_A_path = f"{data_root}/{im_paths[idx1]}"
|
|
68
|
+
im_B_path = f"{data_root}/{im_paths[idx2]}"
|
|
69
|
+
|
|
70
|
+
kpts1, kpts2 = model.match(
|
|
71
|
+
im_A_path, im_B_path, num_keypoints=self.num_keypoints
|
|
72
|
+
)
|
|
73
|
+
|
|
74
|
+
for _ in range(num_ransac_runs):
|
|
75
|
+
shuffling = np.random.permutation(np.arange(len(kpts1)))
|
|
76
|
+
kpts1 = kpts1[shuffling]
|
|
77
|
+
kpts2 = kpts2[shuffling]
|
|
78
|
+
|
|
79
|
+
try:
|
|
80
|
+
threshold = 0.5
|
|
81
|
+
norm_threshold = threshold / (
|
|
82
|
+
np.mean(np.abs(K1[:2, :2])) + np.mean(np.abs(K2[:2, :2]))
|
|
83
|
+
)
|
|
84
|
+
R_est, t_est, _ = estimate_pose_cv2_ransac(
|
|
85
|
+
kpts1,
|
|
86
|
+
kpts2,
|
|
87
|
+
K1,
|
|
88
|
+
K2,
|
|
89
|
+
norm_threshold,
|
|
90
|
+
conf=0.99999,
|
|
91
|
+
)
|
|
92
|
+
e_t, e_R = compute_pose_error(R_est, t_est[:, 0], R, t)
|
|
93
|
+
e_pose = max(e_t, e_R)
|
|
94
|
+
except Exception as e:
|
|
95
|
+
logger.debug(f"Pose estimation error: {e}")
|
|
96
|
+
e_t, e_R = 90, 90
|
|
97
|
+
e_pose = max(e_t, e_R)
|
|
98
|
+
tot_e_t.append(e_t)
|
|
99
|
+
tot_e_R.append(e_R)
|
|
100
|
+
tot_e_pose.append(e_pose)
|
|
101
|
+
|
|
102
|
+
pbar.set_postfix(
|
|
103
|
+
auc=f"{[f'{a.item():.3f}' for a in pose_auc(tot_e_pose, thresholds)]}"
|
|
104
|
+
)
|
|
105
|
+
tot_e_pose = np.array(tot_e_pose)
|
|
106
|
+
auc = pose_auc(tot_e_pose, thresholds)
|
|
107
|
+
acc_5 = (tot_e_pose < 5).mean()
|
|
108
|
+
acc_10 = (tot_e_pose < 10).mean()
|
|
109
|
+
acc_15 = (tot_e_pose < 15).mean()
|
|
110
|
+
acc_20 = (tot_e_pose < 20).mean()
|
|
111
|
+
map_5 = acc_5
|
|
112
|
+
map_10 = np.mean([acc_5, acc_10])
|
|
113
|
+
map_20 = np.mean([acc_5, acc_10, acc_15, acc_20])
|
|
114
|
+
logger.info("%s auc: %s", model.name, auc)
|
|
115
|
+
return {
|
|
116
|
+
"auc_5": auc[0],
|
|
117
|
+
"auc_10": auc[1],
|
|
118
|
+
"auc_20": auc[2],
|
|
119
|
+
"map_5": map_5,
|
|
120
|
+
"map_10": map_10,
|
|
121
|
+
"map_20": map_20,
|
|
122
|
+
}
|
|
123
|
+
|
|
124
|
+
|
|
125
|
+
class Mega1500(MegaDepthPoseEstimationBenchmark):
|
|
126
|
+
def _post_init(self):
|
|
127
|
+
self.scene_names = [
|
|
128
|
+
"0015_0.1_0.3.npz",
|
|
129
|
+
"0015_0.3_0.5.npz",
|
|
130
|
+
"0022_0.1_0.3.npz",
|
|
131
|
+
"0022_0.3_0.5.npz",
|
|
132
|
+
"0022_0.5_0.7.npz",
|
|
133
|
+
]
|
|
134
|
+
self.benchmark_name = "Mega1500"
|
|
135
|
+
self.model = "essential"
|
|
136
|
+
|
|
137
|
+
|
|
138
|
+
class Mega1500_F(MegaDepthPoseEstimationBenchmark):
|
|
139
|
+
def _post_init(self):
|
|
140
|
+
self.scene_names = [
|
|
141
|
+
"0015_0.1_0.3.npz",
|
|
142
|
+
"0015_0.3_0.5.npz",
|
|
143
|
+
"0022_0.1_0.3.npz",
|
|
144
|
+
"0022_0.3_0.5.npz",
|
|
145
|
+
"0022_0.5_0.7.npz",
|
|
146
|
+
]
|
|
147
|
+
# self.benchmark_name = "Mega1500_F"
|
|
148
|
+
self.model = "fundamental"
|
|
149
|
+
|
|
150
|
+
|
|
151
|
+
class MegaIMCPT(MegaDepthPoseEstimationBenchmark):
|
|
152
|
+
def _post_init(self):
|
|
153
|
+
self.scene_names = [
|
|
154
|
+
"mega_8_scenes_0008_0.1_0.3.npz",
|
|
155
|
+
"mega_8_scenes_0008_0.3_0.5.npz",
|
|
156
|
+
"mega_8_scenes_0019_0.1_0.3.npz",
|
|
157
|
+
"mega_8_scenes_0019_0.3_0.5.npz",
|
|
158
|
+
"mega_8_scenes_0021_0.1_0.3.npz",
|
|
159
|
+
"mega_8_scenes_0021_0.3_0.5.npz",
|
|
160
|
+
"mega_8_scenes_0024_0.1_0.3.npz",
|
|
161
|
+
"mega_8_scenes_0024_0.3_0.5.npz",
|
|
162
|
+
"mega_8_scenes_0025_0.1_0.3.npz",
|
|
163
|
+
"mega_8_scenes_0025_0.3_0.5.npz",
|
|
164
|
+
"mega_8_scenes_0032_0.1_0.3.npz",
|
|
165
|
+
"mega_8_scenes_0032_0.3_0.5.npz",
|
|
166
|
+
"mega_8_scenes_0063_0.1_0.3.npz",
|
|
167
|
+
"mega_8_scenes_0063_0.3_0.5.npz",
|
|
168
|
+
"mega_8_scenes_1589_0.1_0.3.npz",
|
|
169
|
+
"mega_8_scenes_1589_0.3_0.5.npz",
|
|
170
|
+
]
|
|
171
|
+
# self.benchmark_name = "MegaIMCPT"
|
|
172
|
+
self.model = "essential"
|
|
173
|
+
|
|
174
|
+
|
|
175
|
+
class MegaIMCPT_F(MegaDepthPoseEstimationBenchmark):
|
|
176
|
+
def _post_init(self):
|
|
177
|
+
self.scene_names = [
|
|
178
|
+
"mega_8_scenes_0008_0.1_0.3.npz",
|
|
179
|
+
"mega_8_scenes_0008_0.3_0.5.npz",
|
|
180
|
+
"mega_8_scenes_0019_0.1_0.3.npz",
|
|
181
|
+
"mega_8_scenes_0019_0.3_0.5.npz",
|
|
182
|
+
"mega_8_scenes_0021_0.1_0.3.npz",
|
|
183
|
+
"mega_8_scenes_0021_0.3_0.5.npz",
|
|
184
|
+
"mega_8_scenes_0024_0.1_0.3.npz",
|
|
185
|
+
"mega_8_scenes_0024_0.3_0.5.npz",
|
|
186
|
+
"mega_8_scenes_0025_0.1_0.3.npz",
|
|
187
|
+
"mega_8_scenes_0025_0.3_0.5.npz",
|
|
188
|
+
"mega_8_scenes_0032_0.1_0.3.npz",
|
|
189
|
+
"mega_8_scenes_0032_0.3_0.5.npz",
|
|
190
|
+
"mega_8_scenes_0063_0.1_0.3.npz",
|
|
191
|
+
"mega_8_scenes_0063_0.3_0.5.npz",
|
|
192
|
+
"mega_8_scenes_1589_0.1_0.3.npz",
|
|
193
|
+
"mega_8_scenes_1589_0.3_0.5.npz",
|
|
194
|
+
]
|
|
195
|
+
# self.benchmark_name = "MegaIMCPT_F"
|
|
196
|
+
self.model = "fundamental"
|
loma/benchmarks/rubik.py
ADDED
|
@@ -0,0 +1,310 @@
|
|
|
1
|
+
import ast
|
|
2
|
+
import json
|
|
3
|
+
import cv2
|
|
4
|
+
import numpy as np
|
|
5
|
+
import os.path as osp
|
|
6
|
+
from tqdm import tqdm
|
|
7
|
+
from scipy.optimize import least_squares
|
|
8
|
+
from dataclasses import dataclass
|
|
9
|
+
from typing import Literal
|
|
10
|
+
from urllib.request import urlopen
|
|
11
|
+
|
|
12
|
+
from loma.geometry import pose_auc
|
|
13
|
+
from loma.random import set_seed
|
|
14
|
+
from loma.loma import LoMa
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
# Reference code: https://github.com/thibautloiseau/RUBIK/blob/main/eval.py
|
|
18
|
+
class RubikBenchmark:
|
|
19
|
+
@dataclass(frozen=True)
|
|
20
|
+
class Cfg:
|
|
21
|
+
# Preprocess the data by following the instructions at https://github.com/thibautloiseau/RUBIK/tree/main
|
|
22
|
+
data_path: str = "data/nuScenes"
|
|
23
|
+
seed: int | None = None
|
|
24
|
+
estimate_pose_method: Literal["essential", "fundamental"] = "essential"
|
|
25
|
+
num_keypoints: int = 4096
|
|
26
|
+
|
|
27
|
+
def __init__(
|
|
28
|
+
self,
|
|
29
|
+
cfg: Cfg | None = None,
|
|
30
|
+
) -> None:
|
|
31
|
+
if cfg is None:
|
|
32
|
+
cfg = RubikBenchmark.Cfg()
|
|
33
|
+
self.cfg = cfg
|
|
34
|
+
if cfg.seed is not None:
|
|
35
|
+
set_seed(cfg.seed)
|
|
36
|
+
self.prefix = f"{type(self).__name__}"
|
|
37
|
+
|
|
38
|
+
def benchmark(self, model: LoMa):
|
|
39
|
+
estimate_pose = self.cfg.estimate_pose_method
|
|
40
|
+
with urlopen(
|
|
41
|
+
"https://raw.githubusercontent.com/thibautloiseau/RUBIK/refs/heads/main/rubik.json"
|
|
42
|
+
) as f:
|
|
43
|
+
data = json.load(f)
|
|
44
|
+
|
|
45
|
+
# Get all scenes to get intrinsics to be able to recover pose
|
|
46
|
+
all_scenes = list(set([scene for box in data for scene in data[box]]))
|
|
47
|
+
|
|
48
|
+
# Collect errors for AUC computation
|
|
49
|
+
tot_R_err = []
|
|
50
|
+
tot_t_err_angle = []
|
|
51
|
+
tot_t_err_metric = []
|
|
52
|
+
tot_pose_err = [] # max of R_err and t_err_angle
|
|
53
|
+
|
|
54
|
+
# Success is defined in the paper as rotation error less than 5 degrees and translation error less than 2m
|
|
55
|
+
success = 0
|
|
56
|
+
total = 0
|
|
57
|
+
|
|
58
|
+
for scene in tqdm(all_scenes):
|
|
59
|
+
# We iterate over scenes whether than boxes to avoid loading all images from every scene at once
|
|
60
|
+
for box in (pbar := tqdm(data, leave=False)):
|
|
61
|
+
if data[box].get(scene) is None:
|
|
62
|
+
continue
|
|
63
|
+
|
|
64
|
+
pairs = [ast.literal_eval(el) for el in list(data[box][scene].keys())]
|
|
65
|
+
paths = [
|
|
66
|
+
[
|
|
67
|
+
osp.join(
|
|
68
|
+
self.cfg.data_path,
|
|
69
|
+
"sweeps",
|
|
70
|
+
el[0].split("__")[1].split("__")[0],
|
|
71
|
+
el[0],
|
|
72
|
+
),
|
|
73
|
+
osp.join(
|
|
74
|
+
self.cfg.data_path,
|
|
75
|
+
"sweeps",
|
|
76
|
+
el[1].split("__")[1].split("__")[0],
|
|
77
|
+
el[1],
|
|
78
|
+
),
|
|
79
|
+
]
|
|
80
|
+
for el in pairs
|
|
81
|
+
]
|
|
82
|
+
|
|
83
|
+
for i, pair in enumerate(paths):
|
|
84
|
+
# Get gt pose
|
|
85
|
+
gt_pose = np.array(data[box][scene][str(pairs[i])]["rel_pose"])
|
|
86
|
+
|
|
87
|
+
mkpts1, mkpts2 = model.match(
|
|
88
|
+
pair[0], pair[1], num_keypoints=self.cfg.num_keypoints
|
|
89
|
+
)
|
|
90
|
+
|
|
91
|
+
# Estimate pose
|
|
92
|
+
K1 = np.array(data[box][scene][str(pairs[i])]["K1"])
|
|
93
|
+
K2 = np.array(data[box][scene][str(pairs[i])]["K2"])
|
|
94
|
+
ret = None
|
|
95
|
+
if estimate_pose == "essential":
|
|
96
|
+
ret = self.estimate_pose_essential(mkpts1, mkpts2, K1, K2, 0.5)
|
|
97
|
+
|
|
98
|
+
elif estimate_pose == "fundamental":
|
|
99
|
+
ret = self.estimate_pose_fundamental(
|
|
100
|
+
mkpts1, mkpts2, K1, K2, 0.5
|
|
101
|
+
)
|
|
102
|
+
|
|
103
|
+
if ret is None:
|
|
104
|
+
R_err = 90 # Failed pose estimation
|
|
105
|
+
t_err_angle = 90
|
|
106
|
+
t_err_metric = np.inf
|
|
107
|
+
else:
|
|
108
|
+
R_est, t_est, _ = ret
|
|
109
|
+
|
|
110
|
+
# Normalize t_est
|
|
111
|
+
t_est = t_est / np.linalg.norm(t_est)
|
|
112
|
+
|
|
113
|
+
# Get scale factor using unidepths by minimizing distance to 3D points after applying transformation
|
|
114
|
+
depth1 = np.load(
|
|
115
|
+
f"{self.cfg.data_path}/unidepths/{osp.basename(pair[0]).replace('.jpg', '.npy')}"
|
|
116
|
+
)
|
|
117
|
+
depth2 = np.load(
|
|
118
|
+
f"{self.cfg.data_path}/unidepths/{osp.basename(pair[1]).replace('.jpg', '.npy')}"
|
|
119
|
+
)
|
|
120
|
+
|
|
121
|
+
# Get 3D points
|
|
122
|
+
pts3D_1 = self.backproject_to_3D(mkpts1, depth1, K1)
|
|
123
|
+
pts3D_2 = self.backproject_to_3D(mkpts2, depth2, K2)
|
|
124
|
+
|
|
125
|
+
# Get scale factor
|
|
126
|
+
scale = self.get_scale(
|
|
127
|
+
self.scale_cost_function, 1, R_est, t_est, pts3D_1, pts3D_2
|
|
128
|
+
)
|
|
129
|
+
t_est *= scale
|
|
130
|
+
|
|
131
|
+
# Compute error
|
|
132
|
+
t_err_angle, t_err_metric, R_err = self.relative_pose_error(
|
|
133
|
+
gt_pose, R_est, t_est
|
|
134
|
+
)
|
|
135
|
+
|
|
136
|
+
tot_R_err.append(R_err)
|
|
137
|
+
tot_t_err_angle.append(t_err_angle)
|
|
138
|
+
tot_t_err_metric.append(t_err_metric)
|
|
139
|
+
tot_pose_err.append(max(R_err, t_err_angle))
|
|
140
|
+
|
|
141
|
+
total += 1
|
|
142
|
+
if (R_err < 5.0) and (t_err_metric < 2.0):
|
|
143
|
+
success += 1
|
|
144
|
+
pbar.set_postfix(
|
|
145
|
+
auc=f"{[f'{a.item() * 100:.1f}' for a in pose_auc(tot_pose_err, [5, 10, 20])]}",
|
|
146
|
+
success_ratio=f"{success / total:.1f}",
|
|
147
|
+
)
|
|
148
|
+
|
|
149
|
+
# Compute summary statistics
|
|
150
|
+
tot_R_err = np.array(tot_R_err)
|
|
151
|
+
tot_t_err_metric = np.array(tot_t_err_metric)
|
|
152
|
+
tot_pose_err = np.array(tot_pose_err)
|
|
153
|
+
|
|
154
|
+
# AUC for rotation error (thresholds in degrees)
|
|
155
|
+
R_thresholds = [5, 10, 20]
|
|
156
|
+
R_auc = pose_auc(tot_R_err, R_thresholds)
|
|
157
|
+
|
|
158
|
+
# AUC for translation error (thresholds in meters)
|
|
159
|
+
t_thresholds = [0.5, 1.0, 2.0]
|
|
160
|
+
t_auc = pose_auc(tot_t_err_metric, t_thresholds)
|
|
161
|
+
|
|
162
|
+
# AUC for combined pose error (max of R and t angular errors, thresholds in degrees)
|
|
163
|
+
pose_thresholds = [5, 10, 20]
|
|
164
|
+
pose_auc_vals = pose_auc(tot_pose_err, pose_thresholds)
|
|
165
|
+
|
|
166
|
+
# Success ratio
|
|
167
|
+
success_ratio = success / total if total > 0 else 0.0
|
|
168
|
+
|
|
169
|
+
return {
|
|
170
|
+
f"{self.prefix}_auc_5": pose_auc_vals[0] * 100,
|
|
171
|
+
f"{self.prefix}_auc_10": pose_auc_vals[1] * 100,
|
|
172
|
+
f"{self.prefix}_auc_20": pose_auc_vals[2] * 100,
|
|
173
|
+
f"{self.prefix}_R_auc_5": R_auc[0] * 100,
|
|
174
|
+
f"{self.prefix}_R_auc_10": R_auc[1] * 100,
|
|
175
|
+
f"{self.prefix}_R_auc_20": R_auc[2] * 100,
|
|
176
|
+
f"{self.prefix}_t_auc_0.5": t_auc[0] * 100,
|
|
177
|
+
f"{self.prefix}_t_auc_1.0": t_auc[1] * 100,
|
|
178
|
+
f"{self.prefix}_t_auc_2.0": t_auc[2] * 100,
|
|
179
|
+
f"{self.prefix}_success_ratio": success_ratio * 100,
|
|
180
|
+
}
|
|
181
|
+
|
|
182
|
+
@staticmethod
|
|
183
|
+
def estimate_pose_essential(kpts0, kpts1, K0, K1, thresh, conf=0.99999):
|
|
184
|
+
# From https://github.com/zju3dv/LoFTR/blob/master/src/utils/metrics.py#L72
|
|
185
|
+
if len(kpts0) < 5:
|
|
186
|
+
return None
|
|
187
|
+
|
|
188
|
+
# normalize keypoints
|
|
189
|
+
kpts0 = (kpts0 - K0[[0, 1], [2, 2]][None]) / K0[[0, 1], [0, 1]][None]
|
|
190
|
+
kpts1 = (kpts1 - K1[[0, 1], [2, 2]][None]) / K1[[0, 1], [0, 1]][None]
|
|
191
|
+
|
|
192
|
+
# normalize ransac threshold
|
|
193
|
+
ransac_thr = thresh / np.mean([K0[0, 0], K1[1, 1], K0[0, 0], K1[1, 1]])
|
|
194
|
+
|
|
195
|
+
# compute pose with cv2
|
|
196
|
+
E, mask = cv2.findEssentialMat(
|
|
197
|
+
kpts0,
|
|
198
|
+
kpts1,
|
|
199
|
+
np.eye(3),
|
|
200
|
+
threshold=ransac_thr,
|
|
201
|
+
prob=conf,
|
|
202
|
+
method=cv2.USAC_MAGSAC,
|
|
203
|
+
)
|
|
204
|
+
|
|
205
|
+
if E is None:
|
|
206
|
+
return None
|
|
207
|
+
|
|
208
|
+
# recover pose from E
|
|
209
|
+
best_num_inliers = 0
|
|
210
|
+
ret = None
|
|
211
|
+
for _E in np.split(E, len(E) // 3):
|
|
212
|
+
pose = cv2.recoverPose(_E, kpts0, kpts1, np.eye(3), 1e9, mask=mask)
|
|
213
|
+
n = int(pose[0])
|
|
214
|
+
R = pose[1]
|
|
215
|
+
t = pose[2]
|
|
216
|
+
if n > best_num_inliers:
|
|
217
|
+
ret = (R, t[:, 0], mask.ravel() > 0)
|
|
218
|
+
best_num_inliers = n
|
|
219
|
+
|
|
220
|
+
return ret
|
|
221
|
+
|
|
222
|
+
@staticmethod
|
|
223
|
+
def estimate_pose_fundamental(kpts0, kpts1, K0, K1, thresh, conf=0.99999):
|
|
224
|
+
if len(kpts0) < 7:
|
|
225
|
+
return None
|
|
226
|
+
|
|
227
|
+
# Find fundamental matrix using RANSAC
|
|
228
|
+
try:
|
|
229
|
+
F, mask = cv2.findFundamentalMat(
|
|
230
|
+
kpts0,
|
|
231
|
+
kpts1,
|
|
232
|
+
ransacReprojThreshold=thresh,
|
|
233
|
+
confidence=conf,
|
|
234
|
+
method=cv2.USAC_MAGSAC,
|
|
235
|
+
)
|
|
236
|
+
except Exception as _:
|
|
237
|
+
return None
|
|
238
|
+
|
|
239
|
+
if F is None:
|
|
240
|
+
return None
|
|
241
|
+
|
|
242
|
+
# Convert fundamental matrix to essential matrix using intrinsics
|
|
243
|
+
E = K1.T @ F @ K0
|
|
244
|
+
|
|
245
|
+
# Recover pose from essential matrix
|
|
246
|
+
best_num_inliers = 0
|
|
247
|
+
ret = None
|
|
248
|
+
for _E in np.split(E, len(E) // 3):
|
|
249
|
+
pose = cv2.recoverPose(_E, kpts0, kpts1, np.eye(3), 1e9, mask=mask)
|
|
250
|
+
n = int(pose[0])
|
|
251
|
+
R = pose[1]
|
|
252
|
+
t = pose[2]
|
|
253
|
+
if n > best_num_inliers:
|
|
254
|
+
ret = (R, t[:, 0], mask.ravel() > 0)
|
|
255
|
+
best_num_inliers = n
|
|
256
|
+
|
|
257
|
+
return ret
|
|
258
|
+
|
|
259
|
+
@staticmethod
|
|
260
|
+
def relative_pose_error(T_0to1, R, t, ignore_gt_t_thr=0.0):
|
|
261
|
+
# Angle error between 2 vectors
|
|
262
|
+
t_gt = T_0to1[:3, 3]
|
|
263
|
+
n = np.linalg.norm(t) * np.linalg.norm(t_gt)
|
|
264
|
+
t_err_angle = np.rad2deg(np.arccos(np.clip(np.dot(t, t_gt) / n, -1.0, 1.0)))
|
|
265
|
+
t_err_angle = np.minimum(t_err_angle, 180 - t_err_angle) # handle E ambiguity
|
|
266
|
+
if np.linalg.norm(t_gt) < ignore_gt_t_thr: # pure rotation is challenging
|
|
267
|
+
t_err_angle = 0
|
|
268
|
+
|
|
269
|
+
# Metric translation error
|
|
270
|
+
t_err_metric = np.linalg.norm(t - t_gt)
|
|
271
|
+
|
|
272
|
+
# Angle error between 2 rotation matrices
|
|
273
|
+
R_gt = T_0to1[:3, :3]
|
|
274
|
+
cos = (np.trace(np.dot(R.T, R_gt)) - 1) / 2
|
|
275
|
+
cos = np.clip(cos, -1.0, 1.0) # handle numerical errors
|
|
276
|
+
R_err = np.rad2deg(np.abs(np.arccos(cos)))
|
|
277
|
+
|
|
278
|
+
return t_err_angle, t_err_metric, R_err
|
|
279
|
+
|
|
280
|
+
@staticmethod
|
|
281
|
+
def backproject_to_3D(uv_points, depth, K):
|
|
282
|
+
# Convert 2D points to homogeneous for and clip points outside image
|
|
283
|
+
uv_homogeneous = (
|
|
284
|
+
np.hstack((uv_points, np.ones((uv_points.shape[0], 1)))).round().astype(int)
|
|
285
|
+
)
|
|
286
|
+
uv_homogeneous = np.clip(uv_homogeneous, a_min=(0, 0, 1), a_max=(1599, 899, 1))
|
|
287
|
+
|
|
288
|
+
selected_depths = depth[uv_homogeneous[:, 1], uv_homogeneous[:, 0]]
|
|
289
|
+
|
|
290
|
+
# Scale by depth
|
|
291
|
+
points_3D = selected_depths[:, None] * (np.linalg.inv(K) @ (uv_homogeneous.T)).T
|
|
292
|
+
return points_3D
|
|
293
|
+
|
|
294
|
+
@staticmethod
|
|
295
|
+
def scale_cost_function(scale, R, t, pts3D_1, pts3D_2, delta=1.0):
|
|
296
|
+
# Apply transformation
|
|
297
|
+
pts3D_1 = (R @ pts3D_1.T).T + scale * t
|
|
298
|
+
res = np.linalg.norm(pts3D_1 - pts3D_2, axis=1)
|
|
299
|
+
|
|
300
|
+
# Huber loss with 1m threshold
|
|
301
|
+
cost = np.where(res <= delta, 0.5 * res**2, delta * (res - 0.5 * delta))
|
|
302
|
+
return cost
|
|
303
|
+
|
|
304
|
+
@staticmethod
|
|
305
|
+
def get_scale(func, scale_ini, R, t, pts3D_1, pts3D_2):
|
|
306
|
+
res = least_squares(
|
|
307
|
+
func, scale_ini, args=(R, t, pts3D_1, pts3D_2), bounds=(0, np.inf)
|
|
308
|
+
)
|
|
309
|
+
scale = res.x[0]
|
|
310
|
+
return scale
|
|
@@ -0,0 +1,121 @@
|
|
|
1
|
+
import os.path as osp
|
|
2
|
+
import logging
|
|
3
|
+
import numpy as np
|
|
4
|
+
from tqdm import tqdm
|
|
5
|
+
|
|
6
|
+
from loma.geometry import compute_pose_error, pose_auc, estimate_pose_cv2_ransac
|
|
7
|
+
from loma.loma import LoMa
|
|
8
|
+
|
|
9
|
+
logger = logging.getLogger(__name__)
|
|
10
|
+
|
|
11
|
+
|
|
12
|
+
class ScanNet1500:
|
|
13
|
+
def __init__(
|
|
14
|
+
self, data_root="data/scannet/scans", num_keypoints: int = 4096
|
|
15
|
+
) -> None:
|
|
16
|
+
self.data_root = data_root
|
|
17
|
+
self.num_keypoints = num_keypoints
|
|
18
|
+
|
|
19
|
+
def benchmark(
|
|
20
|
+
self,
|
|
21
|
+
model: LoMa,
|
|
22
|
+
num_ransac_runs: int = 5,
|
|
23
|
+
):
|
|
24
|
+
thresholds = [5, 10, 20]
|
|
25
|
+
data_root = self.data_root
|
|
26
|
+
tmp = np.load(osp.join(data_root, "test.npz"))
|
|
27
|
+
pairs, rel_pose = tmp["name"], tmp["rel_pose"]
|
|
28
|
+
tot_e_t, tot_e_R, tot_e_pose = [], [], []
|
|
29
|
+
pair_inds = range(len(pairs))
|
|
30
|
+
for pairind in (pbar := tqdm(pair_inds, smoothing=0.9)):
|
|
31
|
+
scene = pairs[pairind]
|
|
32
|
+
scene_name = f"scene0{scene[0]}_00"
|
|
33
|
+
im_A_path = osp.join(
|
|
34
|
+
self.data_root,
|
|
35
|
+
"scans_test",
|
|
36
|
+
scene_name,
|
|
37
|
+
"color",
|
|
38
|
+
f"{scene[2]}.jpg",
|
|
39
|
+
)
|
|
40
|
+
im_B_path = osp.join(
|
|
41
|
+
self.data_root,
|
|
42
|
+
"scans_test",
|
|
43
|
+
scene_name,
|
|
44
|
+
"color",
|
|
45
|
+
f"{scene[3]}.jpg",
|
|
46
|
+
)
|
|
47
|
+
T_gt = rel_pose[pairind].reshape(3, 4)
|
|
48
|
+
R, t = T_gt[:3, :3], T_gt[:3, 3]
|
|
49
|
+
intrinsic_path = osp.join(
|
|
50
|
+
self.data_root,
|
|
51
|
+
"scans_test",
|
|
52
|
+
scene_name,
|
|
53
|
+
"intrinsic",
|
|
54
|
+
"intrinsic_color.txt",
|
|
55
|
+
)
|
|
56
|
+
with open(intrinsic_path, "r") as f:
|
|
57
|
+
K = np.stack(
|
|
58
|
+
[
|
|
59
|
+
np.array([float(i) for i in r.split()])
|
|
60
|
+
for r in f.read().split("\n")
|
|
61
|
+
if r
|
|
62
|
+
]
|
|
63
|
+
)
|
|
64
|
+
|
|
65
|
+
kpts1, kpts2 = model.match(im_A_path, im_B_path)
|
|
66
|
+
|
|
67
|
+
# scale1 = 480 / min(w1, h1)
|
|
68
|
+
# scale2 = 480 / min(w2, h2)
|
|
69
|
+
# w1, h1 = scale1 * w1, scale1 * h1
|
|
70
|
+
# w2, h2 = scale2 * w2, scale2 * h2
|
|
71
|
+
K1 = K.copy()
|
|
72
|
+
K2 = K.copy()
|
|
73
|
+
# K1[:2] = K1[:2] * scale1
|
|
74
|
+
# K2[:2] = K2[:2] * scale2
|
|
75
|
+
|
|
76
|
+
for _ in range(num_ransac_runs):
|
|
77
|
+
shuffling = np.random.permutation(np.arange(len(kpts1)))
|
|
78
|
+
kpts1 = kpts1[shuffling]
|
|
79
|
+
kpts2 = kpts2[shuffling]
|
|
80
|
+
try:
|
|
81
|
+
norm_threshold = 0.5 / (
|
|
82
|
+
np.mean(np.abs(K1[:2, :2])) + np.mean(np.abs(K2[:2, :2]))
|
|
83
|
+
)
|
|
84
|
+
R_est, t_est, mask = estimate_pose_cv2_ransac(
|
|
85
|
+
kpts1,
|
|
86
|
+
kpts2,
|
|
87
|
+
K1,
|
|
88
|
+
K2,
|
|
89
|
+
norm_threshold,
|
|
90
|
+
conf=0.99999,
|
|
91
|
+
)
|
|
92
|
+
e_t, e_R = compute_pose_error(R_est, t_est[:, 0], R, t)
|
|
93
|
+
e_pose = max(e_t, e_R)
|
|
94
|
+
except Exception as e:
|
|
95
|
+
logger.debug(f"Pose estimation error: {e}")
|
|
96
|
+
e_t, e_R = 90, 90
|
|
97
|
+
e_pose = max(e_t, e_R)
|
|
98
|
+
tot_e_t.append(e_t)
|
|
99
|
+
tot_e_R.append(e_R)
|
|
100
|
+
tot_e_pose.append(e_pose)
|
|
101
|
+
pbar.set_postfix(
|
|
102
|
+
auc=f"{[f'{a.item():.3f}' for a in pose_auc(tot_e_pose, thresholds)]}"
|
|
103
|
+
)
|
|
104
|
+
tot_e_pose = np.array(tot_e_pose)
|
|
105
|
+
auc = pose_auc(tot_e_pose, thresholds)
|
|
106
|
+
acc_5 = (tot_e_pose < 5).mean()
|
|
107
|
+
acc_10 = (tot_e_pose < 10).mean()
|
|
108
|
+
acc_15 = (tot_e_pose < 15).mean()
|
|
109
|
+
acc_20 = (tot_e_pose < 20).mean()
|
|
110
|
+
map_5 = acc_5
|
|
111
|
+
map_10 = np.mean([acc_5, acc_10])
|
|
112
|
+
map_20 = np.mean([acc_5, acc_10, acc_15, acc_20])
|
|
113
|
+
logger.info("%s auc: %s", model.name, auc)
|
|
114
|
+
return {
|
|
115
|
+
"auc_5": auc[0],
|
|
116
|
+
"auc_10": auc[1],
|
|
117
|
+
"auc_20": auc[2],
|
|
118
|
+
"map_5": map_5,
|
|
119
|
+
"map_10": map_10,
|
|
120
|
+
"map_20": map_20,
|
|
121
|
+
}
|