OpenDroneMap-ODM/opendm/geo.py

84 wiersze
3.3 KiB
Python

import os
import math
from opendm import log
from opendm import location
from pyproj import CRS
class GeoFile:
def __init__(self, geo_path):
self.geo_path = geo_path
self.entries = {}
self.srs = None
with open(self.geo_path, 'r') as f:
contents = f.read().strip()
lines = list(map(str.strip, contents.split('\n')))
if lines:
self.raw_srs = lines[0] # SRS
self.srs = location.parse_srs_header(self.raw_srs)
longlat = CRS.from_epsg("4326")
for line in lines[1:]:
if line != "" and line[0] != "#":
parts = line.split()
if len(parts) >= 3:
i = 3
filename = parts[0]
x, y = [float(p) for p in parts[1:3]]
z = float(parts[3]) if len(parts) >= 4 else None
# Always convert coordinates to WGS84
if z is not None:
x, y, z = location.transform3(self.srs, longlat, x, y, z)
else:
x, y = location.transform2(self.srs, longlat, x, y)
yaw = pitch = roll = None
if len(parts) >= 7:
yaw, pitch, roll = [float(p) for p in parts[4:7]]
if math.isnan(yaw) or math.isnan(pitch) or math.isnan(roll):
yaw = pitch = roll = None
i = 7
horizontal_accuracy = vertical_accuracy = None
if len(parts) >= 9:
horizontal_accuracy,vertical_accuracy = [float(p) for p in parts[7:9]]
i = 9
extras = " ".join(parts[i:])
self.entries[filename] = GeoEntry(filename, x, y, z,
yaw, pitch, roll,
horizontal_accuracy, vertical_accuracy,
extras)
else:
log.ODM_WARNING("Malformed geo line: %s" % line)
def get_entry(self, filename):
return self.entries.get(filename)
class GeoEntry:
def __init__(self, filename, x, y, z, yaw=None, pitch=None, roll=None, horizontal_accuracy=None, vertical_accuracy=None, extras=None):
self.filename = filename
self.x = x
self.y = y
self.z = z
self.yaw = yaw
self.pitch = pitch
self.roll = roll
self.horizontal_accuracy = horizontal_accuracy
self.vertical_accuracy = vertical_accuracy
self.extras = extras
def __str__(self):
return "{} ({} {} {}) ({} {} {}) ({} {}) {}".format(self.filename,
self.x, self.y, self.z,
self.yaw, self.pitch, self.roll,
self.horizontal_accuracy, self.vertical_accuracy,
self.extras).rstrip()
def position_string(self):
return "{} {} {}".format(self.x, self.y, self.z)