Revert the reverted commit: "Revert "Merge branch 'master' of github.com:borglab/gtsfm""

gtsfm/averaging/rotation/rotation_averaging_base.py

@@ -8,6 +8,7 @@
 from typing import Dict, List, Optional, Tuple
 
 import dask
+import numpy as np
 from dask.delayed import Delayed
 from gtsam import Pose3, Rot3
 
@@ -42,13 +43,15 @@ def run_rotation_averaging(
         num_images: int,
         i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]],
         i1Ti2_priors: Dict[Tuple[int, int], PosePrior],
+        v_corr_idxs: Dict[Tuple[int, int], np.ndarray],
     ) -> List[Optional[Rot3]]:
         """Run the rotation averaging.
 
         Args:
-            num_images: number of poses.
-            i2Ri1_dict: relative rotations as dictionary (i1, i2): i2Ri1.
-            i1Ti2_priors: priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2.
+            num_images: Number of poses.
+            i2Ri1_dict: Relative rotations as dictionary (i1, i2): i2Ri1.
+            i1Ti2_priors: Priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2.
+            v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences.
 
         Returns:
             Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is
@@ -61,6 +64,7 @@ def _run_rotation_averaging_base(
         num_images: int,
         i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]],
         i1Ti2_priors: Dict[Tuple[int, int], PosePrior],
+        v_corr_idxs: Dict[Tuple[int, int], np.ndarray],
         wTi_gt: List[Optional[Pose3]],
     ) -> Tuple[List[Optional[Rot3]], GtsfmMetricsGroup]:
         """Runs rotation averaging and computes metrics.
@@ -69,6 +73,7 @@ def _run_rotation_averaging_base(
             num_images: Number of poses.
             i2Ri1_dict: Relative rotations as dictionary (i1, i2): i2Ri1.
             i1Ti2_priors: Priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2.
+            v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences.
             wTi_gt: Ground truth global rotations to compare against.
 
         Returns:
@@ -78,7 +83,7 @@ def _run_rotation_averaging_base(
             Metrics on global rotations.
         """
         start_time = time.time()
-        wRis = self.run_rotation_averaging(num_images, i2Ri1_dict, i1Ti2_priors)
+        wRis = self.run_rotation_averaging(num_images, i2Ri1_dict, i1Ti2_priors, v_corr_idxs)
         run_time = time.time() - start_time
 
         metrics = self.evaluate(wRis, wTi_gt)
@@ -93,11 +98,11 @@ def evaluate(self, wRi_computed: List[Optional[Rot3]], wTi_gt: List[Optional[Pos
             wRi_computed: List of global rotations computed.
             wTi_gt: Ground truth global rotations to compare against.
 
-        Raises:
-            ValueError: If the length of the computed and GT list differ.
-
         Returns:
             Metrics on global rotations.
+
+        Raises:
+            ValueError: If the length of the computed and GT list differ.
         """
         wRi_gt = [wTi.rotation() if wTi is not None else None for wTi in wTi_gt]
 
@@ -116,22 +121,28 @@ def create_computation_graph(
         num_images: int,
         i2Ri1_graph: Delayed,
         i1Ti2_priors: Dict[Tuple[int, int], PosePrior],
+        v_corr_idxs: Dict[Tuple[int, int], np.ndarray],
         gt_wTi_list: List[Optional[Pose3]],
     ) -> Tuple[Delayed, Delayed]:
         """Create the computation graph for performing rotation averaging.
 
         Args:
-            num_images: number of poses.
-            i2Ri1_graph: dictionary of relative rotations as a delayed task.
-            i1Ti2_priors: priors on relative poses as (i1, i2): PosePrior on i1Ti2.
-            gt_wTi_list: ground truth poses, to be used for evaluation.
+            num_images: Number of poses.
+            i2Ri1_graph: Dictionary of relative rotations as a delayed task.
+            i1Ti2_priors: Priors on relative poses as (i1, i2): PosePrior on i1Ti2.
+            v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences.
+            gt_wTi_list: Ground truth poses, to be used for evaluation.
 
         Returns:
-            global rotations wrapped using dask.delayed.
+            Global rotations wrapped using dask.delayed.
         """
 
         wRis, metrics = dask.delayed(self._run_rotation_averaging_base, nout=2)(
-            num_images, i2Ri1_dict=i2Ri1_graph, i1Ti2_priors=i1Ti2_priors, wTi_gt=gt_wTi_list
+            num_images,
+            i2Ri1_dict=i2Ri1_graph,
+            i1Ti2_priors=i1Ti2_priors,
+            v_corr_idxs=v_corr_idxs,
+            wTi_gt=gt_wTi_list,
         )
 
         return wRis, metrics

gtsfm/averaging/rotation/shonan.py

@@ -16,10 +16,7 @@
 import gtsam
 import numpy as np
 from gtsam import (
-    BetweenFactorPose3,
-    BetweenFactorPose3s,
     LevenbergMarquardtParams,
-    Pose3,
     Rot3,
     ShonanAveraging3,
     ShonanAveragingParameters3,
@@ -29,6 +26,7 @@
 from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase
 from gtsfm.common.pose_prior import PosePrior
 
+ROT3_DOF = 3
 POSE3_DOF = 6
 
 logger = logger_utils.get_logger()
@@ -39,18 +37,23 @@
 class ShonanRotationAveraging(RotationAveragingBase):
     """Performs Shonan rotation averaging."""
 
-    def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_SIGMA) -> None:
+    def __init__(
+        self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_SIGMA, weight_by_inliers: bool = True
+    ) -> None:
         """Initializes module.
 
         Note: `p_min` and `p_max` describe the minimum and maximum relaxation rank.
 
         Args:
             two_view_rotation_sigma: Covariance to use (lower values -> more strictly adhere to input measurements).
+            weight_by_inliers: Whether to weight pairwise costs according to an uncertainty equal to the inverse number
+                of inlier correspondences per edge.
         """
         super().__init__()
         self._two_view_rotation_sigma = two_view_rotation_sigma
         self._p_min = 3
         self._p_max = 64
+        self._weight_by_inliers = weight_by_inliers
 
     def __get_shonan_params(self) -> ShonanAveragingParameters3:
         lm_params = LevenbergMarquardtParams.CeresDefaults()
@@ -59,30 +62,34 @@ def __get_shonan_params(self) -> ShonanAveragingParameters3:
         shonan_params.setCertifyOptimality(True)
         return shonan_params
 
-    def __between_factors_from_2view_relative_rotations(
-        self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int]
-    ) -> BetweenFactorPose3s:
+    def __measurements_from_2view_relative_rotations(
+        self,
+        i2Ri1_dict: Dict[Tuple[int, int], Rot3],
+        num_correspondences_dict: Dict[Tuple[int, int], int],
+    ) -> gtsam.BinaryMeasurementsRot3:
         """Create between factors from relative rotations computed by the 2-view estimator."""
         # TODO: how to weight the noise model on relative rotations compared to priors?
-        noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma)
 
