Fix projected reconstruction export georeferencing

opensfm/actions/export_geocoords.py

@@ -3,11 +3,9 @@
 from opensfm import types
 
 import numpy as np
-import pyproj
-from opensfm import io
+from opensfm import geo, io
 from opensfm.dataset import DataSet, UndistortedDataSet
-from opensfm.geo import TopocentricConverter
-from typing import List, Sequence
+from typing import List
 
 logger: logging.Logger = logging.getLogger(__name__)
 
@@ -20,7 +18,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,6 +31,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)
 
     """
 
@@ -41,8 +41,8 @@ def run_dataset(
 
     reference = data.load_reference()
 
-    projection = pyproj.Proj(proj)
-    t = _get_transformation(reference, projection, offset)
+    projection = geo.construct_proj_transformer(proj, inverse=True)
+    t = geo.get_proj_transform_matrix(reference, projection, offset)
 
     if transformation:
         output = output or "geocoords_transformation.txt"
@@ -57,8 +57,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:
+                geo.transform_reconstruction_with_proj(r, projection)
+        else:
+            raise Exception(f"Invalid mode: {mode}")
+
         output = output or "reconstruction.geocoords.json"
         data.save_reconstruction(reconstructions, output)
 
@@ -69,37 +76,13 @@ def run_dataset(
         _transform_dense_point_cloud(udata, t, output_path)
 
 
-def _get_transformation(reference: TopocentricConverter, projection: pyproj.Proj, offset) -> np.ndarray:
-    """Get the linear transform from reconstruction coords to geocoords."""
-    p = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [0, 0, 0]]
-    q = [_transform(point, reference, projection) for point in p]
-
-    transformation = np.array(
-        [
-            [q[0][0] - q[3][0], q[1][0] - q[3][0], q[2][0] - q[3][0], q[3][0] - offset[0]],
-            [q[0][1] - q[3][1], q[1][1] - q[3][1], q[2][1] - q[3][1], q[3][1] - offset[1]],
-            [q[0][2] - q[3][2], q[1][2] - q[3][2], q[2][2] - q[3][2], q[3][2]],
-            [0, 0, 0, 1],
-        ]
-    )
-    return transformation
-
-
 def _write_transformation(transformation: np.ndarray, filename: str) -> None:
     """Write the 4x4 matrix transformation to a text file."""
     with io.open_wt(filename) as fout:
         for row in transformation:
             fout.write(u" ".join(map(str, row)))
             fout.write(u"\n")
 
-
-def _transform(point: Sequence, reference: TopocentricConverter, projection: pyproj.Proj) -> List[float]:
-    """Transform on point from local coords to a proj4 projection."""
-    lat, lon, altitude = reference.to_lla(point[0], point[1], point[2])
-    easting, northing = projection(lon, lat)
-    return [easting, northing, altitude]
-
-
 def _transform_image_positions(
     reconstructions: List[types.Reconstruction], transformation: np.ndarray, output: str
 ) -> None:
@@ -133,7 +116,6 @@ def _transform_reconstruction(
     for point in reconstruction.points.values():
         point.coordinates = list(np.dot(A, point.coordinates) + b)
 
-
 def _transform_dense_point_cloud(
     udata: UndistortedDataSet, transformation: np.ndarray, output_path: str
 ) -> None:

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,11 @@ 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",
+        )
 

opensfm/geo.py

@@ -1,6 +1,7 @@
 import numpy as np
+import pyproj
 from numpy import ndarray
-from typing import Tuple
+from typing import List, Tuple
 
 WGS84_a = 6378137.0
 WGS84_b = 6356752.314245
@@ -194,3 +195,91 @@ def to_lla(self, x, y, z):
 
     def __eq__(self, o):
         return np.allclose([self.lat, self.lon, self.alt], (o.lat, o.lon, o.alt))
+
+
+def construct_proj_transformer(
+    proj_str: str, inverse: bool = False
+) -> pyproj.Transformer:
+    """Construct a transformer between the given projection and WGS84."""
+    crs_4326 = pyproj.CRS.from_epsg(4326)
+    if inverse:
+        return pyproj.Transformer.from_proj(crs_4326, pyproj.CRS(proj_str))
+    return pyproj.Transformer.from_proj(pyproj.CRS(proj_str), crs_4326)
+
+
+def transform_to_proj(
+    point: List[float],
+    reference: TopocentricConverter,
+    projection: pyproj.Transformer,
+) -> List[float]:
+    """Transform a local topocentric point into the target projection."""
+    assert projection.source_crs.to_epsg() == 4326
+
+    lat, lon, altitude = reference.to_lla(point[0], point[1], point[2])
+    easting, northing = projection.transform(lat, lon)
+    return [easting, northing, altitude]
+
+
+def get_proj_transform_matrix(
+    reference: TopocentricConverter,
+    projection: pyproj.Transformer,
+    offset=(0, 0),
+) -> ndarray:
+    """Get the linear transform from reconstruction coords to geocoords."""
+    p = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [0, 0, 0]]
+    q = [transform_to_proj(point, reference, projection) for point in p]
+
+    return np.array(
+        [
+            [
+                q[0][0] - q[3][0],
+                q[1][0] - q[3][0],
+                q[2][0] - q[3][0],
+                q[3][0] - offset[0],
+            ],
+            [
+                q[0][1] - q[3][1],
+                q[1][1] - q[3][1],
+                q[2][1] - q[3][1],
+                q[3][1] - offset[1],
+            ],
+            [
+                q[0][2] - q[3][2],
+                q[1][2] - q[3][2],
+                q[2][2] - q[3][2],
+                q[3][2],
+            ],
+            [0, 0, 0, 1],
+        ]
+    )
+
+
+def transform_reconstruction_with_proj(
+    reconstruction, transformation: pyproj.Transformer
+) -> None:
+    """Apply a projection transformer to a reconstruction in-place."""
+    eps = 1e-3
+    for shot in reconstruction.shots.values():
+        origin = shot.pose.get_origin()
+
+        p0 = np.array(transform_to_proj(origin, reconstruction.reference, transformation))
+        px = np.array(
+            transform_to_proj(origin + [eps, 0, 0], reconstruction.reference, transformation)
+        )
+        py = np.array(
+            transform_to_proj(origin + [0, eps, 0], reconstruction.reference, transformation)
+        )
+        pz = np.array(
+            transform_to_proj(origin + [0, 0, eps], reconstruction.reference, transformation)
+        )
+        J = np.column_stack(((px - p0) / eps, (py - p0) / eps, (pz - p0) / eps))
+
+        shot.pose.set_origin(p0)
+        shot.pose.set_rotation_matrix(
+            np.dot(shot.pose.get_rotation_matrix(), np.linalg.inv(J))
+        )
+
+    for point in reconstruction.points.values():
+        point.coordinates = transform_to_proj(
+            point.coordinates, reconstruction.reference, transformation
+        )

opensfm/io.py

@@ -903,25 +903,40 @@ def _parse_utm_projection_string(line: str) -> str:
     return s.format(zone_number, zone_hemisphere)
 
 
-def _parse_projection(line: str) -> Optional[pyproj.Transformer]:
-    """Build a proj4 from the GCP format line."""
-    crs_4326 = pyproj.CRS.from_epsg(4326)
-    if line.strip() == "WGS84":
+def _parse_projection_string(line: str) -> Optional[str]:
+    """Parse the projection string from the GCP format line."""
+    line = line.strip()
+    if line == "WGS84":
         return None
     elif line.upper().startswith("WGS84 UTM"):
-        return pyproj.Transformer.from_proj(
-            pyproj.CRS(_parse_utm_projection_string(line)), crs_4326
-        )
-    elif "+proj" in line:
-        return pyproj.Transformer.from_proj(pyproj.CRS(line), crs_4326)
-    elif line.upper().startswith("EPSG:"):
-        return pyproj.Transformer.from_proj(
-            pyproj.CRS.from_epsg(int(line.split(":")[1])), crs_4326
-        )
+        return _parse_utm_projection_string(line)
+    elif "+proj" in line or line.upper().startswith("EPSG:"):
+        return line
     else:
         raise ValueError("Un-supported geo system definition: {}".format(line))
 
 
+def _parse_projection(line: str) -> Optional[pyproj.Transformer]:
+    """Build a proj4 transformer from the GCP format line."""
+    projection_string = _parse_projection_string(line)
+    if projection_string is None:
+        return None
+    return geo.construct_proj_transformer(projection_string)
+
+
+def read_gcp_projection_string(fileobj: IO[str]) -> Optional[str]:
+    """Read the projection string from a gcp_list.txt file."""
+    for line in fileobj:
+        if _valid_gcp_line(line):
+            return _parse_projection_string(line)
+    return None
+
+
+def read_gcp_projection(line: str) -> Optional[str]:
+    """Read the projection string from a GCP format line."""
+    return _parse_projection_string(line)
+
+
 def _valid_gcp_line(line: str) -> bool:
     stripped = line.strip()
     return stripped != "" and stripped[0] != "#"

opensfm/test/test_geo.py

@@ -1,4 +1,5 @@
 import numpy as np
+import pyproj
 from opensfm import geo, pygeo
 
 
@@ -42,3 +43,20 @@ def test_ecef_lla_topocentric_consistency_pygeo() -> None:
 def test_eq_geo() -> None:
     assert geo.TopocentricConverter(40,30,0) == geo.TopocentricConverter(40,30,0)
     assert geo.TopocentricConverter(40,32,0) != geo.TopocentricConverter(40,30,0)
+
+
+def test_transform_to_proj_matches_pyproj() -> None:
+    lat, lon, alt = 41.38946, 2.18378, 12.3
+    reference = geo.TopocentricConverter(lat, lon, alt)
+    proj = "+proj=utm +zone=31 +north +ellps=WGS84 +datum=WGS84 +units=m +no_defs"
+
+    transformer = geo.construct_proj_transformer(proj, inverse=True)
+    expected = pyproj.Transformer.from_proj(
+        pyproj.CRS.from_epsg(4326),
+        pyproj.CRS(proj),
+    ).transform(lat, lon)
+
+    easting, northing, altitude = geo.transform_to_proj([0, 0, 0], reference, transformer)
+
+    assert np.allclose((easting, northing), expected)
+    assert altitude == alt

opensfm/test/test_io.py

@@ -76,6 +76,14 @@ def test_parse_projection() -> None:
     assert np.allclose((lat, lon), (plat, plon))
 
 
+def test_parse_projection_string() -> None:
+    assert io.read_gcp_projection("WGS84") is None
+    assert io.read_gcp_projection("WGS84 UTM 31N") == (
+        "+proj=utm +zone=31 +north +ellps=WGS84 +datum=WGS84 +units=m +no_defs"
+    )
+    assert io.read_gcp_projection("EPSG:32631") == "EPSG:32631"
+
+
 def test_read_gcp_list() -> None:
     text = """WGS84
 13.400740745 52.519134104 12.0792090446 2335.0 1416.7 01.jpg