diff --git a/gtsfm/evaluation/compare_colmap_outputs.py b/gtsfm/evaluation/compare_colmap_outputs.py index ec9514775..e5f689bac 100644 --- a/gtsfm/evaluation/compare_colmap_outputs.py +++ b/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