-        between_factors = BetweenFactorPose3s()
+        # Default noise model if `self._weight_by_inliers` is False, or zero correspondences on edge.
+        noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, self._two_view_rotation_sigma)
 
+        measurements = gtsam.BinaryMeasurementsRot3()
         for (i1, i2), i2Ri1 in i2Ri1_dict.items():
-            if i2Ri1 is not None:
+            if i2Ri1 is None:
+                continue
+            if self._weight_by_inliers and num_correspondences_dict[(i1, i2)] > 0:
                 # ignore translation during rotation averaging
-                i2Ti1 = Pose3(i2Ri1, np.zeros(3))
-                i2_ = old_to_new_idxs[i2]
-                i1_ = old_to_new_idxs[i1]
-                between_factors.append(BetweenFactorPose3(i2_, i1_, i2Ti1, noise_model))
+                noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, 1 / num_correspondences_dict[(i1, i2)])
+
+            measurements.append(gtsam.BinaryMeasurementRot3(i2, i1, i2Ri1, noise_model))
 
-        return between_factors
+        return measurements
 
-    def _between_factors_from_pose_priors(
+    def _measurements_from_pose_priors(
         self, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], old_to_new_idxs: Dict[int, int]
-    ) -> BetweenFactorPose3s:
+    ) -> gtsam.BinaryMeasurementsRot3:
         """Create between factors from the priors on relative poses."""
-        between_factors = BetweenFactorPose3s()
+        measurements = gtsam.BinaryMeasurementsRot3()
 
         def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float:
             """Get the sigma to be used for the isotropic noise model.
@@ -95,13 +102,13 @@ def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float:
             i1_ = old_to_new_idxs[i1]
             i2_ = old_to_new_idxs[i2]
             noise_model_sigma = get_isotropic_noise_model_sigma(i1Ti2_prior.covariance)
-            noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, noise_model_sigma)
-            between_factors.append(BetweenFactorPose3(i1_, i2_, i1Ti2_prior.value, noise_model))
+            noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, noise_model_sigma)
+            measurements.append(gtsam.BinaryMeasurementRot3(i1_, i2_, i1Ti2_prior.value.rotation(), noise_model))
 
-        return between_factors
+        return measurements
 
     def _run_with_consecutive_ordering(
-        self, num_connected_nodes: int, between_factors: BetweenFactorPose3s
+        self, num_connected_nodes: int, measurements: gtsam.BinaryMeasurementsRot3
     ) -> List[Optional[Rot3]]:
         """Run the rotation averaging on a connected graph w/ N keys ordered consecutively [0,...,N-1].
 
