Fix epipolar verification

hloc/triangulation.py

@@ -45,7 +45,7 @@ def create_db_from_model(
 
     for i, camera in reconstruction.cameras.items():
         db.add_camera(
-            camera.model_id,
+            camera.model.value,
             camera.width,
             camera.height,
             camera.params,
@@ -145,7 +145,7 @@ def geometric_verification(
         kps0, noise0 = get_keypoints(features_path, name0, return_uncertainty=True)
         noise0 = 1.0 if noise0 is None else noise0
         if len(kps0) > 0:
-            kps0 = np.stack(cam0.image_to_world(kps0))
+            kps0 = np.stack(cam0.cam_from_img(kps0))
         else:
             kps0 = np.zeros((0, 2))
 
@@ -156,7 +156,7 @@ def geometric_verification(
             kps1, noise1 = get_keypoints(features_path, name1, return_uncertainty=True)
             noise1 = 1.0 if noise1 is None else noise1
             if len(kps1) > 0:
-                kps1 = np.stack(cam1.image_to_world(kps1))
+                kps1 = np.stack(cam1.cam_from_img(kps1))
             else:
                 kps1 = np.zeros((0, 2))
 
@@ -170,15 +170,13 @@ def geometric_verification(
                 db.add_two_view_geometry(id0, id1, matches)
                 continue
 
-            qvec_01, tvec_01 = pycolmap.relative_pose(
-                image0.qvec, image0.tvec, image1.qvec, image1.tvec
-            )
-            _, errors0, errors1 = compute_epipolar_errors(
-                qvec_01, tvec_01, kps0[matches[:, 0]], kps1[matches[:, 1]]
+            cam1_from_cam0 = image1.cam_from_world * image0.cam_from_world.inverse()
+            errors0, errors1 = compute_epipolar_errors(
+                cam1_from_cam0, kps0[matches[:, 0]], kps1[matches[:, 1]]
             )
             valid_matches = np.logical_and(
-                errors0 <= max_error * noise0 / cam0.mean_focal_length(),
-                errors1 <= max_error * noise1 / cam1.mean_focal_length(),
+                errors0 <= cam0.cam_from_img_threshold(noise0 * max_error),
+                errors1 <= cam1.cam_from_img_threshold(noise1 * max_error),
             )
             # TODO: We could also add E to the database, but we need
             # to reverse the transformations if id0 > id1 in utils/database.py.

hloc/utils/geometry.py

@@ -6,28 +6,11 @@ def to_homogeneous(p):
     return np.pad(p, ((0, 0),) * (p.ndim - 1) + ((0, 1),), constant_values=1)
 
 
-def vector_to_cross_product_matrix(v):
-    return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
-
-
-def compute_epipolar_errors(qvec_r2t, tvec_r2t, p2d_r, p2d_t):
-    T_r2t = pose_matrix_from_qvec_tvec(qvec_r2t, tvec_r2t)
-    # Compute errors in normalized plane to avoid distortion.
-    E = vector_to_cross_product_matrix(T_r2t[:3, -1]) @ T_r2t[:3, :3]
-    l2d_r2t = (E @ to_homogeneous(p2d_r).T).T
-    l2d_t2r = (E.T @ to_homogeneous(p2d_t).T).T
-    errors_r = np.abs(np.sum(to_homogeneous(p2d_r) * l2d_t2r, axis=1)) / np.linalg.norm(
-        l2d_t2r[:, :2], axis=1
-    )
-    errors_t = np.abs(np.sum(to_homogeneous(p2d_t) * l2d_r2t, axis=1)) / np.linalg.norm(
-        l2d_r2t[:, :2], axis=1
-    )
-    return E, errors_r, errors_t
-
-
-def pose_matrix_from_qvec_tvec(qvec, tvec):
-    pose = np.zeros((4, 4))
-    pose[:3, :3] = pycolmap.qvec_to_rotmat(qvec)
-    pose[:3, -1] = tvec
-    pose[-1, -1] = 1
-    return pose
+def compute_epipolar_errors(j_from_i: pycolmap.Rigid3d, p2d_i, p2d_j):
+    j_E_i = j_from_i.essential_matrix()
+    l2d_j = to_homogeneous(p2d_i) @ j_E_i.T
+    l2d_i = to_homogeneous(p2d_j) @ j_E_i
+    dist = np.abs(np.sum(to_homogeneous(p2d_i) * l2d_i, axis=1))
+    errors_i = dist / np.linalg.norm(l2d_i[:, :2], axis=1)
+    errors_j = dist / np.linalg.norm(l2d_j[:, :2], axis=1)
+    return errors_i, errors_j