diff --git a/opensfm/actions/export_geocoords.py b/opensfm/actions/export_geocoords.py index 98294d4f3..71326677c 100644 --- a/opensfm/actions/export_geocoords.py +++ b/opensfm/actions/export_geocoords.py @@ -3,12 +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 opensfm.reconstruction import bundle_shot_poses -from typing import List, Sequence +from typing import List logger: logging.Logger = logging.getLogger(__name__) @@ -43,8 +40,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" @@ -64,7 +61,7 @@ def run_dataset( _transform_reconstruction(r, t) elif mode == "projected": for r in reconstructions: - _transform_reconstruction_projected(r, t, offset[0], offset[1], reference, projection, data) + geo.transform_reconstruction_with_proj(r, projection) else: raise Exception(f"Invalid mode: {mode}") @@ -78,22 +75,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: @@ -101,14 +82,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: @@ -142,41 +115,6 @@ 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 ) -> None: 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 ca9896fb7..4ad980691 100644 --- a/opensfm/io.py +++ b/opensfm/io.py @@ -902,25 +902,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