Fetzer Factor Integration

gtsfm/averaging/rotation/shonan.py

@@ -44,6 +44,7 @@ def __init__(
         two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_SIGMA,
         weight_by_inliers: bool = True,
         use_chordal_init: bool = True,
+        chordal_only: bool = False,
     ) -> None:
         """Initializes module.
 
@@ -53,13 +54,20 @@ def __init__(
             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.
+            use_chordal_init: If True, initialize Shonan with chordal init (deterministic). If False, random init.
+            chordal_only: If True, SKIP Shonan's SDP iteration entirely. Build chordal init, then refine with a
+                robust LM solve using Geman-McClure (GLOMAP's `weight_type = GEMAN_MCCLURE`,
+                `irls_loss_parameter_sigma = 5.0` deg, `max_num_irls_iterations = 100`, see
+                colmap/src/colmap/estimators/rotation_averaging.h). Use when Shonan hangs on hard graphs (e.g.
+                Pantheon's repetitive colonnade causes rank escalation). Default False (use Shonan).
         """
         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
         self._use_chordal_init = use_chordal_init
+        self._chordal_only = chordal_only
 
     def __get_shonan_params(self) -> ShonanAveragingParameters3:
         lm_params = LevenbergMarquardtParams.CeresDefaults()
@@ -139,6 +147,9 @@ def _run_with_consecutive_ordering(
                 not be computed (either underconstrained system or ill-constrained system).
         """
 
+        if self._chordal_only:
+            return self._run_chordal_only(measurements, num_connected_nodes)
+
         logger.info(
             "Running Shonan with %d constraints on %d nodes",
             len(measurements),
@@ -167,6 +178,67 @@ def _run_with_consecutive_ordering(
 
         return wRi_list_consecutive
 
+    def _run_chordal_only(
+        self, measurements: gtsam.BinaryMeasurementsRot3, num_connected_nodes: int
+    ) -> List[Optional[Rot3]]:
+        """GLOMAP-style RA fallback: chordal init + Geman-McClure robust LM refinement.
+
+        Skips Shonan's SDP iteration entirely. Used when Shonan hangs on hard graphs (Pantheon-class).
+        Settings mirror GLOMAP's defaults from colmap/src/colmap/estimators/rotation_averaging.h:
+          * weight_type = GEMAN_MCCLURE
+          * irls_loss_parameter_sigma = 5.0 deg
+          * max_num_irls_iterations = 100
+          * irls_step_convergence_threshold = 0.001
+        """
+        logger.info(
+            "Chordal-only RA (Shonan skipped): %d constraints on %d nodes — "
+            "chordal init → Geman-McClure LM refinement",
+            len(measurements), num_connected_nodes,
+        )
+        # No relative-rotation constraints survive (e.g. every edge was filtered out): there is nothing
+        # to initialize or anchor, so return all-None rather than crashing in chordal init / the prior.
+        if len(measurements) == 0:
+            logger.warning("Chordal-only RA: no measurements to optimize; returning all-None orientations.")
+            return [None] * num_connected_nodes
+
+        # Step 1: chordal init. Returns gtsam.Values containing Rot3 (orientations only).
+        initial = self.chordal_initialize(measurements)
+
+        # Step 2: build a robust Rot3-only factor graph for refinement. Geman-McClure
+        # m-estimator on an isotropic base noise (sigma = 5° in radians) — matches GLOMAP's
+        # irls_loss_parameter_sigma.
+        sigma_rad = float(np.deg2rad(5.0))
+        base_noise = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, sigma_rad)
+        gm = gtsam.noiseModel.mEstimator.GemanMcClure.Create(1.0)
+        robust_noise = gtsam.noiseModel.Robust.Create(gm, base_noise)
+
+        graph = gtsam.NonlinearFactorGraph()
+        anchor_key = None
+        for m in measurements:
+            if anchor_key is None:
+                anchor_key = m.key1()
+            graph.add(gtsam.BetweenFactorRot3(m.key1(), m.key2(), m.measured(), robust_noise))
+        # Anchor the gauge with a tight prior on the first node.
+        anchor_noise = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, 1e-6)
+        graph.add(gtsam.PriorFactorRot3(anchor_key, Rot3(), anchor_noise))
+
+        # Step 3: LM with iteration cap matching GLOMAP's max_num_irls_iterations.
+        lm_params = gtsam.LevenbergMarquardtParams.CeresDefaults()
+        lm_params.setMaxIterations(100)
+        lm_params.setRelativeErrorTol(1e-3)  # GLOMAP's irls_step_convergence_threshold
+        try:
+            optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, lm_params)
+            result = optimizer.optimize()
+        except RuntimeError as e:
+            logger.warning("Chordal-only LM refinement failed (%s); returning chordal init only.", e)
+            result = initial
+
+        wRi_list_consecutive: list[None | Rot3] = [None] * num_connected_nodes
+        for i in range(num_connected_nodes):
+            if result.exists(i):
+                wRi_list_consecutive[i] = result.atRot3(i)
+        return wRi_list_consecutive
+
     def _nodes_with_edges(
         self, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], relative_pose_priors: Dict[Tuple[int, int], PosePrior]
     ) -> Set[int]:

gtsfm/bundle/bundle_adjustment.py

@@ -821,7 +821,13 @@ def run_ba_stage_with_filtering(
         if reproj_error_thresh is not None:
             if verbose:
                 logger.info("[Result] Number of tracks before filtering: %d", optimized_data.number_tracks())
-            filtered_result, postfilter_valid_mask = optimized_data.filter_landmarks(reproj_error_thresh)
+            # Per-MEASUREMENT trim (keep long tracks by dropping only the offending observations,
+            # instead of deleting the whole track when any one measurement exceeds the threshold).
+            filtered_result, postfilter_valid_mask = optimized_data.filter_landmark_measurements(
+                reproj_err_thresh=reproj_error_thresh,
+                min_track_length=self._min_track_length,
+                return_valid_mask=True,
+            )
             if verbose:
                 logger.info("[Result] Number of tracks after filtering: %d", filtered_result.number_tracks())
 

gtsfm/common/gtsfm_data.py

@@ -940,20 +940,32 @@ def filter_landmarks(self, reproj_err_thresh: float = 5) -> Tuple["GtsfmData", L
         return filtered_data, valid_mask
 
     def filter_landmark_measurements(
-        self, reproj_err_thresh: float = 5, min_track_length: int = 2, retain_cameras_without_tracks: bool = True
-    ) -> "GtsfmData":
+        self,
+        reproj_err_thresh: float = 5,
+        min_track_length: int = 2,
+        retain_cameras_without_tracks: bool = True,
+        return_valid_mask: bool = False,
+    ) -> Union["GtsfmData", Tuple["GtsfmData", List[bool]]]:
         """Filters out landmarks with high reprojection error
 
+        Unlike `filter_landmarks` (which drops a whole track if ANY measurement exceeds the
+        threshold), this trims the offending measurements per-track and keeps the track as long
+        as `min_track_length` measurements survive.
+
         Args:
             reproj_err_thresh: reprojection err threshold for each measurement.
+            min_track_length: minimum surviving measurements to keep a track.
+            retain_cameras_without_tracks: keep cameras even if all their measurements were filtered.
+            return_valid_mask: if True, also return a per-input-track bool list of which tracks survived.
 
         Returns:
-            New instance with filtered measurements/tracks.
+            New instance with filtered measurements/tracks (and the survival mask if requested).
         """
         # TODO: move this function to utils or GTSAM
         filtered_data = GtsfmData(self.number_images(), gaussian_splats=self._gaussian_splats)
         filtered_data._image_info = self._clone_image_info()
 
+        valid_mask: List[bool] = []
         for track in self._tracks:
             errors, _ = reprojection.compute_track_reprojection_errors(self._cameras, track)
             new_track = SfmTrack(track.point3())
@@ -967,7 +979,10 @@ def filter_landmark_measurements(
                 i, uv = track.measurement(k)
                 new_track.addMeasurement(i, uv)
                 track_cameras.add(i)
-            if len(track_cameras) < min_track_length:
+            # A track survives iff it retains >= min_track_length measurements below threshold.
+            survived = len(track_cameras) >= min_track_length
+            valid_mask.append(survived)
+            if not survived:
                 continue
             for i in track_cameras:
                 camera_i = self.get_camera(i)
@@ -989,6 +1004,8 @@ def filter_landmark_measurements(
                 filtered_data.add_camera(i, camera_i)
 
         filtered_data._pose_covariances = self._clone_pose_covariances(indices=filtered_data.get_valid_camera_indices())
+        if return_valid_mask:
+            return filtered_data, valid_mask
         return filtered_data
 
     def align_via_sim3_and_transform(self, aTi: dict[int, Pose3]) -> "GtsfmData":

gtsfm/configs/megaloc_sift_gp_single_pt.yaml

@@ -0,0 +1,138 @@
+# MegaLoc + SIFT + Global Positioner with Single partition.
+# Based on unified_megaloc.yaml with SIFT detector/matcher swap + GP.
+
+# @package _global_
+_target_: gtsfm.scene_optimizer.SceneOptimizer
+
+loader:
+  _target_: gtsfm.loader.Olsson
+
+image_pairs_generator:
+  _target_: gtsfm.retriever.image_pairs_generator.ImagePairsGenerator
+  global_descriptor:
+    _target_: gtsfm.frontend.cacher.global_descriptor_cacher.GlobalDescriptorCacher
+    global_descriptor_obj:
+      _target_: gtsfm.frontend.global_descriptor.MegaLoc
+  retriever:
+    _target_: gtsfm.retriever.Similarity
+    num_matched: 100
+    min_score: 0.15
+  batch_size: 16
+
+graph_partitioner:
+  _target_: gtsfm.graph_partitioner.Single
+
+cluster_optimizer:
+  _target_: gtsfm.cluster_optimizer.Multiview
+
+  correspondence_generator:
+    _target_: gtsfm.frontend.correspondence_generator.det_desc_correspondence_generator.DetDescCorrespondenceGenerator
+
+    detector_descriptor:
+      _target_: gtsfm.frontend.cacher.detector_descriptor_cacher.DetectorDescriptorCacher
+      detector_descriptor_obj:
+        _target_: gtsfm.frontend.detector_descriptor.colmap_sift.ColmapSIFTDetectorDescriptor
+        max_keypoints: 8192
+
+    matcher:
+      _target_: gtsfm.frontend.cacher.matcher_cacher.MatcherCacher
+      matcher_obj:
+        _target_: gtsfm.frontend.matcher.twoway_matcher.TwoWayMatcher
+        ratio_test_threshold: 0.8
+
+  two_view_estimator:
+    _target_: gtsfm.two_view_estimator_cacher.TwoViewEstimatorCacher
+    two_view_estimator_obj:
+      _target_: gtsfm.two_view_estimator.TwoViewEstimator
+      bundle_adjust_2view: True
+      eval_threshold_px: 4
+      ba_reproj_error_thresholds: [0.5]
+      bundle_adjust_2view_maxiters: 100
+
+      verifier:
+        _target_: gtsfm.frontend.verifier.poselib_verifier.PoseLibVerifier  # pure PoseLib: pose survives 2-view BA (~9722 edges) + focal-independent F (estimate_fundamental) + planar gate (H/F ratio) — no GRIC/pycolmap
+        estimation_threshold_px: 2
+        estimate_calibration_geometry: True
+
+      triangulation_options:
+        _target_: gtsfm.data_association.point3d_initializer.TriangulationOptions
+        mode:
+          _target_: gtsfm.data_association.point3d_initializer.TriangulationSamplingMode
+          value: NO_RANSAC
+
+      inlier_support_processor:
+        _target_: gtsfm.two_view_estimator.InlierSupportProcessor
+        min_num_inliers_est_model: 30  # post-2view-BA pose-quality gate (GLOMAP FilterInlierNum); PoseLib's pose survives it
+        min_inlier_ratio_est_model: 0.15
+  save_gtsfm_data: True
+  save_3d_viz: False
+  save_two_view_viz: False
+  pose_angular_error_thresh: 5
+  multiview_optimizer:
+    _target_: gtsfm.multi_view_optimizer.MultiViewOptimizer
+
+    view_graph_estimator:
+      _target_: gtsfm.view_graph_estimator.cycle_consistent_rotation_estimator.CycleConsistentRotationViewGraphEstimator
+      edge_error_aggregation_criterion: MIN_EDGE_ERROR
+      error_threshold: 7.0
+
+    rot_avg_module:
+      _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging
+      weight_by_inliers: True
+      # Skip Shonan's SDP (which fails/escalates rank on repetitive phototourism scenes);
+      # use chordal init + Geman-McClure IRLS refinement (GLOMAP-style).
+      chordal_only: true
+
+    trans_avg_module:
+      _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM
+      robust_measurement_noise: True
+      projection_sampling_method: SAMPLE_INPUT_MEASUREMENTS
+      reject_outliers: False
+      use_all_tracks_for_averaging: True
+      use_relative_camera_poses: True
+
+    data_association_module:
+      _target_: gtsfm.data_association.data_assoc.DataAssociation
+      min_track_len: 3
+      triangulation_options:
+        _target_: gtsfm.data_association.point3d_initializer.TriangulationOptions
+        reproj_error_threshold: 10
+        mode:
+          _target_: gtsfm.data_association.point3d_initializer.TriangulationSamplingMode
+          value: RANSAC_SAMPLE_UNIFORM
+        max_num_hypotheses: 100
+      save_track_patches_viz: False
+
+    global_positioner:
+      _target_: gtsfm.global_positioner.global_positioner.GlobalPositioner
+      noise_sigma: 0.01
+      huber_loss_scale: 0.1
+      max_iterations: 50
+      min_track_measurements: 3
+      max_reproj_error: 10.0
+
+    run_view_graph_calibration: true
+
+    bundle_adjustment_module:
+      _target_: gtsfm.bundle.bundle_adjustment.BundleAdjustmentOptimizer
+      # BA filters tracks after each threshold pass. The filter now trims per-MEASUREMENT
+      # (filter_landmark_measurements) instead of deleting whole tracks, so long multi-view
+      # tracks survive (matches GLOMAP) rather than being over-pruned.
+      reproj_error_thresholds: [3]
+      robust_ba_mode: "HUBER"
+      shared_calib: False
+      cam_pose3_prior_noise_sigma: 0.1
+      calibration_prior_focal_sigma: 200.0
+      measurement_noise_sigma: 1.0
+      use_calibration_prior: true
+      # GLOMAP-style multi-view retriangulation: after the BA loop converges,
+      # re-triangulate the full 2D track set from union-find with post-BA cameras,
+      # then run a final BA on the augmented set. Recovers tracks that were dropped
+      # between union-find (~46K) and BA's filter passes (~14K). See plan Phase 1.
+      use_multi_view_retriangulation: true
+      mv_retri_min_track_length: 3
+      mv_retri_reproj_error_thresh: 10.0
+
+  dense_multiview_optimizer: null
+  gaussian_splatting_optimizer: null
+

gtsfm/frontend/verifier/degensac.py

@@ -57,7 +57,7 @@ def verify(
         match_indices: np.ndarray,
         camera_intrinsics_i1: CALIBRATION_TYPE,
         camera_intrinsics_i2: CALIBRATION_TYPE,
-    ) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray, float]:
+    ) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray, float, Optional[np.ndarray], Optional[int]]:
         """Performs verification of correspondences between two images to recover the relative pose and indices of
         verified correspondences.
 
@@ -99,4 +99,4 @@ def verify(
             camera_intrinsics_i2,
         )
 
-        return i2Ri1, i2Ui1, v_corr_idxs, inlier_ratio_est_model
+        return i2Ri1, i2Ui1, v_corr_idxs, inlier_ratio_est_model, i2Fi1, None

gtsfm/frontend/verifier/gric_verifier.py

@@ -81,7 +81,7 @@ def __init__(
         )
 
         # for failure, i2Ri1 = None, and i2Ui1 = None, and no verified correspondences, and inlier_ratio_est_model = 0
-        self._failure_result = (None, None, np.array([], dtype=np.uint64), 0.0)
+        self._failure_result = (None, None, np.array([], dtype=np.uint64), 0.0, None, None)
 
     def __estimate_two_view_geometry(
         self,
@@ -121,7 +121,7 @@ def verify(
         match_indices: np.ndarray,
         camera_intrinsics_i1: CALIBRATION_TYPE,
         camera_intrinsics_i2: CALIBRATION_TYPE,
-    ) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray, float]:
+    ) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray, float, Optional[np.ndarray], Optional[int]]:
         """Performs verification of correspondences between two images to recover the relative pose and indices of
         verified correspondences.
 
@@ -164,4 +164,6 @@ def verify(
         i2Ri1 = Rot3(qw, qx, qy, qz)
         i2Ui1 = Unit3(i2Ui1)
 
-        return i2Ri1, i2Ui1, v_corr_idxs, inlier_ratio_est_model
+        # i2Fi1 and config are returned as None: GRIC is retained only for interface
+        # conformance (the pipeline uses PoseLibVerifier). No fundamental matrix is plumbed.
+        return i2Ri1, i2Ui1, v_corr_idxs, inlier_ratio_est_model, None, None

gtsfm/frontend/verifier/loransac.py

@@ -102,7 +102,7 @@ def verify(
         match_indices: np.ndarray,
         camera_intrinsics_i1: CALIBRATION_TYPE,
         camera_intrinsics_i2: CALIBRATION_TYPE,
-    ) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray, float]:
+    ) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray, float, Optional[np.ndarray], Optional[int]]:
         """Performs verification of correspondences between two images to recover the relative pose and indices of
         verified correspondences.
 
@@ -149,6 +149,7 @@ def verify(
         else:
             raise KeyError("LoRANSAC result_dict missing both 'inliers' and 'inlier_mask'.")
         v_corr_idxs = match_indices[inlier_mask]
+        i2Fi1 = None
         if self._use_intrinsics_in_verification:
             # case where E-matrix was estimated
             # See https://github.com/colmap/colmap/blob/dev/src/base/pose.h#L72 for quaternion coefficient ordering
@@ -170,4 +171,4 @@ def verify(
                 camera_intrinsics_i1=camera_intrinsics_i1,
                 camera_intrinsics_i2=camera_intrinsics_i2,
             )
-        return i2Ri1, i2Ui1, v_corr_idxs, inlier_ratio_est_model
+        return i2Ri1, i2Ui1, v_corr_idxs, inlier_ratio_est_model, i2Fi1, None