OpenSfM camera location export to GeoJSON

Former-commit-id: d91c94808c

opendm/pseudogeo.py

@@ -4,6 +4,9 @@ from gdalconst import GA_Update
 from opendm import io
 from opendm import log
 
+def get_pseudogeo_utm():
+    return '+proj=utm +zone=30 +ellps=WGS84 +datum=WGS84 +units=m +no_defs'
+
 def add_pseudo_georeferencing(geotiff, scale=1.0):
     if not io.file_exists(geotiff):
         log.ODM_WARNING("Cannot add pseudo georeferencing, %s does not exist" % geotiff)
@@ -14,7 +17,7 @@ def add_pseudo_georeferencing(geotiff, scale=1.0):
 
         dst_ds = gdal.Open(geotiff, GA_Update)
         srs = osr.SpatialReference()
-        srs.ImportFromProj4('+proj=utm +zone=30 +ellps=WGS84 +datum=WGS84 +units=m +no_defs')
+        srs.ImportFromProj4(get_pseudogeo_utm())
         dst_ds.SetProjection( srs.ExportToWkt() )
         dst_ds.SetGeoTransform( [ 0.0, scale, 0.0, 0.0, 0.0, -scale ] )
         dst_ds = None

opendm/shots.py

@@ -0,0 +1,79 @@
+import os, json
+from opendm import log
+from opendm.pseudogeo import get_pseudogeo_utm
+from opendm.location import transformer
+from pyproj import CRS
+import numpy as np
+import cv2
+
+def get_rotation_matrix(rotation):
+    """Get rotation as a 3x3 matrix."""
+    return cv2.Rodrigues(rotation)[0]
+
+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']))
+
+def extract_shots_from_opensfm(reconstruction_file, geocoords_transformation_file=None, utm_srs=None):
+    """
+    Extract shots from OpenSfM's reconstruction.json
+    """
+
+    # Read transform (if available)
+    if geocoords_transformation_file is not None and utm_srs is not None and os.path.exists(geocoords_transformation_file):
+        geocoords = np.loadtxt(geocoords_transformation_file, usecols=range(4))
+    else:
+        # pseudogeo transform
+        utm_srs = get_pseudogeo_utm()
+        geocoords = np.identity(4)
+
+    crstrans = transformer(CRS.from_proj4(utm_srs), CRS.from_epsg("4326"))
+
+    if os.path.exists(reconstruction_file):
+        with open(reconstruction_file, 'r') as fin:
+            reconstructions = json.loads(fin.read())
+            
+            feats = []
+            cameras = {}
+            added_shots = {}
+            for recon in reconstructions:
+                if 'cameras' in recon:
+                    cameras = recon['cameras']
+                
+            for filename in recon.get('shots', {}):
+                shot = recon['shots'][filename]
+                cam = shot.get('camera')
+                if (not cam in cameras) or (filename in added_shots):
+                    continue
+                
+                cam = cameras[cam]
+                R, T = geocoords[:3, :3], geocoords[:3, 3]
+                origin = get_origin(shot)
+
+                utm_coords = np.dot(R, origin) + T
+                trans_coords = crstrans.TransformPoint(utm_coords[0], utm_coords[1], utm_coords[2])
+
+                feats.append({
+                    'type': 'Feature',
+                    'properties': {
+                        'filename': filename,
+                        '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', [])
+                    },
+                    'geometry':{
+                        'type': 'Point',
+                        'coordinates': list(trans_coords)
+                    }
+                })
+
+                added_shots[filename] = True
+
+        return {
+            'type': 'FeatureCollection',
+            'features': feats
+        }
+    else:
+        raise RuntimeError("%s does not exist." % reconstruction_file)
+

opendm/types.py

@@ -125,7 +125,11 @@ class ODM_Reconstruction(object):
         # dataset is not georeferenced)
         if self.is_georeferenced():
             with open(file, 'w') as f:
-                f.write(self.georef.proj4())
+                f.write(self.get_proj_srs())
+
+    def get_proj_srs(self):
+        if self.is_georeferenced():
+            return self.georef.proj4()
 
     def get_photo(self, filename):
         for p in self.photos: