From 8f79e6c05aa1213cd8d61469eea56274c14c3f80 Mon Sep 17 00:00:00 2001 From: Piero Toffanin Date: Tue, 19 May 2020 21:43:34 +0000 Subject: [PATCH] Transform rotations Former-commit-id: 979da22936f4c0a5ee687affe9791e1dcf59c0f9 --- opendm/shots.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/opendm/shots.py b/opendm/shots.py index c4698118..662f21fa 100644 --- a/opendm/shots.py +++ b/opendm/shots.py @@ -11,6 +11,14 @@ def get_rotation_matrix(rotation): """Get rotation as a 3x3 matrix.""" return cv2.Rodrigues(rotation)[0] +def matrix_to_rotation(rotation_matrix): + R = np.array(rotation_matrix, dtype=float) + # if not np.isclose(np.linalg.det(R), 1): + # raise ValueError("Determinant != 1") + # if not np.allclose(np.linalg.inv(R), R.T): + # raise ValueError("Not orthogonal") + return cv2.Rodrigues(R)[0].ravel() + def get_origin(shot): """The origin of the pose in world coordinates.""" return -get_rotation_matrix(np.array(shot['rotation'])).T.dot(np.array(shot['translation'])) @@ -64,11 +72,17 @@ def get_geojson_shots_from_opensfm(reconstruction_file, geocoords_transformation cam = cameras[cam] Rs, T = geocoords[:3, :3], geocoords[:3, 3] + Rs1 = np.linalg.inv(Rs) origin = get_origin(shot) + # Translation utm_coords = np.dot(Rs, origin) + T trans_coords = crstrans.TransformPoint(utm_coords[0], utm_coords[1], utm_coords[2]) + # Rotation + rotation_matrix = get_rotation_matrix(np.array(shot['rotation'])) + rotation = matrix_to_rotation(np.dot(rotation_matrix, Rs1)) + feats.append({ 'type': 'Feature', 'properties': { @@ -76,7 +90,8 @@ def get_geojson_shots_from_opensfm(reconstruction_file, geocoords_transformation 'focal': cam.get('focal', cam.get('focal_x')), # Focal ratio = focal length (mm) / max(sensor_width, sensor_height) (mm) 'width': cam.get('width', 0), 'height': cam.get('height', 0), - 'rotation': shot.get('rotation', []) + 'translation': shot['translation'], + 'rotation': [rotation[0], rotation[1], rotation[2]] }, 'geometry':{ 'type': 'Point',