Ensure consistent use of pyproj.Transform for Lat/Lon<->Proj CS conversions

Author: Yann N.

opensfm/actions/export_geocoords.py

@@ -7,8 +7,8 @@
 import pyproj
 from numpy.typing import NDArray
 from opensfm import io, types
+from opensfm import geo
 from opensfm.dataset import DataSet, UndistortedDataSet
-from opensfm.geo import TopocentricConverter
 
 logger: logging.Logger = logging.getLogger(__name__)
 
@@ -40,8 +40,8 @@ def run_dataset(
 
     reference = data.load_reference()
 
-    projection = pyproj.Proj(proj)
-    t = _get_transformation(reference, projection)
+    projection = geo.construct_proj_transformer(proj, inverse=True)
+    t = geo.get_proj_transform_matrix(reference, projection)
 
     if transformation:
         output = output or "geocoords_transformation.txt"
@@ -57,7 +57,7 @@ def run_dataset(
     if reconstruction:
         reconstructions = data.load_reconstruction()
         for r in reconstructions:
-            _transform_reconstruction(r, t)
+            geo.transform_reconstruction_with_proj(r, projection)
         output = output or "reconstruction.geocoords.json"
         data.save_reconstruction(reconstructions, output)
 
@@ -68,24 +68,6 @@ def run_dataset(
         _transform_dense_point_cloud(udata, t, output_path)
 
 
-def _get_transformation(
-    reference: TopocentricConverter, projection: pyproj.Proj
-) -> 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]],
-            [q[0][1] - q[3][1], q[1][1] - q[3][1], q[2][1] - q[3][1], q[3][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: NDArray, filename: str) -> None:
     """Write the 4x4 matrix transformation to a text file."""
     with io.open_wt(filename) as fout:
@@ -94,15 +76,6 @@ def _write_transformation(transformation: NDArray, filename: str) -> None:
             fout.write("\n")
 
 
-def _transform(
-    point: Sequence[float], 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: NDArray, output: str
 ) -> None:
@@ -121,22 +94,6 @@ def _transform_image_positions(
         fout.write(text)
 
 
-def _transform_reconstruction(
-    reconstruction: types.Reconstruction, transformation: NDArray
-) -> None:
-    """Apply a transformation to a reconstruction in-place."""
-    A, b = transformation[:3, :3], transformation[:3, 3]
-    A1 = np.linalg.inv(A)
-
-    for shot in reconstruction.shots.values():
-        R = shot.pose.get_rotation_matrix()
-        shot.pose.set_rotation_matrix(np.dot(R, A1))
-        shot.pose.set_origin(np.dot(A, shot.pose.get_origin()) + b)
-
-    for point in reconstruction.points.values():
-        point.coordinates = list(np.dot(A, point.coordinates) + b)
-
-
 def _transform_dense_point_cloud(
     udata: UndistortedDataSet, transformation: NDArray, output_path: str
 ) -> None:

opensfm/geo.py

@@ -1,7 +1,8 @@
 # pyre-strict
-from typing import overload, Sequence, Tuple, Union
+from typing import List, overload, Sequence, Tuple, Union
 
 import numpy as np
+import pyproj
 from numpy.typing import NDArray
 
 Scalars = Union[float, NDArray]
@@ -250,26 +251,6 @@ def gps_distance(
 def gps_distance(latlon_1: NDArray, latlon_2: NDArray) -> NDArray: ...
 
 
-def gps_distance(
-    latlon_1: Union[Sequence[Scalars], NDArray],
-    latlon_2: Union[Sequence[Scalars], NDArray],
-) -> Scalars:
-    """
-    Distance between two (lat,lon) pairs.
-
-    >>> p1 = (42.1, -11.1)
-    >>> p2 = (42.2, -11.3)
-    >>> 19000 < gps_distance(p1, p2) < 20000
-    True
-    """
-    x1, y1, z1 = ecef_from_lla(latlon_1[0], latlon_1[1], 0.0)
-    x2, y2, z2 = ecef_from_lla(latlon_2[0], latlon_2[1], 0.0)
-
-    dis = np.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
-
-    return dis
-
-
 class TopocentricConverter:
     """Convert to and from a topocentric reference frame."""
 
@@ -311,3 +292,108 @@ def to_lla(
 
     def __eq__(self, o: "TopocentricConverter") -> bool:
         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 pyproj Transformer object, converting between the given projection antod WGS84 (EPSG:4326).
+    If inverse is True, the transformation is from WGS84 to the given projection.
+    """
+    crs_4326 = pyproj.CRS.from_epsg(4326)
+    if inverse:
+        return pyproj.Transformer.from_proj(crs_4326, pyproj.CRS(proj_str))
+    else:
+        return pyproj.Transformer.from_proj(pyproj.CRS(proj_str), crs_4326)
+
+
+def transform_to_proj(
+    point: Sequence[float], reference: TopocentricConverter, projection: pyproj.Transformer
+) -> List[float]:
+    """
+    Transform a point defined wrt. the local topocentric frame to a projection
+    defined by the given Transformer. We assume the Transformer goes from
+    WGS84 to the desired projection.
+    """
+    assert projection.source_crs.to_epsg() == 4326, "Transformer source CRS must be WGS84 (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
+) -> 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]
+
+    transformation = np.array(
+        [
+            [q[0][0] - q[3][0], q[1][0] - q[3][0], q[2][0] - q[3][0], q[3][0]],
+            [q[0][1] - q[3][1], q[1][1] - q[3][1], q[2][1] - q[3][1], q[3][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 transform_reconstruction_with_matrix(
+    reconstruction: "types.Reconstruction", transformation: NDArray
+) -> None:
+    """Apply a rigid transformation to a reconstruction in-place."""
+    A, b = transformation[:3, :3], transformation[:3, 3]
+    A1 = np.linalg.inv(A)
+
+    for shot in reconstruction.shots.values():
+        R = shot.pose.get_rotation_matrix()
+        shot.pose.set_rotation_matrix(np.dot(R, A1))
+        shot.pose.set_origin(np.dot(A, shot.pose.get_origin()) + b)
+
+    for point in reconstruction.points.values():
+        point.coordinates = list(np.dot(A, point.coordinates) + b)
+
+
+def transform_reconstruction_with_proj(
+    reconstruction: "types.Reconstruction", transformation: pyproj.Transformer
+) -> None:
+    """Apply a proj Transformer to a reconstruction in-place."""
+    eps = 1e-3
+    for shot in reconstruction.shots.values():
+        origin = shot.pose.get_origin()
+                    
+        # Jacobian for rotation update
+        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(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
+        )
+
+
+def gps_distance(
+    latlon_1: Union[Sequence[Scalars], NDArray],
+    latlon_2: Union[Sequence[Scalars], NDArray],
+) -> Scalars:
+    """
+    Distance between two (lat,lon) pairs.
+
+    >>> p1 = (42.1, -11.1)
+    >>> p2 = (42.2, -11.3)
+    >>> 19000 < gps_distance(p1, p2) < 20000
+    True
+    """
+    x1, y1, z1 = ecef_from_lla(latlon_1[0], latlon_1[1], 0.0)
+    x2, y2, z2 = ecef_from_lla(latlon_2[0], latlon_2[1], 0.0)
+
+    dis = np.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
+
+    return dis

opensfm/io.py

@@ -903,25 +903,36 @@ 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 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[pyproj.Transformer]:
+    """Build a proj4 from the GCP format line."""
+    projection_string = _parse_projection_string(line)
+    if projection_string is None:
+        return None
+
+    return projection_string
+
+
 def _valid_gcp_line(line: str) -> bool:
     stripped = line.strip()
     return stripped != "" and stripped[0] != "#"
@@ -937,7 +948,8 @@ def read_gcp_list(
     """
     all_lines = fileobj.readlines()
     lines = iter(filter(_valid_gcp_line, all_lines))
-    projection = _parse_projection(next(lines))
+    projection_string = read_gcp_projection(next(lines))
+    projection = geo.construct_proj_transformer(projection_string) if projection_string else None
     points = _read_gcp_list_lines(lines, projection, exif)
     return points