Added support for GV from known poses.

hloc/triangulation.py

@@ -3,12 +3,15 @@
 import io
 import sys
 from pathlib import Path
+import numpy as np
 from tqdm import tqdm
 import pycolmap
 
 from . import logger
 from .utils.database import COLMAPDatabase
 from .utils.io import get_keypoints, get_matches
+from .utils.parsers import parse_retrieval
+from .utils.geometry import compute_epipolar_errors
 
 
 class OutputCapture:
@@ -89,7 +92,8 @@ def import_matches(image_ids, database_path, pairs_path, matches_path,
     db.close()
 
 
-def geometric_verification(database_path, pairs_path, verbose=False):
+def estimation_and_geometric_verification(database_path, pairs_path,
+                                          verbose=False):
     logger.info('Performing geometric verification of the matches...')
     with OutputCapture(verbose):
         with pycolmap.ostream():
@@ -98,6 +102,60 @@ def geometric_verification(database_path, pairs_path, verbose=False):
                 max_num_trials=20000, min_inlier_ratio=0.1)
 
 
+def geometric_verification(image_ids, reference, database_path, features_path,
+                           pairs_path, matches_path, max_error=4.0):
+    logger.info('Performing geometric verification of the matches...')
+
+    pairs = parse_retrieval(pairs_path)
+    db = COLMAPDatabase.connect(database_path)
+
+    inlier_ratios = []
+    matched = set()
+    for name0 in tqdm(pairs):
+        id0 = image_ids[name0]
+        image0 = reference.images[id0]
+        cam0 = reference.cameras[image0.camera_id]
+        kps0, noise0 = get_keypoints(
+            features_path, name0, return_uncertainty=True)
+        kps0 = np.array([cam0.image_to_world(kp) for kp in kps0])
+
+        for name1 in pairs[name0]:
+            id1 = image_ids[name1]
+            image1 = reference.images[id1]
+            cam1 = reference.cameras[image1.camera_id]
+            kps1, noise1 = get_keypoints(
+                features_path, name1, return_uncertainty=True)
+            kps1 = np.array([cam1.image_to_world(kp) for kp in kps1])
+
+            matches = get_matches(matches_path, name0, name1)[0]
+
+            if len({(id0, id1), (id1, id0)} & matched) > 0:
+                continue
+            matched |= {(id0, id1), (id1, id0)}
+
+            if matches.shape[0] == 0:
+                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]])
+            valid_matches = np.logical_and(
+                errors0 <= max_error * noise0 / cam0.mean_focal_length(),
+                errors1 <= max_error * noise1 / cam1.mean_focal_length())
+            # TODO: We could also add E to the database, but we need
+            # to reverse the transformations if id0 > id1 in utils/database.py.
+            db.add_two_view_geometry(id0, id1, matches[valid_matches, :])
+            inlier_ratios.append(np.mean(valid_matches))
+    logger.info('mean/med/min/max valid matches %.2f/%.2f/%.2f/%.2f%%.',
+                np.mean(inlier_ratios) * 100, np.median(inlier_ratios) * 100,
+                np.min(inlier_ratios) * 100, np.max(inlier_ratios) * 100)
+
+    db.commit()
+    db.close()
+
+
 def run_triangulation(model_path, database_path, image_dir, reference_model,
                       verbose=False):
     model_path.mkdir(parents=True, exist_ok=True)
@@ -110,8 +168,8 @@ def run_triangulation(model_path, database_path, image_dir, reference_model,
 
 
 def main(sfm_dir, reference_model, image_dir, pairs, features, matches,
-         skip_geometric_verification=False, min_match_score=None,
-         verbose=False):
+         skip_geometric_verification=False, estimate_two_view_geometries=False,
+         min_match_score=None, verbose=False):
 
     assert reference_model.exists(), reference_model
     assert features.exists(), features
@@ -127,7 +185,11 @@ def main(sfm_dir, reference_model, image_dir, pairs, features, matches,
     import_matches(image_ids, database, pairs, matches,
                    min_match_score, skip_geometric_verification)
     if not skip_geometric_verification:
-        geometric_verification(database, pairs, verbose)
+        if estimate_two_view_geometries:
+            estimation_and_geometric_verification(database, pairs, verbose)
+        else:
+            geometric_verification(
+                image_ids, reference, database, features, pairs, matches)
     reconstruction = run_triangulation(sfm_dir, database, image_dir, reference,
                                        verbose)
     logger.info('Finished the triangulation with statistics:\n%s',

hloc/utils/geometry.py

@@ -0,0 +1,37 @@
+import numpy as np
+import pycolmap
+
+
+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

hloc/utils/io.py

@@ -30,9 +30,14 @@ def visit_fn(_, obj):
     return list(set(names))
 
 
-def get_keypoints(path: Path, name: str) -> np.ndarray:
+def get_keypoints(path: Path, name: str,
+                  return_uncertainty: bool = False) -> np.ndarray:
     with h5py.File(str(path), 'r') as hfile:
-        p = hfile[name]['keypoints'].__array__()
+        dset = hfile[name]['keypoints']
+        p = dset.__array__()
+        uncertainty = dset.attrs.get('uncertainty')
+    if return_uncertainty:
+        return p, uncertainty
     return p