make reconstruction comparisons handle different geo references

Summary:
When comparing reconstructions, we were assuming that they were in the same reference frame, but this in not the case even for the synthetically generated ones.

We add a method to change the refenece frame of reconstruction and use it before comparing a reconstruction to another

Differential Revision:
D75475832

Privacy Context Container: L1137435

fbshipit-source-id: c41bb75a7778baee614314159d31ceb76624ed0a

Author: Pau Gargallo

opensfm/synthetic_data/synthetic_metrics.py

@@ -1,10 +1,11 @@
 # pyre-unsafe
+import copy
 from typing import Dict, List, Tuple
 
 import cv2
 import numpy as np
 import opensfm.transformations as tf
-from opensfm import align, multiview, pymap, types
+from opensfm import align, geo, multiview, pymap, types
 
 
 def points_errors(
@@ -125,20 +126,34 @@ def aligned_to_reference(
                 coords2.append(shot2.pose.get_origin())
 
     s, A, b = find_alignment(coords1, coords2)
-    aligned = _copy_reconstruction(reconstruction)
+    aligned = copy.deepcopy(reconstruction)
     align.apply_similarity(aligned, s, A, b)
     return aligned
 
 
-def _copy_reconstruction(reconstruction: types.Reconstruction) -> types.Reconstruction:
-    copy = types.Reconstruction()
-    for camera in reconstruction.cameras.values():
-        copy.add_camera(camera)
-    for shot in reconstruction.shots.values():
-        copy.add_shot(shot)
-    for point in reconstruction.points.values():
-        copy.add_point(point)
-    return copy
+def change_geo_reference(
+    reconstruction: types.Reconstruction,
+    latitude: float,
+    longitude: float,
+    altitude: float,
+) -> types.Reconstruction:
+    """Change the geo reference of a reconstruction.
+
+    This assumes that the new lla is close enough that rotation differences
+    between references can be ignored.
+    """
+    t_old_new = reconstruction.reference.to_topocentric(latitude, longitude, altitude)
+
+    s = 1.0
+    A = np.eye(3)
+    b = -np.array(t_old_new)
+    aligned = copy.deepcopy(reconstruction)
+    aligned.reference = geo.TopocentricConverter(latitude, longitude, altitude)
+    align.apply_similarity(aligned, s, A, b)
+    for shot in aligned.shots.values():
+        if shot.metadata.gps_position.has_value:
+            shot.metadata.gps_position.value = shot.metadata.gps_position.value + b
+    return aligned
 
 
 def rmse(errors: np.ndarray) -> float:

opensfm/synthetic_data/synthetic_scene.py

@@ -8,6 +8,7 @@
 import opensfm.synthetic_data.synthetic_generator as sg
 import opensfm.synthetic_data.synthetic_metrics as sm
 from opensfm import geo, pygeometry, pymap, types
+from opensfm.reconstruction_helpers import exif_to_metadata
 
 
 def get_camera(
@@ -452,6 +453,11 @@ def __init__(
             causal_gps_noise=causal_gps_noise,
         )
 
+        for shot in self.reconstruction.shots.values():
+            shot.metadata = exif_to_metadata(
+                self.exifs[shot.id], False, self.reconstruction.reference
+            )
+
         if generate_projections:
             (self.features, self.tracks_manager, self.gcps) = sg.generate_track_data(
                 reconstruction,
@@ -473,15 +479,18 @@ def compare(
     reconstruction: types.Reconstruction,
 ) -> Dict[str, float]:
     """Compare a reconstruction with reference groundtruth."""
+    geo = reference.reference
+
     completeness = sm.completeness_errors(reference, reconstruction)
 
-    absolute_position = sm.position_errors(reference, reconstruction)
-    absolute_rotation = sm.rotation_errors(reference, reconstruction)
-    absolute_points = sm.points_errors(reference, reconstruction)
-    absolute_gps = sm.gps_errors(reconstruction)
-    absolute_gcp = sm.gcp_errors(reconstruction, gcps)
+    geo_referenced = sm.change_geo_reference(reconstruction, geo.lat, geo.lon, geo.alt)
+    absolute_position = sm.position_errors(reference, geo_referenced)
+    absolute_rotation = sm.rotation_errors(reference, geo_referenced)
+    absolute_points = sm.points_errors(reference, geo_referenced)
+    absolute_gps = sm.gps_errors(geo_referenced)
+    absolute_gcp = sm.gcp_errors(geo_referenced, gcps)
 
-    aligned = sm.aligned_to_reference(reference, reconstruction)
+    aligned = sm.aligned_to_reference(reference, geo_referenced)
     aligned_position = sm.position_errors(reference, aligned)
     aligned_rotation = sm.rotation_errors(reference, aligned)
     aligned_points = sm.points_errors(reference, aligned)

opensfm/test/test_stats.py

@@ -130,7 +130,7 @@ def test_reconstruction_statistics_normal(
     )
 
     assert reconstruction_statistics["components"] == 1
-    assert not reconstruction_statistics["has_gps"]
+    assert reconstruction_statistics["has_gps"]
     assert not reconstruction_statistics["has_gcp"]
     assert 4900 < reconstruction_statistics["initial_points_count"] < 5000
     assert reconstruction_statistics["initial_shots_count"] == 20
@@ -257,7 +257,9 @@ def test_gps_errors_normal(
 ) -> None:
     reference = scene_synthetic.reconstruction
     gps_errors = stats.gps_errors([reference])
-    assert gps_errors == {}
+    assert set(gps_errors.keys()) == {"average_error", "error", "mean", "std"}
+    # scene_synthetic generated GPS noise is 5 meters
+    assert 3.0 < gps_errors["average_error"] < 7.0
 
 
 def test_gps_errors_null(

opensfm/test/test_synthetic_metrics.py

@@ -0,0 +1,36 @@
+# pyre-strict
+import numpy as np
+from opensfm.synthetic_data import synthetic_metrics, synthetic_scene
+
+
+def test_change_geo_reference(
+    scene_synthetic: synthetic_scene.SyntheticInputData,
+) -> None:
+    original = scene_synthetic.reconstruction
+    lat = original.reference.lat + 0.001  # about 111 m
+    lon = original.reference.lon + 0.002
+    alt = original.reference.alt + 2.34
+
+    aligned = synthetic_metrics.change_geo_reference(original, lat, lon, alt)
+
+    for shot_id in original.shots:
+        original_position = original.shots[shot_id].pose.get_origin()
+        aligned_position = aligned.shots[shot_id].pose.get_origin()
+        aligned_lla = aligned.reference.to_lla(*aligned_position)
+        aligned_in_original = original.reference.to_topocentric(*aligned_lla)
+        assert np.allclose(original_position, aligned_in_original, atol=0.01)
+
+        assert original.shots[shot_id].metadata.gps_position.has_value
+        assert aligned.shots[shot_id].metadata.gps_position.has_value
+        original_gps_prior = original.shots[shot_id].metadata.gps_position.value
+        aligned_gps_prior = aligned.shots[shot_id].metadata.gps_position.value
+        aligned_gps_lla = aligned.reference.to_lla(*aligned_gps_prior)
+        aligned_gps_in_original = original.reference.to_topocentric(*aligned_gps_lla)
+        assert np.allclose(original_gps_prior, aligned_gps_in_original, atol=0.01)
+
+    for point_id in original.points:
+        original_position = original.points[point_id].coordinates
+        aligned_position = aligned.points[point_id].coordinates
+        aligned_lla = aligned.reference.to_lla(*aligned_position)
+        aligned_in_original = original.reference.to_topocentric(*aligned_lla)
+        assert np.allclose(original_position, aligned_in_original, atol=0.01)