kopia lustrzana https://github.com/OpenDroneMap/ODM
WIP OPK angle computation
Piero Toffanin
rodzic
b544ca2464
commit
3f8765f5e7
|
|
@ -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")
|
||||
|
|
|
|||
109
opendm/photo.py
109
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()
|
||||
"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])
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue