Projected export geocoords support

Author: pierotofy

opensfm/actions/export_geocoords.py

@@ -7,6 +7,7 @@
 from opensfm import io
 from opensfm.dataset import DataSet, UndistortedDataSet
 from opensfm.geo import TopocentricConverter
+from opensfm.reconstruction import bundle_shot_poses
 from typing import List, Sequence
 
 logger: logging.Logger = logging.getLogger(__name__)
@@ -20,7 +21,8 @@ def run_dataset(
     reconstruction: bool,
     dense : bool,
     output: str,
-    offset = (0, 0)
+    offset = (0, 0),
+    mode = "affine"
 ) -> None:
     """Export reconstructions in geographic coordinates
 
@@ -32,7 +34,7 @@ def run_dataset(
         dense : export dense point cloud (depthmaps/merged.ply)
         output : path of the output file relative to the dataset
         offset : offset to substract from the translation (optional)
-
+        mode : Method of georeferencing (optional)
     """
 
     if not (transformation or image_positions or reconstruction or dense):
@@ -57,8 +59,15 @@ def run_dataset(
 
     if reconstruction:
         reconstructions = data.load_reconstruction()
-        for r in reconstructions:
-            _transform_reconstruction(r, t)
+        if mode == "affine":
+            for r in reconstructions:
+                _transform_reconstruction(r, t)
+        elif mode == "projected":
+            for r in reconstructions:
+                _transform_reconstruction_projected(r, t, offset[0], offset[1], reference, projection, data)
+        else:
+            raise Exception(f"Invalid mode: {mode}")
+        
         output = output or "reconstruction.geocoords.json"
         data.save_reconstruction(reconstructions, output)
 
@@ -133,6 +142,40 @@ def _transform_reconstruction(
     for point in reconstruction.points.values():
         point.coordinates = list(np.dot(A, point.coordinates) + b)
 
+def _transform_points_projected(pts, offset_x, offset_y, reference, projection):
+    lat, lon, alt = reference.to_lla(pts[:,0], pts[:,1], pts[:,2])
+    easting, northing = projection(lon, lat)
+    return easting - offset_x, northing - offset_y, alt
+
+def _transform_reconstruction_projected(
+    reconstruction: types.Reconstruction, transformation: np.ndarray, offset_x, offset_y, reference, projection, data
+) -> None:
+    """Apply a transformation to a reconstruction in-place by projection."""
+    
+    # Points
+    pts =  np.array([p.coordinates for p in reconstruction.points.values()])
+    easting, northing, alt = _transform_points_projected(pts, offset_x, offset_y, reference, projection)
+
+    for i, point in enumerate(reconstruction.points.values()):
+        point.coordinates = [easting[i], northing[i], alt[i]]
+
+    # Cameras
+    A, b = transformation[:3, :3], transformation[:3, 3]
+    A1 = np.linalg.inv(A)
+
+    pts = np.array([shot.pose.get_origin() for shot in reconstruction.shots.values()])
+    easting, northing, alt = _transform_points_projected(pts, offset_x, offset_y, reference, projection)
+    
+    for i, shot in enumerate(reconstruction.shots.values()):
+        R = shot.pose.get_rotation_matrix()
+        shot.pose.set_rotation_matrix(np.dot(R, A1))
+        shot.pose.set_origin([easting[i], northing[i], alt[i]])
+    
+    logger.info("Bundle shot poses")
+    camera_priors = data.load_camera_models()
+    rig_camera_priors = data.load_rig_cameras()
+    bundle_shot_poses(reconstruction, set(reconstruction.shots.keys()), camera_priors, rig_camera_priors, data.config)
+    
 
 def _transform_dense_point_cloud(
     udata: UndistortedDataSet, transformation: np.ndarray, output_path: str

opensfm/commands/export_geocoords.py

@@ -18,7 +18,8 @@ def run_impl(self, dataset: DataSet, args: argparse.Namespace) -> None:
             args.reconstruction,
             args.dense,
             args.output,
-            (args.offset_x, args.offset_y)
+            (args.offset_x, args.offset_y),
+            args.mode
         )
 
     def add_arguments_impl(self, parser: argparse.ArgumentParser) -> None:
@@ -56,4 +57,10 @@ def add_arguments_impl(self, parser: argparse.ArgumentParser) -> None:
         parser.add_argument(
             "--offset-y", type=float, help="Value to add to the final translation Y axis", default=0.0
         )
-        
+        parser.add_argument(
+            "--mode",
+            help="Georeferencing method",
+            type=str,
+            choices=["affine", "projected"],
+            default="affine",
+        )