def calculate_attenuation_parameters(images, distances, image_height,
image_width, image_channels):
"""Compute attenuation parameters for all images
Parameters
-----------
images : numpy.ndarray
image memmap reshaped as a vector
distances : numpy.ndarray
distance memmap reshaped as a vector
image_height : int
height of an image
image_width : int
width of an image
Returns
-------
numpy.ndarray
attenuation_parameters
"""
image_attenuation_parameters = np.empty(
(image_channels, image_height, image_width, 3), dtype=np.float32)
# Check available RAM and allocate threads accordingly
mem = psutil.virtual_memory()
available_bytes = mem.available # in bytes
required_bytes = image_channels * image_height * image_width * 4 * len(
images)
num_jobs = min(int(available_bytes / required_bytes), 12)
# Keep one alive!
cpus = psutil.cpu_count() - 1
if num_jobs > cpus:
num_jobs = cpus
elif num_jobs <= 0:
num_jobs = 1
Console.warn("You might have not enough available RAM to continue.")
if num_jobs < cpus - 1:
Console.info("Assigning", num_jobs, "jobs to your CPU to save RAM")
else:
Console.info("Assigning", num_jobs, "jobs to your CPU")
for i_channel in range(image_channels):
with tqdm_joblib(
tqdm(desc="Curve fitting", total=image_height * image_width)):
results = joblib.Parallel(n_jobs=num_jobs, verbose=0)([
joblib.delayed(curve_fitting)(distances[:, i_pixel],
images[:, i_pixel, i_channel])
for i_pixel in range(image_height * image_width)
])
attenuation_parameters = np.array(results)
attenuation_parameters = attenuation_parameters.reshape(
[image_height, image_width, 3])
image_attenuation_parameters[i_channel] = attenuation_parameters
return image_attenuation_parameters
def call_rescale(args):
path = Path(args.path).resolve()
time_string = time.strftime("%Y%m%d_%H%M%S", time.localtime())
Console.set_logging_file(
get_processed_folder(path) /
("log/" + time_string + "_correct_images_rescale.log"))
correct_config, camerasystem = load_configuration_and_camera_system(
path, args.suffix)
# install freeimage plugins if not installed
imageio.plugins.freeimage.download()
if correct_config.camerarescale is None:
Console.error("Camera rescale configuration not found")
Console.error(
"Please populate the correct_images.yaml file with a rescale configuration"
)
Console.quit("Malformed correct_images.yaml file")
# obtain parameters for rescale from correct_config
rescale_cameras = correct_config.camerarescale.rescale_cameras
for camera in rescale_cameras:
corrections.rescale_camera(path, camerasystem, camera)
Console.info("Rescaling completed for all cameras ...")
def save_cloud(filename, cloud):
"""Write list of 3D points to ply file
Returns
-------
None
"""
cloud_size = len(cloud)
header_msg = "ply\n\
format ascii 1.0\n\
element vertex {0}\n\
property float x\n\
property float y\n\
property float z\n\
end_header\n".format(
cloud_size
)
Console.info("Saving cloud to " + str(filename))
with filename.open("w") as f:
f.write(header_msg)
for p in cloud:
f.write("{0:.5f} {1:.5f} {2:.5f}\n".format(p[0][0], p[1][0], p[2][0]))
def write_csv(
csv_filepath: Path,
data_list: Union[List[BodyVelocity], List[Orientation], List[Depth],
List[Altitude], List[Usbl], List[Tide], List[Other],
List[Camera], List[SyncedOrientationBodyVelocity], ],
csv_filename: str,
csv_flag: Optional[bool] = True,
mutex: Optional[Lock] = None,
):
if csv_flag is True and len(data_list) > 1:
csv_file = Path(csv_filepath)
if not csv_file.exists():
if mutex is not None:
mutex.acquire()
csv_file.mkdir(parents=True, exist_ok=True)
if mutex is not None:
mutex.release()
Console.info("Writing outputs to {}.csv ...".format(csv_filename))
file = csv_file / "{}.csv".format(csv_filename)
covariance_file = csv_file / "{}_cov.csv".format(csv_filename)
fileout = None
fileout_cov = None
if len(data_list) > 0:
fileout = file.open("w")
# write headers
str_to_write = data_list[0].get_csv_header()
fileout.write(str_to_write)
if hasattr(data_list[0], "covariance") and hasattr(
data_list[0], "get_csv_header_cov"):
if data_list[0].covariance is not None:
fileout_cov = covariance_file.open("w")
str_to_write_cov = data_list[0].get_csv_header_cov()
fileout_cov.write(str_to_write_cov)
# Loop for each line in csv
for i in range(len(data_list)):
try:
str_to_write = data_list[i].to_csv_row()
if fileout_cov is not None:
str_to_write_cov = data_list[i].to_csv_cov_row()
fileout_cov.write(str_to_write_cov)
fileout.write(str_to_write)
except IndexError:
Console.error(
"There is something wrong with camera filenames and \
indexing for the file",
csv_filename,
)
Console.quit("Check write_csv function.")
fileout.close()
if fileout_cov is not None:
fileout_cov.close()
Console.info("... done writing {}.csv.".format(csv_filename))
else:
Console.warn("Empty data list {}".format(str(csv_filename)))
def print_report(self):
Console.info("EKF measurements report:")
for key in self.measurements:
Console.info(
"\t",
key,
"measurements:",
self.measurements[key].dropped,
"/",
self.measurements[key].total,
"dropped",
)
def test_console(self):
with patch.object(Console, "get_version", return_value="testing"):
Console.warn("This is a warning")
Console.error("This is an error message")
Console.info("This is an informative message")
Console.banner()
Console.get_username()
Console.get_hostname()
Console.get_date()
Console.get_version()
for i in range(1, 10):
Console.progress(i, 10)
def set_loader(self, loader_name):
if loader_name == "xviii":
self._loader = xviii.loader
self._loader_name = "xviii"
elif loader_name == "default":
self._loader = default.loader
self._loader_name = "default"
elif loader_name == "rosbag":
self._loader = rosbag.loader
self._loader_name = "rosbag"
else:
Console.quit("The loader type", loader_name, "is not implemented.")
Console.info("Loader set to", loader_name)
def parse_NOC_polpred(mission, vehicle, category, ftype, outpath):
# parser meta data
sensor_string = "autosub"
category = category
output_format = ftype
if category == Category.TIDE:
filepath = mission.tide.filepath
timezone = mission.tide.timezone
timeoffset = mission.tide.timeoffset
timezone_offset = read_timezone(timezone)
tide = Tide(mission.tide.std_offset)
tide.sensor_string = sensor_string
path = get_raw_folder(outpath / ".." / filepath)
file_list = get_file_list(path)
data_list = []
Console.info("... parsing NOC tide data")
# Data format sample
# Date Time Level Speed Direc'n
# m m/s deg
# 6/ 9/2019 00:00 0.74 0.14 51
# 6/ 9/2019 01:00 0.58 0.15 56
for file in file_list:
with file.open("r", errors="ignore") as tide_file:
for line in tide_file.readlines()[6:]:
# we have to skip the first 5 rows of the file
dd = int(line[0:2])
mm = int(line[3:5])
yyyy = int(line[6:10])
hour = int(line[12:14])
mins = int(line[15:17])
secs = 0 # current models only provide resolution in minutes
msec = 0
epoch_time = date_time_to_epoch(yyyy, mm, dd, hour, mins,
secs, timezone_offset)
epoch_timestamp = epoch_time + msec / 1000 + timeoffset
tide.epoch_timestamp = epoch_timestamp
tide.height = float(line[22:28])
tide.height_std = tide.height * tide.height_std_factor
data = tide.export(output_format)
data_list.append(data)
return data_list
def determine_extension_and_images_per_folder(folder_path, image_list, label):
"""Determine filename extension and number of images per subfolder
The number of images per subfolder dertermines how the subfolders are
named. The subfolder name is 3 letters long and either starts from the
first (index = 0) or the second (index = 1) digit of the image number.
:param folder_path: Path where images are stored
:type folder_path: pathlib.Path
:param image_list: list of image 7-digit zeropadded image numbers
:type image_list: list of str
:param label: Camera label
:type label: str
:returns:
-index_start_of_folder_name (`int`) - Index where subfolder name starts
-extension (`str`) - Filename extension of images ("jpg" or "tif")
"""
Console.info(" Determine filename extension and images per subfolder "
"of camera {}...".format(label))
if build_image_path(folder_path, image_list[-1], 0, "jpg").is_file():
index_start_of_folder_name = 0
extension = "jpg"
Console.info(' ...Filename extension: "{}", 10000 images '
"per subfolder.".format(extension))
elif build_image_path(folder_path, image_list[-1], 1, "jpg").is_file():
index_start_of_folder_name = 1
extension = "jpg"
Console.info(' ...Filename extension: "{}", 1000 images '
"per subfolder.".format(extension))
elif build_image_path(folder_path, image_list[-1], 0, "tif").is_file():
index_start_of_folder_name = 0
extension = "tif"
Console.info(' ...Filename extension: "{}", 10000 images '
"per subfolder.".format(extension))
elif build_image_path(folder_path, image_list[-1], 1, "tif").is_file():
index_start_of_folder_name = 1
extension = "tif"
Console.info(' ...Filename extension: "{}", 1000 images '
"per subfolder.".format(extension))
else:
index_start_of_folder_name = 0
extension = "jpg"
Console.warn(" ...Did not find images from camera {} in {}. "
'Default to using extension "{}" and 10000 images per '
"subfolder.".format(label, folder_path, extension))
return index_start_of_folder_name, extension
def parse(self):
# parse phins data
Console.info("... parsing phins standard data")
data_list = []
path = get_raw_folder(self.outpath / ".." / self.filepath /
self.filename)
with path.open("r", encoding="utf-8", errors="ignore") as filein:
for complete_line in filein.readlines():
line_and_md5 = complete_line.strip().split("*")
line = line_and_md5[0].strip().split(",")
if not self.line_is_valid(line, line_and_md5):
continue
header = line[1]
data = self.process_line(header, line)
if data is not None:
data_list.append(data)
return data_list
def load_memmap_from_numpyfilelist(filepath, numpyfilelist: list):
"""Generate memmaps from numpy arrays
Parameters
-----------
filepath : Path
path to output memmap folder
numpyfilelist : list
list of paths to numpy files
Returns
--------
Path, numpy.ndarray
memmap_path and memmap_handle
"""
image = np.load(str(numpyfilelist[0]))
list_shape = [len(numpyfilelist)]
list_shape = list_shape + list(image.shape)
filename_map = "memmap_" + str(uuid.uuid4()) + ".map"
memmap_path = Path(filepath) / filename_map
memmap_handle = np.memmap(
filename=memmap_path,
mode="w+",
shape=tuple(list_shape),
dtype=np.float32,
)
Console.info("Loading memmaps from numpy files...")
def memmap_loader(numpyfilelist, memmap_handle, idx):
memmap_handle[idx, ...] = np.load(numpyfilelist[idx])
with tqdm_joblib(
tqdm(desc="numpy images to memmap", total=len(numpyfilelist))):
joblib.Parallel(n_jobs=-2, verbose=0)(
joblib.delayed(memmap_loader)(numpyfilelist, memmap_handle, idx)
for idx in range(len(numpyfilelist)))
return memmap_path, memmap_handle
def write_sidescan_csv(csv_filepath, data_list, csv_filename, csv_flag):
if csv_flag:
csv_file = Path(csv_filepath)
if not csv_file.exists():
csv_file.mkdir(parents=True, exist_ok=True)
Console.info("Writing SSS outputs to {}.txt ...".format(csv_filename))
file = csv_file / "{}.txt".format(csv_filename)
if len(data_list) > 0:
str_to_write = data_list[0].get_sidescan_header()
with file.open("w") as fileout:
fileout.write(str_to_write)
for i in range(len(data_list)):
try:
str_to_write = data_list[i].to_sidescan_row()
fileout.write(str_to_write)
except IndexError:
break
Console.info(
"... done writing SSS outputs to {}.txt.".format(csv_filename))
else:
Console.warn("Empty data list {}".format(str(csv_filename)))
def get_altitude_and_depth_maps(self):
"""Generate distance matrix numpy files and save them"""
# read altitude / depth map depending on distance_metric
if self.distance_metric == "uniform":
Console.info("Null distance matrix created")
self.depth_map_list = []
return
elif self.distance_metric == "altitude":
Console.info("Null distance matrix created")
self.depth_map_list = []
return
elif self.distance_metric == "depth_map":
path_depth = self.path_processed / "depth_map"
if not path_depth.exists():
Console.quit("Depth maps not found...")
else:
Console.info("Path to depth maps found...")
depth_map_list = list(path_depth.glob("*.npy"))
self.depth_map_list = [
Path(item)
for item in depth_map_list
for image_path in self.camera_image_list
if Path(image_path).stem in Path(item).stem
]
if len(self.camera_image_list) != len(self.depth_map_list):
Console.quit(
"The number of images does not coincide with the ",
"number of depth maps.",
)
def plane_fitting_ransac(
cloud_xyz,
min_distance_threshold,
sample_size,
goal_inliers,
max_iterations,
plot=False,
):
model, inliers, iterations = run_ransac(
cloud_xyz,
fit_plane,
lambda x, y: is_inlier(x, y, min_distance_threshold),
sample_size,
goal_inliers,
max_iterations,
)
a, b, c, d = model
if plot:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure()
ax = Axes3D(fig)
# min_x, max_x, min_y, max_y = bounding_box(cloud_xyz[:, 0:2])
xx, yy, zz = plot_plane(a, b, c, d) # , min_x, max_x, min_y, max_y)
ax.plot_surface(xx, yy, zz, color=(0, 1, 0, 0.5))
ax.scatter3D(cloud_xyz.T[0], cloud_xyz.T[1], cloud_xyz.T[2], s=1)
ax.set_xlabel("$X$")
ax.set_ylabel("$Y$")
ax.set_zlabel("$Z$")
plt.show()
Console.info(
"RANSAC took",
iterations + 1,
" iterations. Best model:",
model,
"explains:",
len(inliers),
)
return (a, b, c, d), inliers
def __init__(self, filepath, force_overwite=False, overwrite_all=False):
filepath = Path(filepath).resolve()
self.filepath = get_raw_folder(filepath)
self.fo = force_overwite
self.foa = overwrite_all
if self.foa:
self.fo = True
self.configuration_path = (get_config_folder(self.filepath.parent) /
"calibration")
calibration_config_file = self.configuration_path / "calibration.yaml"
if calibration_config_file.exists():
Console.info("Loading existing calibration.yaml at {}".format(
calibration_config_file))
else:
root = Path(__file__).parents[1]
default_file = root / "auv_cal/default_yaml" / "default_calibration.yaml"
Console.warn("Cannot find {}, generating default from {}".format(
calibration_config_file, default_file))
# save localisation yaml to processed directory
default_file.copy(calibration_config_file)
Console.warn("Edit the file at: \n\t" +
str(calibration_config_file))
Console.warn(
"Try to use relative paths to the calibration datasets")
Console.quit(
"Modify the file calibration.yaml and run this code again.")
with calibration_config_file.open("r") as stream:
self.calibration_config = yaml.safe_load(stream)
# Create the calibration folder at the same level as the dives
self.output_path = get_processed_folder(
self.filepath.parent) / "calibration"
if not self.output_path.exists():
self.output_path.mkdir(parents=True)
if not self.configuration_path.exists():
self.configuration_path.mkdir(parents=True)
def call_process(args):
"""Perform processing on source images using correction parameters
generated in parse and outputs corrected images
Parameters
-----------
args : parse_args object
User provided arguments for path of source images
"""
path = Path(args.path).resolve()
time_string = time.strftime("%Y%m%d_%H%M%S", time.localtime())
Console.set_logging_file(
get_processed_folder(path) /
("log/" + time_string + "_correct_images_process.log"))
correct_config, camerasystem = load_configuration_and_camera_system(
path, args.suffix)
for camera in camerasystem.cameras:
Console.info("Processing for camera", camera.name)
if len(camera.image_list) == 0:
Console.info(
"No images found for the camera at the path provided...")
continue
else:
corrector = Corrector(args.force, args.suffix, camera,
correct_config, path)
if corrector.camera_found:
corrector.process()
Console.info("Process completed for all cameras...")
def rescale_images(
imagenames_list,
image_directory,
interpolate_method,
target_pixel_size_m,
dataframe,
output_directory,
f_x,
f_y,
maintain_pixels,
):
Console.info("Rescaling images...")
for idx in trange(len(imagenames_list)):
image_name = imagenames_list[idx]
source_image_path = Path(image_directory) / image_name
output_image_path = Path(output_directory) / image_name
image_path_list = dataframe["relative_path"]
trimmed_path_list = [
path for path in image_path_list if Path(path).stem in image_name
]
trimmed_dataframe = dataframe.loc[dataframe["relative_path"].isin(
trimmed_path_list)]
altitude = trimmed_dataframe["altitude [m]"]
if len(altitude) > 0:
image = imageio.imread(source_image_path).astype("uint8")
rescaled_image = rescale(
image,
interpolate_method,
target_pixel_size_m,
altitude,
f_x,
f_y,
maintain_pixels,
)
imageio.imwrite(output_image_path, rescaled_image, format="PNG-FI")
else:
Console.warn("Did not get distance values for image: " +
image_name)
def mono(self):
for c in self.calibration_config["cameras"]:
cam_name = c["name"]
# Find if the calibration file exists
calibration_file = self.output_path / str("mono_" + cam_name +
".yaml")
Console.info("Looking for a calibration file at " +
str(calibration_file))
if calibration_file.exists() and not self.fo:
Console.warn(
"The camera " + c["name"] +
" has already been calibrated. If you want to overwrite " +
"the JSON, use the -F flag.")
else:
Console.info(
"The camera is not calibrated, running mono calibration..."
)
filepaths = build_filepath(
get_processed_folder(self.filepath),
c["camera_calibration"]["path"],
)
if "glob_pattern" not in c["camera_calibration"]:
Console.error(
"Could not find the key glob_pattern for the camera ",
c["name"],
)
Console.quit("glob_pattern expected in calibration.yaml")
calibrate_mono(
cam_name,
filepaths,
str(c["camera_calibration"]["glob_pattern"]),
self.calibration_config["camera_calibration"],
calibration_file,
self.fo,
self.foa,
)
def calibrate_mono(
name,
filepaths,
extension,
config,
output_file,
fo,
foa,
target_width=None,
target_height=None,
):
if not check_dirs_exist(filepaths):
filepaths = get_raw_folders(filepaths)
Console.info("Looking for {} calibration images in {}".format(
extension, str(filepaths)))
image_list = collect_image_files(filepaths, extension)
Console.info("Found " + str(len(image_list)) + " images.")
if len(image_list) < 8:
Console.quit("Too few images. Try to get more.")
mc = MonoCalibrator(
boards=[
ChessboardInfo(config["rows"], config["cols"], config["size"])
],
pattern=check_pattern(config),
invert=config["invert"],
name=name,
target_width=target_width,
target_height=target_height,
)
try:
image_list_file = output_file.with_suffix(".json")
if foa or not image_list_file.exists():
mc.cal(image_list)
with image_list_file.open("w") as f:
Console.info("Writing JSON to "
"'{}'"
"".format(image_list_file))
json.dump(mc.json, f)
else:
mc.cal_from_json(image_list_file, image_list)
mc.report()
Console.info("Writing calibration to " "'{}'" "".format(output_file))
with output_file.open("w") as f:
f.write(mc.yaml())
except CalibrationException:
Console.error("The calibration pattern was not found in the images.")
def parse(self, path_list, correct_config_list):
# both path_list and correct_config_list are assumed to be valid + equivalent
for i in range(len(path_list)): # for each dive
path = path_list[i]
correct_config = correct_config_list[i]
Console.info("Parsing dive:", path)
# Console.info("Setting path...")
self.set_path(path) # update the dive path
# Console.info("Loading configuration...")
self.load_configuration(correct_config) # load the dive config
# Set the user specified list - if any
self.user_specified_image_list = self.user_specified_image_list_parse
# Update list of images by appending user-defined list
# TODO: this list must be populated from AFTER loading the configuration and BEFORE getting image list
self.get_imagelist()
# Show the total number of images after filtering + merging the dives. It should match the sum of the filtered images of each dive.
if len(path_list) > 1:
Console.info(
"Total number of images after merging dives:",
len(self.camera_image_list),
)
Console.info("Output directories created / existing...")
if self.correction_method == "colour_correction":
self.get_altitude_and_depth_maps()
self.generate_attenuation_correction_parameters()
for i in range(len(path_list)): # for each dive
path = get_processed_folder(path_list[i])
attn_dir = Path(self.attenuation_parameters_folder)
relative_folder = attn_dir.relative_to(self.path_processed)
dest_dir = path / relative_folder
if dest_dir == attn_dir:
# Do not copy over the original files
continue
copy_file_if_exists(self.attenuation_params_filepath, dest_dir)
copy_file_if_exists(self.correction_gains_filepath, dest_dir)
copy_file_if_exists(self.corrected_mean_filepath, dest_dir)
copy_file_if_exists(self.corrected_std_filepath, dest_dir)
copy_file_if_exists(self.raw_mean_filepath, dest_dir)
copy_file_if_exists(self.raw_std_filepath, dest_dir)
elif self.correction_method == "manual_balance":
Console.info("run process for manual_balance...")
def process_correction(self):
"""Execute series of corrections for a set of input images"""
# check for calibration file if distortion correction needed
if self.undistort:
camera_params_folder = Path(self.path_processed).parents[0] / "calibration"
camera_params_filename = "mono" + self.camera_name + ".yaml"
camera_params_file_path = camera_params_folder / camera_params_filename
if not camera_params_file_path.exists():
Console.info("Calibration file not found...")
self.undistort = False
else:
Console.info("Calibration file found...")
self.camera_params_file_path = camera_params_file_path
Console.info(
"Processing",
len(self.camera_image_list),
"images for color, distortion, gamma corrections...",
)
with tqdm_joblib(
tqdm(desc="Correcting images", total=len(self.camera_image_list))
):
self.processed_image_list = joblib.Parallel(n_jobs=-2, verbose=0)(
joblib.delayed(self.process_image)(idx)
for idx in range(0, len(self.camera_image_list))
)
# write a filelist.csv containing image filenames which are processed
image_files = []
for path in self.processed_image_list:
if path is not None:
image_files.append(Path(path).name)
dataframe = pd.DataFrame(image_files)
filelist_path = self.output_images_folder / "filelist.csv"
dataframe.to_csv(filelist_path)
Console.info("Processing of images is completed...")
def interpolate_sensor_list(
sensor_list,
sensor_name,
sensor_offsets,
origin_offsets,
latlon_reference,
_centre_list,
):
j = 0
[origin_x_offset, origin_y_offset, origin_z_offset] = origin_offsets
[latitude_reference, longitude_reference] = latlon_reference
# Check if camera activates before dvl and orientation sensors.
start_time = _centre_list[0].epoch_timestamp
end_time = _centre_list[-1].epoch_timestamp
if (sensor_list[0].epoch_timestamp > end_time
or sensor_list[-1].epoch_timestamp < start_time):
Console.warn(
"{} timestamps does not overlap with dead reckoning data, "
"check timestamp_history.pdf via -v option.".format(sensor_name))
else:
sensor_overlap_flag = 0
for i in range(len(sensor_list)):
if sensor_list[i].epoch_timestamp < start_time:
sensor_overlap_flag = 1
pass
else:
if i > 0:
Console.warn(
"Deleted",
i,
"entries from sensor",
sensor_name,
". Reason: data before start of mission",
)
del sensor_list[:i]
break
for i in range(len(sensor_list)):
if j >= len(_centre_list) - 1:
ii = len(sensor_list) - i
if ii > 0:
Console.warn(
"Deleted",
ii,
"entries from sensor",
sensor_name,
". Reason: data after end of mission",
)
del sensor_list[i:]
sensor_overlap_flag = 1
break
while _centre_list[j].epoch_timestamp < sensor_list[
i].epoch_timestamp:
if (j + 1 > len(_centre_list) - 1
or _centre_list[j + 1].epoch_timestamp >
sensor_list[-1].epoch_timestamp):
break
j += 1
# if j>=1: ?
sensor_list[i].roll = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].roll,
_centre_list[j].roll,
)
sensor_list[i].pitch = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].pitch,
_centre_list[j].pitch,
)
if abs(_centre_list[j].yaw - _centre_list[j - 1].yaw) > 180:
if _centre_list[j].yaw > _centre_list[j - 1].yaw:
sensor_list[i].yaw = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].yaw,
_centre_list[j].yaw - 360,
)
else:
sensor_list[i].yaw = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].yaw - 360,
_centre_list[j].yaw,
)
if sensor_list[i].yaw < 0:
sensor_list[i].yaw += 360
elif sensor_list[i].yaw > 360:
sensor_list[i].yaw -= 360
else:
sensor_list[i].yaw = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].yaw,
_centre_list[j].yaw,
)
sensor_list[i].x_velocity = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].x_velocity,
_centre_list[j].x_velocity,
)
sensor_list[i].y_velocity = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].y_velocity,
_centre_list[j].y_velocity,
)
sensor_list[i].z_velocity = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].z_velocity,
_centre_list[j].z_velocity,
)
sensor_list[i].altitude = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].altitude,
_centre_list[j].altitude,
)
[x_offset, y_offset, z_offset] = body_to_inertial(
sensor_list[i].roll,
sensor_list[i].pitch,
sensor_list[i].yaw,
origin_x_offset - sensor_offsets[0],
origin_y_offset - sensor_offsets[1],
origin_z_offset - sensor_offsets[2],
)
sensor_list[i].northings = (interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].northings,
_centre_list[j].northings,
) - x_offset)
sensor_list[i].eastings = (interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].eastings,
_centre_list[j].eastings,
) - y_offset)
sensor_list[i].altitude = (interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].altitude,
_centre_list[j].altitude,
) - z_offset)
sensor_list[i].depth = (interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].depth,
_centre_list[j].depth,
) - z_offset)
[sensor_list[i].latitude,
sensor_list[i].longitude] = metres_to_latlon(
latitude_reference,
longitude_reference,
sensor_list[i].eastings,
sensor_list[i].northings,
)
sensor_list[i].northings_std = interpolate_property(
_centre_list, i, sensor_list, j, "northings_std")
sensor_list[i].eastings_std = interpolate_property(
_centre_list, i, sensor_list, j, "eastings_std")
sensor_list[i].depth_std = interpolate_property(
_centre_list, i, sensor_list, j, "depth_std")
sensor_list[i].roll_std = interpolate_property(
_centre_list, i, sensor_list, j, "roll_std")
sensor_list[i].pitch_std = interpolate_property(
_centre_list, i, sensor_list, j, "pitch_std")
sensor_list[i].yaw_std = interpolate_property(
_centre_list, i, sensor_list, j, "yaw_std")
sensor_list[i].x_velocity_std = interpolate_property(
_centre_list, i, sensor_list, j, "x_velocity_std")
sensor_list[i].y_velocity_std = interpolate_property(
_centre_list, i, sensor_list, j, "y_velocity_std")
sensor_list[i].z_velocity_std = interpolate_property(
_centre_list, i, sensor_list, j, "z_velocity_std")
sensor_list[i].vroll_std = interpolate_property(
_centre_list, i, sensor_list, j, "vroll_std")
sensor_list[i].vpitch_std = interpolate_property(
_centre_list, i, sensor_list, j, "vpitch_std")
sensor_list[i].vyaw_std = interpolate_property(
_centre_list, i, sensor_list, j, "vyaw_std")
if _centre_list[j].covariance is not None:
sensor_list[i].covariance = interpolate_covariance(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].covariance,
_centre_list[j].covariance,
)
if sensor_overlap_flag == 1:
Console.warn(
"Sensor data from {} spans further than dead reckoning data."
" Data outside DR is ignored.".format(sensor_name))
Console.info("Complete interpolation and coordinate transfomations "
"for {}".format(sensor_name))
def parse_acfr_images(mission, vehicle, category, ftype, outpath):
# parser meta data
class_string = "measurement"
frame_string = "body"
category = "image"
sensor_string = "acfr_standard"
timezone = mission.image.timezone
timeoffset = mission.image.timeoffset
filepath = mission.image.cameras[0].path
camera1_label = mission.image.cameras[0].name
camera2_label = mission.image.cameras[1].name
# read in timezone
if isinstance(timezone, str):
if timezone == "utc" or timezone == "UTC":
timezone_offset = 0
elif timezone == "jst" or timezone == "JST":
timezone_offset = 9
else:
try:
timezone_offset = float(timezone)
except ValueError:
print(
"Error: timezone",
timezone,
"in mission.cfg not recognised, please enter value from UTC",
"in hours",
)
return
# convert to seconds from utc
# timeoffset = -timezone_offset*60*60 + timeoffset
Console.info("... parsing " + sensor_string + " images")
# determine file paths
filepath = get_raw_folder(outpath / ".." / filepath)
all_list = os.listdir(str(filepath))
camera1_filename = [
line for line in all_list
if camera1_label in line and ".txt" not in line and "._" not in line
]
camera2_filename = [
line for line in all_list
if camera2_label in line and ".txt" not in line and "._" not in line
]
data_list = []
if ftype == "acfr":
data_list = ""
for i in range(len(camera1_filename)):
epoch_timestamp = acfr_timestamp_from_filename(camera1_filename[i],
timezone_offset,
timeoffset)
epoch_timestamp_camera1.append(str(epoch_timestamp))
for i in range(len(camera2_filename)):
epoch_timestamp = acfr_timestamp_from_filename(camera2_filename[i],
timezone_offset,
timeoffset)
epoch_timestamp_camera2.append(str(epoch_timestamp))
for i in range(len(camera1_filename)):
# print(epoch_timestamp_camera1[i])
values = []
for j in range(len(camera2_filename)):
# print(epoch_timestamp_camera2[j])
values.append(
abs(
float(epoch_timestamp_camera1[i]) -
float(epoch_timestamp_camera2[j])))
(sync_difference, sync_pair) = min(
(v, k) for k, v in enumerate(values))
if sync_difference > tolerance:
# Skip the pair
continue
if ftype == "oplab":
data = {
"epoch_timestamp":
float(epoch_timestamp_camera1[i]),
"class":
class_string,
"sensor":
sensor_string,
"frame":
frame_string,
"category":
category,
"camera1": [{
"epoch_timestamp":
float(epoch_timestamp_camera1[i]),
"filename":
str(filepath) + "/" + str(camera1_filename[i]),
}],
"camera2": [{
"epoch_timestamp":
float(epoch_timestamp_camera2[sync_pair]),
"filename":
str(filepath) + "/" + str(camera2_filename[sync_pair]),
}],
}
data_list.append(data)
if ftype == "acfr":
data = ("VIS: " + str(float(epoch_timestamp_camera1[i])) + " [" +
str(float(epoch_timestamp_camera1[i])) + "] " +
str(camera1_filename[i]) + " exp: 0\n")
# fileout.write(data)
data_list += data
data = ("VIS: " + str(float(epoch_timestamp_camera2[sync_pair])) +
" [" + str(float(epoch_timestamp_camera2[sync_pair])) +
"] " + str(camera2_filename[sync_pair]) + " exp: 0\n")
# fileout.write(data)
data_list += data
return data_list
def parse_autosub(mission, vehicle, category, ftype, outpath):
# parser meta data
sensor_string = "autosub"
category = category
output_format = ftype
filename = mission.velocity.filename
filepath = mission.velocity.filepath
# autosub std models
depth_std_factor = mission.depth.std_factor
velocity_std_factor = mission.velocity.std_factor
velocity_std_offset = mission.velocity.std_offset
orientation_std_offset = mission.orientation.std_offset
altitude_std_factor = mission.altitude.std_factor
headingoffset = vehicle.dvl.yaw
body_velocity = BodyVelocity(velocity_std_factor, velocity_std_offset,
headingoffset)
orientation = Orientation(headingoffset, orientation_std_offset)
depth = Depth(depth_std_factor)
altitude = Altitude(altitude_std_factor)
body_velocity.sensor_string = sensor_string
orientation.sensor_string = sensor_string
depth.sensor_string = sensor_string
altitude.sensor_string = sensor_string
path = get_raw_folder(outpath / ".." / filepath / filename)
autosub_log = loadmat(str(path))
autosub_adcp = autosub_log["missionData"]["ADCPbin00"]
autosub_ins = autosub_log["missionData"]["INSAttitude"]
autosub_dep_ctl = autosub_log["missionData"]["DepCtlNode"]
autosub_adcp_log1 = autosub_log["missionData"]["ADCPLog_1"]
data_list = []
if category == Category.VELOCITY:
Console.info("... parsing autosub velocity")
previous_timestamp = 0
for i in range(len(autosub_adcp["eTime"])):
body_velocity.from_autosub(autosub_adcp, i)
data = body_velocity.export(output_format)
if body_velocity.epoch_timestamp > previous_timestamp:
data_list.append(data)
else:
data_list[-1] = data
previous_timestamp = body_velocity.epoch_timestamp
if category == Category.ORIENTATION:
Console.info("... parsing autosub orientation")
previous_timestamp = 0
for i in range(len(autosub_ins["eTime"])):
orientation.from_autosub(autosub_ins, i)
data = orientation.export(output_format)
if orientation.epoch_timestamp > previous_timestamp:
data_list.append(data)
else:
data_list[-1] = data
previous_timestamp = orientation.epoch_timestamp
if category == Category.DEPTH:
Console.info("... parsing autosub depth")
previous_timestamp = 0
for i in range(len(autosub_dep_ctl["eTime"])):
depth.from_autosub(autosub_dep_ctl, i)
data = depth.export(output_format)
if depth.epoch_timestamp > previous_timestamp:
data_list.append(data)
else:
data_list[-1] = data
previous_timestamp = depth.epoch_timestamp
if category == Category.ALTITUDE:
Console.info("... parsing autosub altitude")
previous_timestamp = 0
for i in range(len(autosub_adcp_log1["eTime"])):
altitude.from_autosub(autosub_adcp_log1, i)
data = altitude.export(output_format)
if altitude.epoch_timestamp > previous_timestamp:
data_list.append(data)
else:
data_list[-1] = data
previous_timestamp = altitude.epoch_timestamp
return data_list
def parse(filepath, force_overwrite, merge):
# Filepath is a list. Get the first element by default
for p in filepath:
parse_single(p, force_overwrite)
if merge and len(filepath) > 1:
Console.info("Merging the dives...")
# Generate a merged output
dates = []
# Collect json files
json_files = []
for p in filepath:
folder_name = Path(p).name
yyyy = int(folder_name[0:4])
mm = int(folder_name[4:6])
dd = int(folder_name[6:8])
hh = int(folder_name[9:11])
mm1 = int(folder_name[11:13])
ss = int(folder_name[13:15])
d = datetime(yyyy, mm, dd, hh, mm1, ss)
dates.append(d)
outpath = get_processed_folder(p)
nav_file = outpath / "nav/nav_standard.json"
json_files.append(nav_file)
data_list = merge_json_files(json_files)
# Get first and last dates
dates.sort()
foldername = (dates[0].strftime("%Y%m%d%H%M%S") + "_" +
dates[-1].strftime("%Y%m%d%H%M%S") + "_merged")
# Create folder if it does not exist
processed_path = get_processed_folder(filepath[0]).parent
nav_folder = processed_path / foldername / "nav"
# make output path if not exist
if not nav_folder.exists():
try:
nav_folder.mkdir(parents=True, exist_ok=True)
except Exception as e:
print("Warning:", e)
# create file (overwrite if exists)
filename = "nav_standard.json"
nav_file = nav_folder / filename
Console.info("Writing the ouput to:", str(nav_file))
with nav_file.open("w") as fileout:
json.dump(data_list, fileout, indent=2)
fileout.close()
# copy mission.yaml and vehicle.yaml to processed folder for
# process step
mission_processed = get_processed_folder(filepath[0]) / "mission.yaml"
vehicle_processed = get_processed_folder(filepath[0]) / "vehicle.yaml"
mission_merged = processed_path / foldername / "mission.yaml"
vehicle_merged = processed_path / foldername / "vehicle.yaml"
mission_processed.copy(mission_merged)
vehicle_processed.copy(vehicle_merged)
# interlace the data based on timestamps
Console.info("Interlacing merged data...")
parse_interlacer(nav_folder, filename)
Console.info(
"...done interlacing merged data. Output saved to",
nav_folder / filename,
)
plot_parse_data(nav_folder)
Console.info("Complete merging data")
def parse_single(filepath, force_overwrite):
# initiate data and processing flags
filepath = Path(filepath).resolve()
filepath = get_raw_folder(filepath)
if not force_overwrite:
existing_files = check_output_files_exist(
get_processed_folder(filepath))
if existing_files:
msg = ("It looks like this dataset has already been parsed.\n" +
"The following file(s) already exist:\n" + existing_files +
"If you would like auv_nav to overwrite existing file," +
" rerun it with the flag -F.\n" +
"Example: auv_nav parse -F PATH")
Console.warn(msg)
Console.warn("Dataset skipped")
return
ftype = "oplab"
# load mission.yaml config file
mission_file = filepath / "mission.yaml"
vehicle_file = filepath / "vehicle.yaml"
mission_file = get_raw_folder(mission_file)
vehicle_file = get_raw_folder(vehicle_file)
Console.info("Loading mission.yaml at", mission_file)
mission = Mission(mission_file)
Console.info("Loading vehicle.yaml at", vehicle_file)
vehicle = Vehicle(vehicle_file)
# copy mission.yaml and vehicle.yaml to processed folder for process step
mission_processed = get_processed_folder(mission_file)
vehicle_processed = get_processed_folder(vehicle_file)
# Write mission with metadata (username, date and hostname)
mission.write(mission_processed)
# mission_file.copy(mission_processed)
vehicle.write(vehicle_processed)
# check for recognised formats and create nav file
outpath = get_processed_folder(filepath)
outpath = outpath / "nav"
filename = "nav_standard.json"
# make file path if not exist
if not outpath.is_dir():
try:
outpath.mkdir()
except Exception as e:
print("Warning:", e)
Console.info("Loading raw data...")
if multiprocessing.cpu_count() < 4:
cpu_to_use = 1
else:
cpu_to_use = multiprocessing.cpu_count() - 2
try:
pool = multiprocessing.Pool(cpu_to_use)
except AttributeError as e:
print(
"Error: ",
e,
"\n===============\nThis error is known to \
happen when running the code more than once from the same \
console in Spyder. Please run the code from a new console \
to prevent this error from happening. You may close the \
current console.\n==============",
)
pool_list = []
# read in, parse data and write data
if not mission.image.empty():
if mission.image.format == "acfr_standard" or mission.image.format == "unagi":
pool_list.append(
pool.apply_async(
parse_acfr_images,
[mission, vehicle, "images", ftype, outpath],
))
elif mission.image.format == "seaxerocks_3":
pool_list.append(
pool.apply_async(
parse_seaxerocks_images,
[mission, vehicle, "images", ftype, outpath],
))
elif (mission.image.format == "biocam"
or mission.image.format == "biocam4000_15c"):
pool_list.append(
pool.apply_async(
parse_biocam_images,
[mission, vehicle, "images", ftype, outpath],
))
elif mission.image.format == "ntnu_stereo":
pool_list.append(
pool.apply_async(
parse_ntnu_stereo_images,
[mission, vehicle, "images", ftype, outpath],
))
elif mission.image.format == "rosbag_extracted_images":
pool_list.append(
pool.apply_async(
parse_rosbag_extracted_images,
[mission, vehicle, "images", ftype, outpath],
))
elif mission.image.format == "rosbag":
pool_list.append(
pool.apply_async(
parse_rosbag,
[mission, vehicle, "images", ftype, outpath],
))
else:
Console.quit("Mission image format", mission.image.format,
"not supported.")
if not mission.usbl.empty():
if mission.usbl.format == "gaps":
pool_list.append(
pool.apply_async(parse_gaps,
[mission, vehicle, "usbl", ftype, outpath]))
elif mission.usbl.format == "usbl_dump":
pool_list.append(
pool.apply_async(parse_usbl_dump,
[mission, vehicle, "usbl", ftype, outpath]))
elif mission.usbl.format == "NOC_nmea":
pool_list.append(
pool.apply_async(parse_NOC_nmea,
[mission, vehicle, "usbl", ftype, outpath]))
elif mission.usbl.format == "eiva_navipac":
pool_list.append(
pool.apply_async(
parse_eiva_navipac,
[mission, vehicle, "usbl", ftype, outpath],
))
elif mission.usbl.format == "rosbag":
pool_list.append(
pool.apply_async(
parse_rosbag,
[mission, vehicle, "usbl", ftype, outpath],
))
else:
Console.quit("Mission usbl format", mission.usbl.format,
"not supported.")
if not mission.velocity.empty():
if mission.velocity.format == "phins":
pool_list.append(
pool.apply_async(
parse_phins,
[mission, vehicle, "velocity", ftype, outpath]))
elif mission.velocity.format == "ae2000":
pool_list.append(
pool.apply_async(
parse_ae2000,
[mission, vehicle, "velocity", ftype, outpath],
))
elif mission.velocity.format == "alr":
pool_list.append(
pool.apply_async(
parse_alr,
[mission, vehicle, "velocity", ftype, outpath],
))
elif mission.velocity.format == "autosub":
pool_list.append(
pool.apply_async(
parse_autosub,
[mission, vehicle, "velocity", ftype, outpath],
))
elif mission.velocity.format == "rdi":
pool_list.append(
pool.apply_async(
parse_rdi, [mission, vehicle, "velocity", ftype, outpath]))
elif mission.velocity.format == "ntnu_dvl":
pool_list.append(
pool.apply_async(
parse_ntnu_dvl,
[mission, vehicle, "velocity", ftype, outpath],
))
elif mission.usbl.format == "rosbag":
pool_list.append(
pool.apply_async(
parse_rosbag,
[mission, vehicle, "velocity", ftype, outpath],
))
else:
Console.quit(
"Mission velocity format",
mission.velocity.format,
"not supported.",
)
if not mission.orientation.empty():
if mission.orientation.format == "phins":
pool_list.append(
pool.apply_async(
parse_phins,
[mission, vehicle, "orientation", ftype, outpath],
))
elif mission.orientation.format == "ae2000":
pool_list.append(
pool.apply_async(
parse_ae2000,
[mission, vehicle, "orientation", ftype, outpath],
))
elif mission.orientation.format == "alr":
pool_list.append(
pool.apply_async(
parse_alr,
[mission, vehicle, "orientation", ftype, outpath],
))
elif mission.orientation.format == "autosub":
pool_list.append(
pool.apply_async(
parse_autosub,
[mission, vehicle, "orientation", ftype, outpath],
))
elif mission.orientation.format == "rdi":
pool_list.append(
pool.apply_async(
parse_rdi,
[mission, vehicle, "orientation", ftype, outpath],
))
elif mission.orientation.format == "eiva_navipac":
pool_list.append(
pool.apply_async(
parse_eiva_navipac,
[mission, vehicle, "orientation", ftype, outpath],
))
elif mission.usbl.format == "rosbag":
pool_list.append(
pool.apply_async(
parse_rosbag,
[mission, vehicle, "orientation", ftype, outpath],
))
else:
Console.quit(
"Mission orientation format",
mission.orientation.format,
"not supported.",
)
if not mission.depth.empty():
if mission.depth.format == "phins":
pool_list.append(
pool.apply_async(parse_phins,
[mission, vehicle, "depth", ftype, outpath]))
elif mission.depth.format == "ae2000":
pool_list.append(
pool.apply_async(parse_ae2000,
[mission, vehicle, "depth", ftype, outpath]))
elif mission.depth.format == "alr":
pool_list.append(
pool.apply_async(parse_alr,
[mission, vehicle, "depth", ftype, outpath]))
elif mission.depth.format == "autosub":
pool_list.append(
pool.apply_async(parse_autosub,
[mission, vehicle, "depth", ftype, outpath]))
elif mission.depth.format == "gaps":
pool_list.append(
pool.apply_async(parse_gaps,
[mission, vehicle, "depth", ftype, outpath]))
elif mission.depth.format == "eiva_navipac":
pool_list.append(
pool.apply_async(
parse_eiva_navipac,
[mission, vehicle, "depth", ftype, outpath],
))
elif mission.usbl.format == "rosbag":
pool_list.append(
pool.apply_async(
parse_rosbag,
[mission, vehicle, "depth", ftype, outpath],
))
else:
Console.quit("Mission depth format", mission.depth.format,
"not supported.")
if not mission.altitude.empty():
if mission.altitude.format == "phins":
pool_list.append(
pool.apply_async(
parse_phins,
[mission, vehicle, "altitude", ftype, outpath]))
elif mission.altitude.format == "ae2000":
pool_list.append(
pool.apply_async(
parse_ae2000,
[mission, vehicle, "altitude", ftype, outpath],
))
elif mission.altitude.format == "alr":
pool_list.append(
pool.apply_async(
parse_alr,
[mission, vehicle, "altitude", ftype, outpath],
))
elif mission.altitude.format == "autosub":
pool_list.append(
pool.apply_async(
parse_autosub,
[mission, vehicle, "altitude", ftype, outpath],
))
elif mission.altitude.format == "rdi":
pool_list.append(
pool.apply_async(
parse_rdi, [mission, vehicle, "altitude", ftype, outpath]))
elif mission.altitude.format == "ntnu_dvl":
pool_list.append(
pool.apply_async(
parse_ntnu_dvl,
[mission, vehicle, "altitude", ftype, outpath],
))
elif mission.usbl.format == "rosbag":
pool_list.append(
pool.apply_async(
parse_rosbag,
[mission, vehicle, "altitude", ftype, outpath],
))
else:
Console.quit(
"Mission altitude format",
mission.altitude.format,
"not supported.",
)
if not mission.tide.empty():
if mission.tide.format == "NOC_polpred":
tide_list = parse_NOC_polpred(mission, vehicle, "tide", ftype,
outpath)
else:
Console.quit("Mission tide format", mission.tide.format,
"not supported.")
else:
tide_list = None
pool.close()
pool.join()
Console.info("...done loading raw data.")
Console.info("Compile data list...")
data_list = [[{
"epoch_timestamp":
0.0,
"class":
"origin",
"category":
"origin",
"data": [{
"latitude": mission.origin.latitude,
"longitude": mission.origin.longitude,
"crs": mission.origin.crs,
"date": mission.origin.date,
}],
}]]
# Set advance True/False flag so not comparing for every i in pool_list
if mission.image.format == "biocam":
correcting_timestamps = True
else:
correcting_timestamps = False
for i in pool_list:
results = i.get()
# If current retrieved data is DEPTH
# and if TIDE data is available
if len(results) < 1:
continue
if results[0] is None:
Console.warn(
"Some results are empty. Please check whether this is correct or not"
)
continue
if correcting_timestamps:
if results[0]["category"] == "image":
Console.info("Correction of BioCam cpu timestamps...")
results = correct_timestamps(results)
if (results[0]["category"] == Category.DEPTH
or results[0]["category"] == Category.USBL):
if not mission.tide.empty():
# proceed to tidal correction
Console.info("Tidal correction of depth vector...")
# Offset depth to acknowledge for tides
j = 0
for k in range(len(results)):
while (j < len(tide_list)
and tide_list[j]["epoch_timestamp"] <
results[k]["epoch_timestamp"]):
j = j + 1
if j >= 1:
_result = interpolate(
results[k]["epoch_timestamp"],
tide_list[j - 1]["epoch_timestamp"],
tide_list[j]["epoch_timestamp"],
tide_list[j - 1]["data"][0]["height"],
tide_list[j]["data"][0]["height"],
)
if results[0]["category"] == Category.DEPTH:
results[k]["data"][0]["depth"] = (
results[k]["data"][0]["depth"] - _result)
elif results[0]["category"] == Category.USBL:
results[k]["data_target"][4]["depth"] = (
results[k]["data_target"][4]["depth"] -
_result)
data_list.append(results)
Console.info("...done compiling data list.")
Console.info("Writing to output file...")
data_list_temp = []
for i in data_list:
data_list_temp += i
# create file (overwrite if exists)
nav_file = outpath / filename
with nav_file.open("w") as fileout:
json.dump(data_list_temp, fileout, indent=2)
fileout.close()
Console.info("...done writing to output file.")
del data_list_temp
del data_list
# interlace the data based on timestamps
Console.info("Interlacing data...")
parse_interlacer(outpath, filename)
Console.info("...done interlacing data. Output saved to",
outpath / filename)
plot_parse_data(outpath, ftype)
Console.info("Complete parse data")
def parse_seaxerocks_images(mission, vehicle, category, ftype, outpath):
data_list = []
if ftype == "acfr":
data_list = ""
# parser meta data
class_string = "measurement"
frame_string = "body"
category_stereo = "image"
category_laser = "laser"
sensor_string = "seaxerocks_3"
timezone = mission.image.timezone
timeoffset = mission.image.timeoffset
camera1_filepath = mission.image.cameras[0].path
camera2_filepath = mission.image.cameras[1].path
camera3_filepath = mission.image.cameras[2].path
camera1_label = mission.image.cameras[0].name
camera2_label = mission.image.cameras[1].name
camera3_label = mission.image.cameras[2].name
epoch_timestamp_stereo = []
epoch_timestamp_laser = []
epoch_timestamp_camera1 = []
epoch_timestamp_camera2 = []
epoch_timestamp_camera3 = []
stereo_index = []
laser_index = []
camera1_index = []
camera2_index = []
camera3_index = []
camera1_filename = []
camera2_filename = []
camera3_filename = []
camera1_serial = list(camera1_label)
camera2_serial = list(camera2_label)
camera3_serial = list(camera3_label)
for i in range(1, len(camera1_label)):
if camera1_label[i] == "/":
camera1_serial[i] = "_"
for i in range(1, len(camera2_label)):
if camera2_label[i] == "/":
camera2_serial[i] = "_"
for i in range(1, len(camera3_label)):
if camera3_label[i] == "/":
camera3_serial[i] = "_"
camera1_serial = "".join(camera1_serial)
camera2_serial = "".join(camera2_serial)
camera3_serial = "".join(camera3_serial)
i = 0
# read in timezone
# TODO change ALL timezones to integers
if isinstance(timezone, str):
if timezone == "utc" or timezone == "UTC":
timezone_offset = 0
elif timezone == "jst" or timezone == "JST":
timezone_offset = 9
else:
try:
timezone_offset = float(timezone)
except ValueError:
print(
"Error: timezone",
timezone,
"in mission.cfg not recognised, \
please enter value from UTC in hours",
)
return
# convert to seconds from utc
# timeoffset = -timezone_offset*60*60 + timeoffset
Console.info(" Parsing " + sensor_string + " images...")
cam1_path = get_raw_folder(outpath / ".." / camera1_filepath / "..")
cam1_filetime = cam1_path / "FileTime.csv"
with cam1_filetime.open("r", encoding="utf-8", errors="ignore") as filein:
for line in filein.readlines():
stereo_index_timestamps = line.strip().split(",")
index_string = stereo_index_timestamps[0]
date_string = stereo_index_timestamps[1]
time_string = stereo_index_timestamps[2]
ms_time_string = stereo_index_timestamps[3]
# read in date
if date_string != "date": # ignore header
stereo_index.append(index_string)
if len(date_string) != 8:
Console.warn(
"Date string ({}) in FileTime.csv file has "
"unexpected length. Expected length: 8.".format(
date_string))
yyyy = int(date_string[0:4])
mm = int(date_string[4:6])
dd = int(date_string[6:8])
# read in time
if len(time_string) != 6:
Console.warn(
"Time string ({}) in FileTime.csv file has "
"unexpected length. Expected length: 6.".format(
time_string))
hour = int(time_string[0:2])
mins = int(time_string[2:4])
secs = int(time_string[4:6])
msec = int(ms_time_string[0:3])
epoch_time = date_time_to_epoch(yyyy, mm, dd, hour, mins, secs,
timezone_offset)
epoch_timestamp_stereo.append(
float(epoch_time + msec / 1000 + timeoffset))
camera1_list = ["{}.raw".format(i) for i in stereo_index]
camera2_list = ["{}.raw".format(i) for i in stereo_index]
for i in range(len(camera1_list)):
camera1_image = camera1_list[i].split(".")
camera2_image = camera2_list[i].split(".")
camera1_index.append(camera1_image[0])
camera2_index.append(camera2_image[0])
j = 0
for i in range(len(camera1_list)):
# find corresponding timestamp even if some images are deletec
if camera1_index[i] == stereo_index[j]:
epoch_timestamp_camera1.append(epoch_timestamp_stereo[j])
epoch_timestamp_camera2.append(epoch_timestamp_stereo[j])
date = epoch_to_day(epoch_timestamp_stereo[0])
if ftype == "acfr":
camera1_filename.append("sx3_" + date[2:4] + date[5:7] +
date[8:10] + "_image" +
str(camera1_index[i]) + "_FC.png")
camera2_filename.append("sx3_" + date[2:4] + date[5:7] +
date[8:10] + "_image" +
str(camera2_index[i]) + "_AC.png")
j = j + 1
elif stereo_index[j] > camera1_index[i]:
j = j + 1
else:
j = j - 1
if ftype == "oplab":
camera1_filename = [line for line in camera1_list]
camera2_filename = [line for line in camera2_list]
for i in range(len(camera1_list)):
if ftype == "acfr":
data = ("VIS: " + str(float(epoch_timestamp_camera1[i])) + " [" +
str(float(epoch_timestamp_camera1[i])) + "] " +
str(camera1_filename[i]) + " exp: 0\n")
data_list += data
data = ("VIS: " + str(float(epoch_timestamp_camera2[i])) + " [" +
str(float(epoch_timestamp_camera2[i])) + "] " +
str(camera2_filename[i]) + " exp: 0\n")
data_list += data
if ftype == "oplab":
data = {
"epoch_timestamp":
float(epoch_timestamp_camera1[i]),
"class":
class_string,
"sensor":
sensor_string,
"frame":
frame_string,
"category":
category_stereo,
"camera1": [{
"epoch_timestamp":
float(epoch_timestamp_camera1[i]),
"serial":
camera1_serial,
"filename":
str(camera1_filepath + "/" + camera1_filename[i]),
}],
"camera2": [{
"epoch_timestamp":
float(epoch_timestamp_camera2[i]),
"serial":
camera2_serial,
"filename":
str(camera2_filepath + "/" + camera2_filename[i]),
}],
}
data_list.append(data)
cam3_path = get_raw_folder(outpath / ".." / camera3_filepath)
cam3_filetime = cam3_path / "FileTime.csv"
with cam3_filetime.open("r", encoding="utf-8", errors="ignore") as filein:
for line in filein.readlines():
laser_index_timestamps = line.strip().split(",")
if len(laser_index_timestamps) < 4:
Console.warn("The laser FileTime.csv is apparently corrupt...")
continue
index_string = laser_index_timestamps[0]
date_string = laser_index_timestamps[1]
time_string = laser_index_timestamps[2]
ms_time_string = laser_index_timestamps[3]
# read in date
if date_string != "date": # ignore header
laser_index.append(index_string)
yyyy = int(date_string[0:4])
mm = int(date_string[4:6])
dd = int(date_string[6:8])
# read in time
hour = int(time_string[0:2])
mins = int(time_string[2:4])
secs = int(time_string[4:6])
msec = int(ms_time_string[0:3])
epoch_time = date_time_to_epoch(yyyy, mm, dd, hour, mins, secs,
timezone_offset)
epoch_timestamp_laser.append(
float(epoch_time + msec / 1000 + timeoffset))
# try use pandas for all parsers, should be faster
camera3_list = ["{}".format(i) for i in laser_index]
# The LM165 images are saved either as jpg or as tif, and are split into
# subfolders either at every 1000 or every 10000 images. Find out which
# convention is used in current dataset by looking at the files.
if len(camera3_list) > 0:
s, extension = determine_extension_and_images_per_folder(
cam3_path, camera3_list, camera3_label)
for i in range(len(camera3_list)):
camera3_filename.append("{}/image{}.{}".format(
camera3_list[i][s:(s + 3)],
camera3_list[i],
extension, # noqa: E203
))
camera3_index.append(camera3_list[i])
j = 0
# original comment: find corresponding timestamp even if some images are
# deleted
for i in range(len(camera3_filename)):
if camera3_index[i] == laser_index[j]:
epoch_timestamp_camera3.append(epoch_timestamp_laser[j])
j = j + 1
# Jin: incomplete? it means that laser data is missing for this image
# file, so no epoch_timestamp data, and do what when this happens?
elif laser_index[j] > camera3_index[i]:
j = j + 1
else:
# Jin: incomplete and possibly wrong? it means that this laser
# data is extra, with no accompanying image file, so it should be
# j+1 till index match?
j = j - 1
if ftype == "oplab":
data = {
"epoch_timestamp": float(epoch_timestamp_camera3[i]),
"class": class_string,
"sensor": sensor_string,
"frame": frame_string,
"category": category_laser,
"serial": camera3_serial,
"filename": camera3_filepath + "/" + str(camera3_filename[i]),
}
data_list.append(data)
Console.info(" ...done parsing " + sensor_string + " images.")
return data_list
def acfr_to_oplab(args):
csv_filepath = Path(args.output_folder)
force_overwite = args.force
if not args.vehicle_pose and not args.stereo_pose:
Console.error("Please provide at least one file")
Console.quit("Missing comandline arguments")
if args.vehicle_pose:
Console.info("Vehicle pose provided! Converting it to DR CSV...")
vpp = AcfrVehiclePoseParser(args.vehicle_pose)
dr = vpp.get_dead_reckoning()
csv_filename = "auv_acfr_centre.csv"
if (csv_filepath / csv_filename).exists() and not force_overwite:
Console.error(
"The DR file already exists. Use -F to force overwrite it")
Console.quit("Default behaviour: not to overwrite results")
else:
write_csv(csv_filepath, dr, csv_filename, csv_flag=True)
Console.info("Output vehicle pose written to",
csv_filepath / csv_filename)
if args.stereo_pose:
Console.info("Stereo pose provided! Converting it to camera CSVs...")
spp = AcfrStereoPoseParser(args.stereo_pose)
cam1, cam2 = spp.get_cameras()
if (csv_filepath / "auv_dr_LC.csv").exists() and not force_overwite:
Console.error(
"The camera files already exists. Use -F to force overwrite it"
)
Console.quit("Default behaviour: not to overwrite results")
else:
write_csv(csv_filepath, cam1, "auv_acfr_LC", csv_flag=True)
write_csv(csv_filepath, cam2, "auv_acfr_RC", csv_flag=True)
Console.info("Output camera files written at", csv_filepath)
if args.dive_folder is None:
return
# If the user provides a dive path, we can interpolate the ACFR navigation
# to the laser timestamps
path_processed = get_processed_folder(args.dive_folder)
nav_standard_file = path_processed / "nav" / "nav_standard.json"
nav_standard_file = get_processed_folder(nav_standard_file)
Console.info("Loading json file {}".format(nav_standard_file))
start_datetime = ""
finish_datetime = ""
file3 = csv_filepath / "auv_acfr_laser.csv"
if file3.exists() and not force_overwite:
Console.error(
"The laser file already exists. Use -F to force overwrite it")
Console.quit("Default behaviour: not to overwrite results")
with nav_standard_file.open("r") as nav_standard:
parsed_json_data = json.load(nav_standard)
if start_datetime == "":
epoch_start_time = epoch_from_json(parsed_json_data[1])
start_datetime = epoch_to_datetime(epoch_start_time)
else:
epoch_start_time = string_to_epoch(start_datetime)
if finish_datetime == "":
epoch_finish_time = epoch_from_json(parsed_json_data[-1])
finish_datetime = epoch_to_datetime(epoch_finish_time)
else:
epoch_finish_time = string_to_epoch(finish_datetime)
Console.info("Interpolating laser to ACFR stereo pose data...")
fileout3 = file3.open(
"w"
) # ToDo: Check if file exists and only overwrite if told ('-F')
fileout3.write(cam1[0].get_csv_header())
for i in range(len(parsed_json_data)):
Console.progress(i, len(parsed_json_data))
epoch_timestamp = parsed_json_data[i]["epoch_timestamp"]
if (epoch_timestamp >= epoch_start_time
and epoch_timestamp <= epoch_finish_time):
if "laser" in parsed_json_data[i]["category"]:
filename = parsed_json_data[i]["filename"]
c3_interp = interpolate_camera(epoch_timestamp, cam1,
filename)
fileout3.write(c3_interp.to_csv_row())
Console.info("Done! Laser file available at", str(file3))
def hybis_to_oplab(args):
if args.navigation_file is None:
Console.error("Please provide a navigation file")
Console.quit("Missing comandline arguments")
if args.image_path is None:
Console.error("Please provide an image path")
Console.quit("Missing comandline arguments")
if args.reference_lat is None:
Console.error("Please provide a reference_lat")
Console.quit("Missing comandline arguments")
if args.reference_lon is None:
Console.error("Please provide a reference_lon")
Console.quit("Missing comandline arguments")
if args.output_folder is None:
Console.error("Please provide an output path")
Console.quit("Missing comandline arguments")
parser = HybisParser(
args.navigation_file,
args.image_path,
args.reference_lat,
args.reference_lon,
)
force_overwite = args.force
filepath = get_processed_folder(args.output_folder)
start_datetime = epoch_to_datetime(parser.start_epoch)
finish_datetime = epoch_to_datetime(parser.finish_epoch)
# make path for processed outputs
json_filename = ("json_renav_" + start_datetime[0:8] + "_" +
start_datetime[8:14] + "_" + finish_datetime[0:8] + "_" +
finish_datetime[8:14])
renavpath = filepath / json_filename
if not renavpath.is_dir():
try:
renavpath.mkdir()
except Exception as e:
print("Warning:", e)
elif renavpath.is_dir() and not force_overwite:
# Check if dataset has already been processed
Console.error(
"It looks like this dataset has already been processed for the",
"specified time span.",
)
Console.error(
"The following directory already exist: {}".format(renavpath))
Console.error(
"To overwrite the contents of this directory rerun auv_nav with",
"the flag -F.",
)
Console.error("Example: auv_nav process -F PATH")
Console.quit("auv_nav process would overwrite json_renav files")
output_dr_centre_path = renavpath / "csv" / "dead_reckoning"
if not output_dr_centre_path.exists():
output_dr_centre_path.mkdir(parents=True)
camera_name = "hybis_camera"
output_dr_centre_path = output_dr_centre_path / ("auv_dr_" + camera_name +
".csv")
parser.write(output_dr_centre_path)
parser.write(output_dr_centre_path)
Console.info("Output written to", output_dr_centre_path)
def oplab_to_acfr(args):
if not args.dive_folder:
Console.error("No dive folder provided. Exiting...")
Console.quit("Missing comandline arguments")
if not args.output_folder:
Console.error("No output folder provided. Exiting...")
Console.quit("Missing comandline arguments")
output_folder = get_processed_folder(args.output_folder)
vf = output_folder / "vehicle_pose_est.data"
sf = output_folder / "stereo_pose_est.data"
if (vf.exists() or sf.exists()) and not args.force:
Console.error(
"Output folder already exists. Use -F to overwrite. Exiting...")
Console.quit("Default behaviour: not to overwrite results")
Console.info("Loading mission.yaml")
path_processed = get_processed_folder(args.dive_folder)
mission_file = path_processed / "mission.yaml"
mission = Mission(mission_file)
vehicle_file = path_processed / "vehicle.yaml"
vehicle = Vehicle(vehicle_file)
origin_lat = mission.origin.latitude
origin_lon = mission.origin.longitude
json_list = list(path_processed.glob("json_*"))
if len(json_list) == 0:
Console.quit(
"No navigation solution could be found. Please run ",
"auv_nav parse and process first",
)
navigation_path = path_processed / json_list[0]
# Try if ekf exists:
full_navigation_path = navigation_path / "csv" / "ekf"
vpw = AcfrVehiclePoseWriter(vf, origin_lat, origin_lon)
vehicle_navigation_file = full_navigation_path / "auv_ekf_centre.csv"
dataframe = pd.read_csv(vehicle_navigation_file)
dr_list = []
for _, row in dataframe.iterrows():
sb = SyncedOrientationBodyVelocity()
sb.from_df(row)
dr_list.append(sb)
vpw.write(dr_list)
Console.info("Vehicle pose written to", vf)
if not mission.image.cameras:
Console.warn("No cameras found in the mission file.")
return
if len(mission.image.cameras) == 1:
Console.error("Only one camera found in the mission file. Exiting...")
Console.quit("ACFR stereo pose est requires two cameras.")
camera0_name = mission.image.cameras[0].name
camera1_name = mission.image.cameras[1].name
camera0_nav_file = full_navigation_path / ("auv_ekf_" + camera0_name +
".csv")
dataframe = pd.read_csv(camera0_nav_file)
cam1_list = []
for _, row in dataframe.iterrows():
sb = Camera()
sb.from_df(row)
cam1_list.append(sb)
camera1_nav_file = full_navigation_path / ("auv_ekf_" + camera1_name +
".csv")
dataframe = pd.read_csv(camera1_nav_file)
cam2_list = []
for _, row in dataframe.iterrows():
sb = Camera()
sb.from_df(row)
cam2_list.append(sb)
spw = AcfrStereoPoseWriter(sf, origin_lat, origin_lon)
spw.write(cam1_list, cam2_list)
Console.info("Stereo pose written to", sf)
Console.info("Generating combined.RAW")
nav_standard_file = path_processed / "nav" / "nav_standard.json"
nav_standard_file = get_processed_folder(nav_standard_file)
Console.info("Loading json file {}".format(nav_standard_file))
with nav_standard_file.open("r") as nav_standard:
parsed_json_data = json.load(nav_standard)
start_datetime = ""
finish_datetime = ""
# setup start and finish date time
if start_datetime == "":
epoch_start_time = epoch_from_json(parsed_json_data[1])
start_datetime = epoch_to_datetime(epoch_start_time)
else:
epoch_start_time = string_to_epoch(start_datetime)
if finish_datetime == "":
epoch_finish_time = epoch_from_json(parsed_json_data[-1])
finish_datetime = epoch_to_datetime(epoch_finish_time)
else:
epoch_finish_time = string_to_epoch(finish_datetime)
sensors_std = {
"usbl": {
"model": "sensor"
},
"dvl": {
"model": "sensor"
},
"depth": {
"model": "sensor"
},
"orientation": {
"model": "sensor"
},
}
exporter = AcfrCombinedRawWriter(mission, vehicle, output_folder)
# read in data from json file
# i here is the number of the data packet
for i in range(len(parsed_json_data)):
Console.progress(i, len(parsed_json_data))
epoch_timestamp = parsed_json_data[i]["epoch_timestamp"]
if epoch_timestamp >= epoch_start_time and epoch_timestamp <= epoch_finish_time:
if "velocity" in parsed_json_data[i]["category"]:
if "body" in parsed_json_data[i]["frame"]:
# to check for corrupted data point which have inertial
# frame data values
if "epoch_timestamp_dvl" in parsed_json_data[i]:
# confirm time stamps of dvl are aligned with main
# clock (within a second)
if (abs(parsed_json_data[i]["epoch_timestamp"] -
parsed_json_data[i]["epoch_timestamp_dvl"])
) < 1.0:
velocity_body = BodyVelocity()
velocity_body.from_json(parsed_json_data[i],
sensors_std["dvl"])
exporter.add(velocity_body)
if "inertial" in parsed_json_data[i]["frame"]:
velocity_inertial = InertialVelocity()
velocity_inertial.from_json(parsed_json_data[i])
exporter.add(velocity_inertial)
if "orientation" in parsed_json_data[i]["category"]:
orientation = Orientation()
orientation.from_json(parsed_json_data[i],
sensors_std["orientation"])
exporter.add(orientation)
if "depth" in parsed_json_data[i]["category"]:
depth = Depth()
depth.from_json(parsed_json_data[i], sensors_std["depth"])
exporter.add(depth)
if "altitude" in parsed_json_data[i]["category"]:
altitude = Altitude()
altitude.from_json(parsed_json_data[i])
exporter.add(altitude)
if "usbl" in parsed_json_data[i]["category"]:
usbl = Usbl()
usbl.from_json(parsed_json_data[i], sensors_std["usbl"])
exporter.add(usbl)
if "image" in parsed_json_data[i]["category"]:
camera1 = Camera()
# LC
camera1.from_json(parsed_json_data[i], "camera1")
exporter.add(camera1)
camera2 = Camera()
camera2.from_json(parsed_json_data[i], "camera2")
exporter.add(camera2)