Better and unified GCP thresholds handling :

 - Use a single threshold from triangulation to stats
 - Only use  GCPs for linear alignment at the end since GPS and GCP have no garantee of consistency (bias)

Author: Yann N.

opensfm/align.py

@@ -127,11 +127,13 @@ def alignment_constraints(
     X, Xp = [], []
     # Get Ground Control Point correspondences
     if gcp and config["bundle_use_gcp"]:
-        triangulated, measured = triangulate_all_gcp(reconstruction, gcp)
+        triangulated, measured = triangulate_all_gcp(
+            reconstruction, gcp, config["gcp_reprojection_error_threshold"]
+        )
         X.extend(triangulated)
         Xp.extend(measured)
     # Get camera center correspondences
-    if use_gps and config["bundle_use_gps"]:
+    elif use_gps and config["bundle_use_gps"]:
         for rig_instance in reconstruction.rig_instances.values():
             gpses = [
                 shot.metadata.gps_position.value
@@ -433,14 +435,17 @@ def get_horizontal_and_vertical_directions(
 
 
 def triangulate_all_gcp(
-    reconstruction: types.Reconstruction, gcp: List[pymap.GroundControlPoint]
+    reconstruction: types.Reconstruction,
+    gcp: List[pymap.GroundControlPoint],
+    threshold: float,
 ) -> Tuple[List[NDArray], List[NDArray]]:
     """Group and triangulate Ground Control Points seen in 2+ images."""
     triangulated, measured = [], []
     for point in gcp:
         x = multiview.triangulate_gcp(
             point,
             reconstruction.shots,
+            threshold,
         )
         if x is not None and len(point.lla):
             point_enu = np.array(

opensfm/config.py

@@ -267,7 +267,7 @@ class OpenSfMConfig:
     # The default vertical standard deviation of the GCPs (in meters)
     gcp_vertical_sd: float = 0.1
     # Global weight for GCPs, expressed a ratio of the sum of (# projections) + (# shots) + (# relative motions)
-    gcp_global_weight: float = 0.01
+    gcp_global_weight: float = 0.03
     # The standard deviation of the rig translation
     rig_translation_sd: float = 0.1
     # The standard deviation of the rig rotation
@@ -313,7 +313,7 @@ class OpenSfMConfig:
     save_partial_reconstructions: bool = False
 
     ##################################
-    # Params for GPS alignment
+    # Params for GPS/GCP alignment
     ##################################
     # Use or ignore EXIF altitude tag
     use_altitude_tag: bool = True
@@ -327,6 +327,8 @@ class OpenSfMConfig:
     bundle_use_gcp: bool = True
     # Compensate GPS with a per-camera similarity transform
     bundle_compensate_gps_bias: bool = False
+    # Thrershold for the reprojection error of GCPs to be considered outliers
+    gcp_reprojection_error_threshold: float = 0.05
 
     ##################################
     # Params for rigs

opensfm/multiview.py

@@ -533,6 +533,7 @@ def triangulate_gcp(
     reproj_threshold: float = 0.02,
     min_ray_angle_degrees: float = 1.0,
     min_depth: float = 0.001,
+    iterations: int = 10,
 ) -> Optional[NDArray]:
     """Compute the reconstructed position of a GCP from observations."""
 
@@ -550,13 +551,17 @@ def triangulate_gcp(
 
     if len(os) >= 2:
         thresholds = len(os) * [reproj_threshold]
+        os = np.asarray(os)
+        bs = np.asarray(bs)
         valid_triangulation, X = pygeometry.triangulate_bearings_midpoint(
-            np.asarray(os),
-            np.asarray(bs),
+            os,
+            bs,
             thresholds,
             np.radians(min_ray_angle_degrees),
             min_depth,
         )
+        
         if valid_triangulation:
+            X = pygeometry.point_refinement(os, bs, X, iterations)
             return X
     return None

opensfm/reconstruction.py

@@ -1511,7 +1511,7 @@ def grow_reconstruction(
     rig_camera_priors = data.load_rig_cameras()
 
     paint_reconstruction(data, tracks_manager, reconstruction)
-    align_reconstruction(reconstruction, gcp, config)
+    align_reconstruction(reconstruction, [], config)
 
     bundle(reconstruction, camera_priors, rig_camera_priors, None, 0, config)
     remove_outliers(reconstruction, config)
@@ -1598,7 +1598,7 @@ def grow_reconstruction(
 
             if should_retriangulate.should():
                 logger.info("Re-triangulating")
-                align_reconstruction(reconstruction, gcp, config)
+                align_reconstruction(reconstruction, [], config)
                 b1rep = bundle(
                     reconstruction, camera_priors, rig_camera_priors, None, local_ba_grid, config
                 )
@@ -1614,7 +1614,7 @@ def grow_reconstruction(
                 should_retriangulate.done()
                 should_bundle.done()
             elif should_bundle.should():
-                align_reconstruction(reconstruction, gcp, config)
+                align_reconstruction(reconstruction, [], config)
                 brep = bundle(
                     reconstruction, camera_priors, rig_camera_priors, None, local_ba_grid, config
                 )
@@ -1711,7 +1711,7 @@ def triangulation_reconstruction(
 
             step = {}
             logger.info(f"Triangulation SfM. Inner iteration {j}, running bundle ...")
-            align_reconstruction(reconstruction, gcp, config_override)
+            align_reconstruction(reconstruction, [], config_override)
             b1rep = bundle(
                 reconstruction, camera_priors, rig_camera_priors, None, config_override
             )

opensfm/src/bundle/src/bundle_adjuster.cc

@@ -809,8 +809,7 @@ void BundleAdjuster::Run() {
         projection_type, use_analytic_, observation, projection_loss, &problem);
 
     // Add relative depth error blocks
-    geometry::Dispatch<AddRelativeDepthError>(projection_type, observation,
-                                              projection_loss, &problem);
+    geometry::Dispatch<AddRelativeDepthError>(projection_type, observation, projection_loss, &problem);
   }
 
   // Add relative motion errors

opensfm/src/sfm/ba_helpers.h

@@ -72,6 +72,7 @@ class BAHelpers {
   static bool TriangulateGCP(
       const map::GroundControlPoint& point,
       const std::unordered_map<map::ShotId, map::Shot>& shots,
+      float reproj_threshold,
       Vec3d& coordinates);
 
   static void AlignmentConstraints(

opensfm/src/sfm/src/ba_helpers.cc

@@ -351,8 +351,8 @@ py::tuple BAHelpers::BundleLocal(
 bool BAHelpers::TriangulateGCP(
     const map::GroundControlPoint& point,
     const std::unordered_map<map::ShotId, map::Shot>& shots,
+    float reproj_threshold,
     Vec3d& coordinates) {
-  constexpr auto reproj_threshold{1.0};
   constexpr auto min_ray_angle = 0.1 * M_PI / 180.0;
   constexpr auto min_depth = 1e-3;  // Assume GCPs 1mm+ away from the camera
   MatX3d os, bs;
@@ -390,52 +390,58 @@ size_t BAHelpers::AddGCPToBundle(
   const auto& reference = map.GetTopocentricConverter();
   const auto& shots = map.GetShots();
 
-  const auto dominant_terms = ba.GetRigInstances().size() +
-                              ba.GetProjectionsCount() +
-                              ba.GetRelativeMotionsCount();
 
-  size_t total_terms = 0;
-  for (const auto& point : gcp) {
-    Vec3d coordinates;
-    if (TriangulateGCP(point, shots, coordinates) || !point.lla_.empty()) {
-      ++total_terms;
-    }
-    for (const auto& obs : point.observations_) {
-      total_terms += (shots.count(obs.shot_id_) > 0);
-    }
-  }
-
-  double global_weight = config["gcp_global_weight"].cast<double>() *
-                         dominant_terms / std::max<size_t>(1, total_terms);
+  const float reproj_threshold =
+      config["gcp_reprojection_error_threshold"].cast<float>();
 
   size_t added_gcp_observations = 0;
   for (const auto& point : gcp) {
     const auto point_id = "gcp-" + point.id_;
     Vec3d coordinates;
-    if (!TriangulateGCP(point, shots, coordinates)) {
+    if (!TriangulateGCP(point, shots, reproj_threshold, coordinates)) {
       if (!point.lla_.empty()) {
         coordinates = reference.ToTopocentric(point.GetLlaVec3d());
       } else {
         continue;
       }
     }
+
+    double avg_observations = 0.;
+    int valid_shots = 0;
+    for (const auto& obs : point.observations_) {
+      const auto shot_it = shots.find(obs.shot_id_);
+      if (shot_it != shots.end()) {
+        const auto& shot = (shot_it->second);
+        avg_observations += shot.GetLandmarkObservations().size();
+        ++valid_shots;
+      }
+    }
+
+    if(!valid_shots) {
+      continue;
+    }
+    avg_observations /= valid_shots;
+
+
+    const double prior_weight = config["gcp_global_weight"].cast<double>() * avg_observations;
     constexpr auto point_constant{false};
     ba.AddPoint(point_id, coordinates, point_constant);
     if (!point.lla_.empty()) {
       const auto point_std = Vec3d(config["gcp_horizontal_sd"].cast<double>(),
                                    config["gcp_horizontal_sd"].cast<double>(),
                                    config["gcp_vertical_sd"].cast<double>());
       ba.AddPointPrior(point_id, reference.ToTopocentric(point.GetLlaVec3d()),
-                       point_std / global_weight, point.has_altitude_);
+                       point_std / prior_weight, point.has_altitude_);
     }
 
     // Now iterate through the observations
+    const double obs_weight = config["gcp_global_weight"].cast<double>() * avg_observations;
     for (const auto& obs : point.observations_) {
       const auto& shot_id = obs.shot_id_;
       if (shots.count(shot_id) > 0) {
         constexpr double scale{0.001};
         ba.AddPointProjectionObservation(shot_id, point_id, obs.projection_,
-                                         scale / global_weight);
+                                         scale  / obs_weight);
         ++added_gcp_observations;
       }
     }
@@ -829,7 +835,7 @@ py::dict BAHelpers::Bundle(
     AddGCPToBundle(ba, map, gcp, config);
   }
 
-  if (config["bundle_compensate_gps_bias"].cast<bool>()) {
+  if (config["bundle_compensate_gps_bias"].cast<bool>() && !gcp.empty()) {
     const auto& biases = map.GetBiases();
     for (const auto& camera : map.GetCameras()) {
       ba.SetCameraBias(camera.first, biases.at(camera.first));
@@ -989,7 +995,7 @@ void BAHelpers::AlignmentConstraints(
         continue;
       }
       Vec3d coordinates;
-      if (TriangulateGCP(point, shots, coordinates)) {
+      if (TriangulateGCP(point, shots, config["gcp_reprojection_error_threshold"].cast<float>(), coordinates)) {
         Xp.row(idx) = topocentricConverter.ToTopocentric(point.GetLlaVec3d());
         X.row(idx) = coordinates;
         ++idx;

opensfm/stats.py

@@ -94,7 +94,9 @@ def gcp_errors(
 
         triangulated = None
         for rec in reconstructions:
-            triangulated = multiview.triangulate_gcp(gcp, rec.shots, 1.0, 0.1)
+            triangulated = multiview.triangulate_gcp(
+                gcp, rec.shots, data.config["gcp_reprojection_error_threshold"]
+            )
             if triangulated is None:
                 continue
             else:
@@ -580,7 +582,9 @@ def save_residual_histogram(
         n,
         _,
         p_angular,
-    ) = axs[2].hist(b_angular[:-1], b_angular, weights=h_angular)
+    ) = axs[
+        2
+    ].hist(b_angular[:-1], b_angular, weights=h_angular)
     n = n.astype("int")
     for i in range(len(p_angular)):
         p_angular[i].set_facecolor(plt.cm.viridis(n[i] / max(n)))

opensfm/test/test_reconstruction_triangulation.py

@@ -40,7 +40,7 @@ def test_reconstruction_triangulation(
     assert 0.01 < errors["absolute_gps_rmse"] < 0.1
 
     # Sanity check that GCP error is similar to the generated gcp_noise
-    assert 0.01 < errors["absolute_gcp_rmse_horizontal"] < 0.05
+    assert 0.01 < errors["absolute_gcp_rmse_horizontal"] < 0.03
     assert 0.005 < errors["absolute_gcp_rmse_vertical"] < 0.04
 
     # Check that the GPS bias (only translation) is recovered