diff --git a/opensfm/actions/export_geocoords.py b/opensfm/actions/export_geocoords.py index 142f08085..0b1c13f90 100644 --- a/opensfm/actions/export_geocoords.py +++ b/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,22 +76,6 @@ 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: @@ -92,14 +83,6 @@ def _write_transformation(transformation: np.ndarray, filename: str) -> None: 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: diff --git a/opensfm/commands/export_geocoords.py b/opensfm/commands/export_geocoords.py index 9ba044495..9ef0b9a21 100644 --- a/opensfm/commands/export_geocoords.py +++ b/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", + ) diff --git a/opensfm/geo.py b/opensfm/geo.py index a0324f9b9..ad90f8512 100644 --- a/opensfm/geo.py +++ b/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 + ) diff --git a/opensfm/io.py b/opensfm/io.py index d7f8756ce..33c8b8556 100644 --- a/opensfm/io.py +++ b/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] != "#" diff --git a/opensfm/test/test_geo.py b/opensfm/test/test_geo.py index a6d534be4..78a6c48b3 100644 --- a/opensfm/test/test_geo.py +++ b/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 diff --git a/opensfm/test/test_io.py b/opensfm/test/test_io.py index 23c90c140..fa4abb288 100644 --- a/opensfm/test/test_io.py +++ b/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