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 ADDED
@@ -0,0 +1,7 @@
1
+ from .loma import (
2
+ LoMa as LoMa,
3
+ LoMaB as LoMaB,
4
+ LoMaL as LoMaL,
5
+ LoMaG as LoMaG,
6
+ LoMaB128 as LoMaB128,
7
+ )
@@ -0,0 +1,4 @@
1
+ from .mega import Mega1500 as Mega1500
2
+ from .scannet import ScanNet1500 as ScanNet1500
3
+ from .wxbs import WxBSBenchmark as WxBSBenchmark
4
+ from .rubik import RubikBenchmark as RubikBenchmark
@@ -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"
@@ -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
+ }