@@ -112,7 +119,7 @@ def _run_with_consecutive_ordering(
         Args:
             num_connected_nodes: Number of unique connected nodes (i.e. images) in the graph
                 (<= the number of images in the dataset)
-            between_factors: BetweenFactorPose3s created from relative rotations from 2-view estimator and the priors.
+            measurements: BinaryMeasurementsRot3 created from relative rotations from 2-view estimator and the priors.
 
         Returns:
             Global rotations for each **CONNECTED** camera pose, i.e. wRi, as a list. The number of entries in
@@ -122,10 +129,10 @@ def _run_with_consecutive_ordering(
 
         logger.info(
             "Running Shonan with %d constraints on %d nodes",
-            len(between_factors),
+            len(measurements),
             num_connected_nodes,
         )
-        shonan = ShonanAveraging3(between_factors, self.__get_shonan_params())
+        shonan = ShonanAveraging3(measurements, self.__get_shonan_params())
 
         initial = shonan.initializeRandomly()
         logger.info("Initial cost: %.5f", shonan.cost(initial))
@@ -159,6 +166,7 @@ def run_rotation_averaging(
         num_images: int,
         i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]],
         i1Ti2_priors: Dict[Tuple[int, int], PosePrior],
+        v_corr_idxs: Dict[Tuple[int, int], np.ndarray],
     ) -> List[Optional[Rot3]]:
         """Run the rotation averaging on a connected graph with arbitrary keys, where each key is a image/pose index.
 
@@ -170,6 +178,7 @@ def run_rotation_averaging(
             num_images: Number of images. Since we have one pose per image, it is also the number of poses.
             i2Ri1_dict: Relative rotations for each image pair-edge as dictionary (i1, i2): i2Ri1.
             i1Ti2_priors: Priors on relative poses.
+            v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences.
 
         Returns:
             Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is
@@ -183,17 +192,36 @@ def run_rotation_averaging(
             return wRi_list
 
         nodes_with_edges = sorted(list(self._nodes_with_edges(i2Ri1_dict, i1Ti2_priors)))
-        old_to_new_idxes = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)}
-
-        between_factors: BetweenFactorPose3s = self.__between_factors_from_2view_relative_rotations(
-            i2Ri1_dict, old_to_new_idxes
-        )
-        between_factors.extend(self._between_factors_from_pose_priors(i1Ti2_priors, old_to_new_idxes))
-
-        wRi_list_subset = self._run_with_consecutive_ordering(
-            num_connected_nodes=len(nodes_with_edges), between_factors=between_factors
-        )
-
+        old_to_new_idxs = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)}
+
+        i2Ri1_dict_remapped = {
+            (old_to_new_idxs[i1], old_to_new_idxs[i2]): i2Ri1 for (i1, i2), i2Ri1 in i2Ri1_dict.items()
+        }
+        num_correspondences_dict: Dict[Tuple[int, int], int] = {
+            (old_to_new_idxs[i1], old_to_new_idxs[i2]): len(v_corr_idxs[(i1, i2)])
+            for (i1, i2) in v_corr_idxs.keys()
+            if (i1, i2) in i2Ri1_dict
+        }
+
+        def _create_factors_and_run() -> List[Rot3]:
+            measurements: gtsam.BinaryMeasurementsRot3 = self.__measurements_from_2view_relative_rotations(
+                i2Ri1_dict=i2Ri1_dict_remapped, num_correspondences_dict=num_correspondences_dict
+            )
+            measurements.extend(self._measurements_from_pose_priors(i1Ti2_priors, old_to_new_idxs))
+            wRi_list_subset = self._run_with_consecutive_ordering(
+                num_connected_nodes=len(nodes_with_edges), measurements=measurements
+            )
+            return wRi_list_subset
+
+        try:
+            wRi_list_subset = _create_factors_and_run()
+        except RuntimeError:
+            logger.exception("Shonan failed")
+            if self._weight_by_inliers is True:
+                logger.info("Reattempting Shonan without inlier-weighted costs...")
+                # At times, Shonan's `SparseMinimumEigenValue` fails to compute minimum eigenvalue.
+                self._weight_by_inliers = False
+                wRi_list_subset = _create_factors_and_run()
         wRi_list = [None] * num_images
         for remapped_i, original_i in enumerate(nodes_with_edges):
             wRi_list[original_i] = wRi_list_subset[remapped_i]

gtsfm/configs/unified.yaml

@@ -65,15 +65,17 @@ SceneOptimizer:
     # comment out to not run
     view_graph_estimator:
       _target_: gtsfm.view_graph_estimator.cycle_consistent_rotation_estimator.CycleConsistentRotationViewGraphEstimator
-      edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR
+      edge_error_aggregation_criterion: MIN_EDGE_ERROR
 
     rot_avg_module:
       _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging
+      weight_by_inliers: True
 
     trans_avg_module:
       _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM
       robust_measurement_noise: True
       projection_sampling_method: SAMPLE_INPUT_MEASUREMENTS
+      reject_outliers: True
 
     data_association_module:
       _target_: gtsfm.data_association.data_assoc.DataAssociation

gtsfm/frontend/verifier/verifier_base.py

@@ -33,6 +33,12 @@ def get_ui_metadata() -> UiMetadata:
             parent_plate="Two-View Estimator",
         )
 
+    def __repr__(self) -> str:
+        return (
+            f"{type(self).__name__}"
+            + f"__use_intrinsics{self._use_intrinsics_in_verification}_{self._estimation_threshold_px}px"
+        )
+
     def __init__(
         self,
         use_intrinsics_in_verification: bool,