Merge pull request #776 from borglab/feature/compare_colmap_poses

Use `align_robust` from Colmap to align reconstructions

gtsfm/evaluation/compare_colmap_outputs.py

@@ -7,14 +7,19 @@
 import os
 from typing import Dict, List
 
-from gtsam import Pose3
+import numpy as np
+import pycolmap
+from gtsam import Pose3, Similarity3
+from scipy.spatial.transform import Rotation
 
 import gtsfm.runner.gtsfm_runner_base as runner_base
-import gtsfm.utils.geometry_comparisons as geometry_comparisons
 import gtsfm.utils.io as io_utils
+import gtsfm.utils.logger as logger_utils
 import gtsfm.utils.metrics as metric_utils
 from gtsfm.evaluation.metrics import GtsfmMetricsGroup
 
+logger = logger_utils.get_logger()
+
 
 def load_poses(colmap_dirpath: str) -> Dict[str, Pose3]:
     wTi_list, img_fnames, _, _, _, _ = io_utils.read_scene_data_from_colmap_format(colmap_dirpath)
@@ -35,6 +40,26 @@ def compare_poses(baseline_dirpath: str, eval_dirpath: str, output_dirpath: str)
 
     common_fnames = baseline_wTi_dict.keys() & current_wTi_dict.keys()
 
+    current_reconstruction = pycolmap.Reconstruction(eval_dirpath)
+    baselineScurrent = Similarity3(
+        current_reconstruction.align_robust(
+            list(baseline_wTi_dict.keys()),
+            [wTi.translation() for wTi in baseline_wTi_dict.values()],
+            min_common_images=3  # int(len(common_fnames) * 0.2),
+            # min_inlier_ratio=0.1,
+            # max_error=1,
+        ).matrix
+    )
+
+    current_wTi_dict = {fname: baselineScurrent.transformFrom(wTi) for fname, wTi in current_wTi_dict.items()}
+
+    aRb = baselineScurrent.rotation().matrix()
+    atb = baselineScurrent.translation()
+    rz, ry, rx = Rotation.from_matrix(aRb).as_euler("zyx", degrees=True)
+    logger.info("Sim(3) Rotation `aRb`: rz=%.2f deg., ry=%.2f deg., rx=%.2f deg.", rz, ry, rx)
+    logger.info(f"Sim(3) Translation `atb`: [tx,ty,tz]={str(np.round(atb,2))}")
+    logger.info("Sim(3) Scale `asb`: %.2f", float(baselineScurrent.scale()))
+
     print(f"Baseline: {len(baseline_wTi_dict)}, current: {len(current_wTi_dict)} , common: {len(common_fnames)} poses")
 
     baseline_wTi_list: List[Pose3] = []
@@ -43,18 +68,16 @@ def compare_poses(baseline_dirpath: str, eval_dirpath: str, output_dirpath: str)
         baseline_wTi_list.append(wTi)
         current_wTi_list.append(current_wTi_dict.get(fname))
 
-    aligned_curr_wTi_list, _ = geometry_comparisons.align_poses_sim3_ignore_missing(baseline_wTi_list, current_wTi_list)
-
     i2Ui1_dict_gt = metric_utils.get_twoview_translation_directions(baseline_wTi_list)
 
-    wRi_aligned_list, wti_aligned_list = metric_utils.get_rotations_translations_from_poses(aligned_curr_wTi_list)
+    wRi_aligned_list, wti_aligned_list = metric_utils.get_rotations_translations_from_poses(current_wTi_list)
     baseline_wRi_list, baseline_wti_list = metric_utils.get_rotations_translations_from_poses(baseline_wTi_list)
 
     metrics = []
     metrics.append(metric_utils.compute_rotation_angle_metric(wRi_aligned_list, baseline_wRi_list))
     metrics.append(metric_utils.compute_translation_distance_metric(wti_aligned_list, baseline_wti_list))
-    metrics.append(metric_utils.compute_relative_translation_angle_metric(i2Ui1_dict_gt, aligned_curr_wTi_list))
-    metrics.append(metric_utils.compute_translation_angle_metric(baseline_wTi_list, aligned_curr_wTi_list))
+    metrics.append(metric_utils.compute_relative_translation_angle_metric(i2Ui1_dict_gt, current_wTi_list))
+    metrics.append(metric_utils.compute_translation_angle_metric(baseline_wTi_list, current_wTi_list))
 
     rotation_angular_errors = metrics[0]._data
     translation_angular_errors = metrics[3]._data