diff --git a/opendm/osfm.py b/opendm/osfm.py index 5c141991..1cc3c265 100644 --- a/opendm/osfm.py +++ b/opendm/osfm.py @@ -22,7 +22,7 @@ from opensfm.dataset import DataSet from opensfm import report from opendm.multispectral import get_photos_by_band from opendm.gpu import has_gpus -from opensfm import multiview +from opensfm import multiview, exif from opensfm.actions.export_geocoords import _transform class OSFMContext: @@ -117,14 +117,6 @@ class OSFMContext: except Exception as e: log.ODM_WARNING("Cannot set camera_models_overrides.json: %s" % str(e)) - # GPSDOP override if we have GPS accuracy information (such as RTK) - if 'gps_accuracy_is_set' in args: - log.ODM_INFO("Forcing GPS DOP to %s for all images" % args.gps_accuracy) - - for p in photos: - p.override_gps_dop(args.gps_accuracy) - - # Check image masks masks = [] for p in photos: @@ -270,14 +262,40 @@ class OSFMContext: def photos_to_metadata(self, photos, rerun=False): metadata_dir = self.path("exif") - if io.dir_exists(metadata_dir) and rerun: + + if io.dir_exists(metadata_dir) and not rerun: + log.ODM_WARNING("%s already exists, not rerunning photo to metadata" % metadata_dir) + return + + if io.dir_exists(metadata_dir): shutil.rmtree(metadata_dir) - os.makedirs(metadata_dir) + os.makedirs(metadata_dir, exist_ok=True) + camera_models = {} + data = DataSet(self.opensfm_project_path) + for p in photos: + d = p.to_opensfm_exif() with open(os.path.join(metadata_dir, "%s.exif" % p.filename), 'w') as f: - f.write(json.dumps(p.to_opensfm_exif())) + f.write(json.dumps(d)) + + camera_id = p.camera_id() + if camera_id not in camera_models: + camera = exif.camera_from_exif_metadata(d, data) + camera_models[camera_id] = camera + + # Override any camera specified in the camera models overrides file. + if data.camera_models_overrides_exists(): + overrides = data.load_camera_models_overrides() + if "all" in overrides: + for key in camera_models: + camera_models[key] = copy.copy(overrides["all"]) + camera_models[key].id = key + else: + for key, value in overrides.items(): + camera_models[key] = value + data.save_camera_models(camera_models) def is_feature_matching_done(self): features_dir = self.path("features") diff --git a/opendm/photo.py b/opendm/photo.py index 10e9ecdd..c9be3ab1 100644 --- a/opendm/photo.py +++ b/opendm/photo.py @@ -1,6 +1,7 @@ import logging import re import os +import math import exifread import numpy as np @@ -15,6 +16,7 @@ import xmltodict as x2d from opendm import get_image_size from xml.parsers.expat import ExpatError from opensfm.sensors import sensor_data +from opensfm.geo import ecef_from_lla projections = ['perspective', 'fisheye', 'brown', 'dual', 'equirectangular', 'spherical'] @@ -103,6 +105,14 @@ class ODM_Photo: self.irradiance_scale_to_si = None self.utc_time = None + # OPK angles + self.yaw = None + self.pitch = None + self.roll = None + self.omega = None + self.phi = None + self.kappa = None + # DLS self.sun_sensor = None self.dls_yaw = None @@ -139,6 +149,9 @@ class ODM_Photo: self.latitude = geo_entry.y self.longitude = geo_entry.x self.altitude = geo_entry.z + self.omega = geo_entry.omega + self.phi = geo_entry.phi + self.kappa = geo_entry.kappa self.dls_yaw = geo_entry.omega self.dls_pitch = geo_entry.phi self.dls_roll = geo_entry.kappa @@ -317,6 +330,11 @@ class ODM_Photo: if camera_projection in projections: self.camera_projection = camera_projection + # OPK + self.set_attr_from_xmp_tag('yaw', tags, ['@drone-dji:GimbalYawDegree', '@Camera:Yaw', 'Camera:Yaw'], float) + self.set_attr_from_xmp_tag('pitch', tags, ['@drone-dji:GimbalPitchDegree', '@Camera:Pitch', 'Camera:Pitch'], float) + self.set_attr_from_xmp_tag('roll', tags, ['@drone-dji:GimbalRollDegree', '@Camera:Roll', 'Camera:Roll'], float) + except Exception as e: log.ODM_WARNING("Cannot read XMP tags for %s: %s" % (_path_file, str(e))) @@ -335,6 +353,8 @@ class ODM_Photo: # Sanitize band name since we use it in folder paths self.band_name = re.sub('[^A-Za-z0-9]+', '', self.band_name) + self.compute_opk() + def extract_focal(self, make, model, tags): if make != "unknown": # remove duplicate 'make' information in 'model' @@ -586,6 +606,19 @@ class ODM_Photo: def is_thermal(self): return self.band_name.upper() in ["LWIR"] # TODO: more? + def camera_id(self): + return " ".join( + [ + "v2", + self.camera_make.strip(), + self.camera_model.strip(), + str(int(self.width)), + str(int(self.height)), + self.camera_projection, + str(float(self.focal_ratio))[:6], + ] + ).lower() + def to_opensfm_exif(self): capture_time = 0.0 if self.utc_time is not None: @@ -606,6 +639,14 @@ class ODM_Photo: gps['dop'] = dop + opk = {} + if self.omega and self.phi and self.kappa: + opk = { + 'omega': self.omega, + 'phi': self.phi, + 'kappa': self.kappa + } + return { "make": self.camera_make, "model": self.camera_model, @@ -616,15 +657,61 @@ class ODM_Photo: "orientation": self.orientation, "capture_time": capture_time, "gps": gps, - "camera": " ".join( - [ - "v2", - self.camera_make.strip(), - self.camera_model.strip(), - str(int(self.width)), - str(int(self.height)), - self.camera_projection, - str(float(self.focal_ratio))[:6], - ] - ).lower() - } \ No newline at end of file + "opk": opk, + "camera": self.camera_id() + } + + def compute_opk(self): + if self.yaw is not None and \ + self.pitch is not None and \ + self.roll is not None: + + y, p, r = self.yaw, self.pitch, self.roll + + # Ref: New Calibration and Computing Method for Direct + # Georeferencing of Image and Scanner Data Using the + # Position and Angular Data of an Hybrid Inertial Navigation System + # by Manfred Bäumker + + # YPR rotation matrix + cnb = np.array([[ math.cos(y) * math.cos(p), math.cos(y) * math.sin(p) * math.sin(r) - math.sin(y) * math.cos(r), math.cos(y) * math.sin(p) * math.cos(r) - math.sin(y) * math.sin(r)], + [ math.sin(y) * math.cos(p), math.sin(y) * math.sin(p) * math.sin(r) + math.cos(y) * math.cos(r), math.sin(y) * math.sin(p) * math.cos(r) - math.cos(y) * math.sin(r)], + [ -math.sin(p), math.cos(p) * math.sin(r), math.cos(p) * math.cos(r)], + ]) + + # Convert between image and body coordinates + # Top of image pixels point to flying direction + # and camera is looking down. + # We might need to change this if we want different + # camera mount orientations (e.g. backward or sideways) + + # (Swap X/Y, flip Z) + cbb = np.array([[0, 1, 0], + [1, 0, 0], + [0, 0, -1]]) + + delta = 1e-7 + + p1 = np.array(ecef_from_lla(self.latitude + delta, self.longitude, self.altitude)) + p2 = np.array(ecef_from_lla(self.latitude - delta, self.longitude, self.altitude)) + xnp = p1 - p2 + m = np.linalg.norm(xnp) + + if m == 0: + log.ODM_WARNING("Cannot compute OPK angles, divider = 0") + return + + # Unit vector pointing north + xnp /= m + + znp = np.array([0, 0, -1]).T + ynp = np.cross(znp, xnp) + + cen = np.array([xnp, ynp, znp]) + + # OPK rotation matrix + ceb = cen.dot(cnb).dot(cbb) + + self.omega = math.atan2(-ceb[1][2], ceb[2][2]) + self.phi = math.asin(ceb[0][2]) + self.kappa = math.atan2(-ceb[0][1], ceb[0][0]) diff --git a/stages/dataset.py b/stages/dataset.py index 8da125d9..6b168948 100644 --- a/stages/dataset.py +++ b/stages/dataset.py @@ -130,6 +130,13 @@ class ODMLoadDatasetStage(types.ODM_Stage): updated += 1 log.ODM_INFO("Updated %s image positions" % updated) + # GPSDOP override if we have GPS accuracy information (such as RTK) + if 'gps_accuracy_is_set' in args: + log.ODM_INFO("Forcing GPS DOP to %s for all images" % args.gps_accuracy) + + for p in photos: + p.override_gps_dop(args.gps_accuracy) + # Save image database for faster restart save_images_database(photos, images_database_file) else: