def prepare_imugps_Hyspex(processed_imugps_file, raw_imugps_file,
boresight_offsets, map_crs, boresight_options):
""" Prepare Hyspex IMU and GPS data.
Parameters
----------
processed_imugps_file: str
Processed IMUGPS filename.
raw_imugps_file: str
Raw IMUGPS filename.
boresight_offsets: list of float
Boresight offsets, [roll_offset, pitch_offset, heading_offset, altitude_offset].
boresight_options: list of boolean
Boresight offset options, true or false.
map_crs: osr object
Map coordinate system.
"""
if os.path.exists(processed_imugps_file):
logger.info('Write the IMU and GPS data to %s.' %
processed_imugps_file)
return
from Geography import define_wgs84_crs, get_grid_convergence
# Load raw GPS & IMU data
raw_imugps = np.loadtxt(raw_imugps_file)
# Apply boresight offsets
processed_imugps = np.zeros((raw_imugps.shape[0], 15))
# Scan line index
processed_imugps[:, 0] = raw_imugps[:, 0]
# GPS & IMU
wgs84_crs = define_wgs84_crs()
transform = osr.CoordinateTransformation(wgs84_crs, map_crs)
xyz = np.array(transform.TransformPoints(raw_imugps[:, 1:3]))
processed_imugps[:, 1] = xyz[:, 0] # Flight easting
processed_imugps[:, 2] = xyz[:, 1] # Flight northing
processed_imugps[:, 3] = raw_imugps[:, 3] # Flight altitude
processed_imugps[:, 4] = raw_imugps[:, 4] # Roll
processed_imugps[:, 5] = raw_imugps[:, 5] # Pitch
processed_imugps[:, 6] = raw_imugps[:, 6] # Heading
del transform, xyz
# Boresight offsets
for i in range(len(boresight_options)):
if boresight_options[i]:
processed_imugps[:, 7 + i] = boresight_offsets[i]
# Grid convergence
grid_convergence = get_grid_convergence(raw_imugps[:, 1], raw_imugps[:, 2],
map_crs)
processed_imugps[:, 11] = grid_convergence
# Longitude & latitude
processed_imugps[:, 12] = raw_imugps[:, 1] # Longitude
processed_imugps[:, 13] = raw_imugps[:, 2] # Latitude
# Timestamp
processed_imugps[:, 14] = raw_imugps[:, 7]
# Write updated GPS & IMU data to file
header = [
'Map coordinate system = %s' % (map_crs.ExportToWkt()), 'Index ' +
'Map_X Map_Y Map_Z Roll Pitch Heading ' +
'Roll_Offset Pitch_Offset Heading_Offset Altitude_Offset Grid_Convergence '
+ 'Longitude Latitude ' + 'Timestamp'
]
np.savetxt(
processed_imugps_file,
processed_imugps,
header='\n'.join(header),
fmt=
'%d %.3f %.3f %.3f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.5f'
)
logger.info('Write the IMU and GPS data to %s.' % processed_imugps_file)
def boresight_calibration(boresight_file, gcp_file, imugps_file,
sensor_model_file, dem_image_file,
boresight_options):
""" Do boresight calibration.
Arguments:
boresight_file: str
Boresight filename.
gcp_file: str
Ground control points filename.
igm_image_file: str
The IGM image filename.
imugps_file: str
The IMUGPS filename.
sensor_model_file: str
The sensor model filename.
dem_image_file: str
The DEM image filename.
boresight_options: list of boolean
Boresight offset options, true or false.
"""
if os.path.exists(boresight_file):
logger.info('Write boresight data to %s.' % boresight_file)
return
from osgeo import gdal, osr
from Geography import define_wgs84_crs
from scipy import optimize
# Read IMU and GPS data.
imugps = np.loadtxt(
imugps_file
) # ID, X, Y, Z, R, P, H, Timestamp, Longitude, Latitude, Grid_Convergence, Roll...
# Read sensor model data.
sensor_model = np.loadtxt(sensor_model_file, skiprows=1)[:, 1:]
# Read DEM data.
ds = gdal.Open(dem_image_file, gdal.GA_ReadOnly)
dem_image = ds.GetRasterBand(1).ReadAsArray()
dem_geotransform = ds.GetGeoTransform()
dem_prj = ds.GetProjection()
ds = None
# Read GCP data.
gcp_data = np.loadtxt(
gcp_file, comments=';') # Longitude, Latitude, Image Column, Image Row
# Convert gcp longitudes and latitudes to map x and y.
wgs84_crs = define_wgs84_crs()
map_crs = osr.SpatialReference()
map_crs.ImportFromWkt(dem_prj)
transform = osr.CoordinateTransformation(wgs84_crs, map_crs)
gcp_xyz = np.array(transform.TransformPoints(gcp_data[:, :2]))
del wgs84_crs, transform
# Interpolate flight imu and xyz.
lines = np.arange(imugps.shape[0])
flight_xyz = np.zeros((gcp_data.shape[0], 3))
flight_xyz[:, 0] = np.interp(gcp_data[:, 3] - 1, lines, imugps[:, 1])
flight_xyz[:, 1] = np.interp(gcp_data[:, 3] - 1, lines, imugps[:, 2])
flight_xyz[:, 2] = np.interp(gcp_data[:, 3] - 1, lines, imugps[:, 3])
flight_imu = np.zeros((gcp_data.shape[0], 3))
flight_imu[:, 0] = np.interp(gcp_data[:, 3] - 1, lines, imugps[:, 4])
flight_imu[:, 1] = np.interp(gcp_data[:, 3] - 1, lines, imugps[:, 5])
flight_imu[:, 2] = np.interp(gcp_data[:, 3] - 1, lines, imugps[:, 6])
# Apply grid convergence.
flight_imu[:, 2] = flight_imu[:, 2] - np.interp(gcp_data[:, 3] - 1, lines,
imugps[:, 11])
del lines
# Interpolate sensor model.
samples = np.arange(sensor_model.shape[0])
flight_sensor_model = np.zeros((gcp_data.shape[0], 2))
flight_sensor_model[:, 0] = np.interp(gcp_data[:, 2] - 1, samples,
sensor_model[:, 0])
flight_sensor_model[:, 1] = np.interp(gcp_data[:, 2] - 1, samples,
sensor_model[:, 1])
del samples
# Optimize.
p = optimize.minimize(cost_fun, [0, 0, 0, 0],
method='L-BFGS-B',
args=(flight_xyz, flight_imu, flight_sensor_model,
dem_image,
[dem_geotransform[0],
dem_geotransform[3]], dem_geotransform[1],
gcp_xyz, boresight_options))
logger.info('Roll, pitch, heading and altitude offsets: %s, %s, %s, %s' %
(p.x[0], p.x[1], p.x[2], p.x[3]))
# Save offsets.
imugps[:, 7] = p.x[0]
imugps[:, 8] = p.x[1]
imugps[:, 9] = p.x[2]
imugps[:, 10] = p.x[3]
header = [
'Map coordinate system = %s' % (map_crs.ExportToWkt()),
'Index Map_X Map_Y Map_Z Roll Pitch Heading ' +
'Roll_Offset Pitch_Offset Heading_Offset Altitude_Offset Grid_Convergence '
+ 'Longitude Latitude Timestamp'
]
np.savetxt(
imugps_file,
imugps,
header='\n'.join(header),
fmt=
'%d %.3f %.3f %.3f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.5f'
)
# Estimate geometric correction accuracy.
est_gcp_xyz = estimate_gcp_xyz(p.x, flight_xyz, flight_imu,
flight_sensor_model, dem_image,
[dem_geotransform[0], dem_geotransform[3]],
dem_geotransform[1], boresight_options)
boresight_data = np.zeros((gcp_data.shape[0], 10))
boresight_data[:, 0] = np.arange(gcp_data.shape[0])
boresight_data[:, 1] = gcp_data[:, 2]
boresight_data[:, 2] = gcp_data[:, 3]
boresight_data[:, 3] = gcp_xyz[:, 0]
boresight_data[:, 4] = gcp_xyz[:, 1]
boresight_data[:, 5] = est_gcp_xyz[:, 0]
boresight_data[:, 6] = est_gcp_xyz[:, 1]
boresight_data[:, 7] = gcp_xyz[:, 0] - est_gcp_xyz[:, 0]
boresight_data[:, 8] = gcp_xyz[:, 1] - est_gcp_xyz[:, 1]
boresight_data[:, 9] = np.sqrt(boresight_data[:, 7]**2 +
boresight_data[:, 8]**2)
header = [
'Map coordinate system = %s' % (map_crs.ExportToWkt()),
'Roll offset = %s' % (p.x[0]),
'Pitch offset = %s' % (p.x[1]),
'Heading offset = %s' % (p.x[2]),
'Altitude offset = %s' % (p.x[3]),
'Min RMS = %.4f' % (boresight_data[:, 9].min()),
'Mean RMS = %.4f' % (boresight_data[:, 9].mean()),
'Max RMS = %.4f' % (boresight_data[:, 9].max()),
'Index Image_X Image_Y Map_X Map_Y Predict_X Predict_Y Error_X Error_Y RMS'
]
np.savetxt(
boresight_file,
boresight_data,
header='\n'.join(header),
fmt=
'%d %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f'
)
logger.info('Boresight accuracy (min, mean, max): %.2f, %.2f, %2.f.' %
(boresight_data[:, 9].min(), boresight_data[:, 9].mean(),
boresight_data[:, 9].max()))
del boresight_data
logger.info('Write boresight data to %s.' % boresight_file)