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 __init__(self, node):
"""__init__ is the constructor function
Parameters
-----------
node : cdict
dictionary object for an entry in correct_images.yaml file
"""
valid_distance_metrics = ["uniform", "altitude", "depth_map"]
self.distance_metric = node["distance_metric"]
if self.distance_metric == "none":
self.distance_metric = "uniform"
Console.warn("Distance metric is set to none. This is deprecated.",
"Please use uniform instead.")
if self.distance_metric not in valid_distance_metrics:
Console.error("Distance metric not valid. Please use one of the following:",
valid_distance_metrics)
Console.quit("Invalid distance metric: {}".format(self.distance_metric))
self.metric_path = node["metric_path"]
self.altitude_max = node["altitude_filter"]["max_m"]
self.altitude_min = node["altitude_filter"]["min_m"]
self.smoothing = node["smoothing"]
self.window_size = node["window_size"]
self.outlier_reject = node["curve_fitting_outlier_rejection"]
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 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 line_is_valid(self, line, line_split):
start_or_heading = (line[0] == PhinsHeaders.START
or line[0] == PhinsHeaders.HEADING)
if len(line_split) == 2 and start_or_heading:
# Get timestamp
# Do a check sum as a lot of broken packets are found in phins data
check_sum = str(line_split[1])
# extract from $ to * as per phins manual
string_to_check = ",".join(line)
string_to_check = string_to_check[1:len(
string_to_check)] # noqa E203
string_sum = 0
for i in range(len(string_to_check)):
string_sum ^= ord(string_to_check[i])
if str(hex(string_sum)[2:].zfill(2).upper()) == check_sum.upper():
return True
else:
Console.warn("Broken packet: " + str(line))
Console.warn("Check sum calculated " +
str(hex(string_sum).zfill(2).upper()))
Console.warn("Does not match that provided " +
str(check_sum.upper()))
Console.warn("Ignore and move on")
return False
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 get_camera_idx(self):
idx = [
i
for i, camera_config in enumerate(self.cameraconfigs)
if camera_config.camera_name == self.camera_name
]
if len(idx) > 0:
return idx[0]
else:
Console.warn(
"The camera",
self.camera_name,
"could not be found in the correct_images.yaml",
)
return None
def epoch_timestamp_from_phins(self, line):
epoch_timestamp = None
time_string = str(line[2])
if len(time_string) == 10:
hour = int(time_string[0:2])
mins = int(time_string[2:4])
try:
secs = int(time_string[4:6])
# phins sometimes returns 60s...
if secs < 60:
msec = int(time_string[7:10])
epoch_timestamp = self.get(hour, mins, secs, msec)
except Exception as exc:
Console.warn("Badly formatted packet (PHINS TIME): " +
time_string + " Exception: " + str(exc))
else:
Console.warn("Badly formatted packet (PHINS TIME): " + str(line))
return epoch_timestamp
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 clamp_rotation(self, rotation):
# rotation = (rotation % 2*math.pi)
if abs(rotation) > 3 * math.pi:
Console.warn(
"Absolute value of angle (",
rotation,
") > 3*pi. "
"This is not supposed to happen. This tends to happen when "
"there are no sensor readings for long periods of time, "
"either because there aren't any in this span of time, or due "
"to the Mahalanobis Distance threshold being exceeded "
"repeatedly. Check the covariance matrices and the sensor "
"uncertainties used for the EKF (probably need to bigger), "
"or, as a workaround, use a larger Mahalanobis Distance "
"threshold.",
)
while rotation > math.pi:
rotation -= 2 * math.pi
while rotation < -math.pi:
rotation += 2 * math.pi
return rotation
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 collect_image_files(image_dirs, file_pattern):
# TODO try all different formats: png, jpeg, tif
images = []
if isinstance(image_dirs, list):
for d in image_dirs:
if d.is_dir():
images.extend(list(d.glob(file_pattern)))
else:
Console.warn("Directory " "'{}'" " cannot be found".format(d))
else:
if image_dirs.is_dir():
images = list(image_dirs.glob(file_pattern))
else:
Console.warn("Directory "
"'{}'"
" cannot be found".format(image_dirs))
images.sort()
resolved_images = []
for i in images:
p = Path(i).resolve()
resolved_images.append(p)
return resolved_images
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 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 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)
def parse_gaps(mission, vehicle, category, ftype, outpath):
Console.info(" Parsing GAPS data...")
# parser meta data
class_string = "measurement"
sensor_string = "gaps"
frame_string = "inertial"
timezone = mission.usbl.timezone
timeoffset = mission.usbl.timeoffset
filepath = mission.usbl.filepath
usbl_id = mission.usbl.label
latitude_reference = mission.origin.latitude
longitude_reference = mission.origin.longitude
# define headers used in phins
header_absolute = "$PTSAG" # '<< $PTSAG' #georeferenced strings
header_heading = "$HEHDT" # '<< $HEHDT'
# gaps std models
distance_std_factor = mission.usbl.std_factor
distance_std_offset = mission.usbl.std_offset
broken_packet_flag = False
# read in timezone
timezone_offset = read_timezone(timezone)
# convert to seconds from utc
# timeoffset = -timezone_offset*60*60 + timeoffset
# determine file paths
path = (outpath / ".." / filepath).absolute()
filepath = get_raw_folder(path)
all_list = os.listdir(str(filepath))
gaps_list = [line for line in all_list if ".dat" in line]
Console.info(" " + str(len(gaps_list)) + " GAPS file(s) found")
# extract data from files
data_list = []
if ftype == "acfr":
data_list = ""
for i in range(len(gaps_list)):
path_gaps = filepath / gaps_list[i]
with path_gaps.open("r", errors="ignore") as gaps:
# initialise flag
flag_got_time = 0
for line in gaps.readlines():
line_split = line.strip().split("*")
line_split_no_checksum = line_split[0].strip().split(",")
broken_packet_flag = False
# print(line_split_no_checksum)
# keep on upating ship position to find the prior interpolation
# value of ship coordinates
# line_split_no_checksum[0] == header_absolute:
if header_absolute in line_split_no_checksum[0]:
# start with a ship coordinate
if line_split_no_checksum[6] == str(
usbl_id) and flag_got_time == 2:
if (line_split_no_checksum[11] == "F"
and line_split_no_checksum[13] == "1"):
# read in date
yyyy = int(line_split_no_checksum[5])
mm = int(line_split_no_checksum[4])
dd = int(line_split_no_checksum[3])
# read in time
time_string = str(line_split_no_checksum[2])
try:
hour = int(time_string[0:2])
mins = int(time_string[2:4])
secs = int(time_string[4:6])
msec = int(time_string[7:10])
except ValueError:
broken_packet_flag = True
pass
if secs >= 60:
mins += 1
secs = 0
broken_packet_flag = True
if mins >= 60:
hour += 1
mins = 0
broken_packet_flag = True
if hour >= 24:
dd += 1
hour = 0
broken_packet_flag = True
epoch_time = date_time_to_epoch(
yyyy, mm, dd, hour, mins, secs,
timezone_offset)
# dt_obj = datetime(yyyy,mm,dd,hour,mins,secs)
# time_tuple = dt_obj.timetuple()
# epoch_time = time.mktime(time_tuple)
epoch_timestamp = epoch_time + msec / 1000 + timeoffset
# get position
latitude_negative_flag = False
longitude_negative_flag = False
latitude_string = line_split_no_checksum[7]
latitude_degrees = int(latitude_string[0:2])
latitude_minutes = float(latitude_string[2:10])
if line_split_no_checksum[8] == "S":
latitude_negative_flag = True
longitude_string = line_split_no_checksum[9]
longitude_degrees = int(longitude_string[0:3])
longitude_minutes = float(longitude_string[3:11])
if line_split_no_checksum[10] == "W":
longitude_negative_flag = True
depth = float(line_split_no_checksum[12])
latitude = latitude_degrees + latitude_minutes / 60.0
longitude = longitude_degrees + longitude_minutes / 60.0
if latitude_negative_flag:
latitude = -latitude
if longitude_negative_flag:
longitude = -longitude
# flag raised to proceed
flag_got_time = 3
else:
flag_got_time = 0
if line_split_no_checksum[6] == "0":
if flag_got_time < 3:
# read in date
yyyy = int(line_split_no_checksum[5])
mm = int(line_split_no_checksum[4])
dd = int(line_split_no_checksum[3])
# print(yyyy,mm,dd)
# read in time
time_string = str(line_split_no_checksum[2])
# print(time_string)
hour = int(time_string[0:2])
mins = int(time_string[2:4])
secs = int(time_string[4:6])
try:
msec = int(time_string[7:10])
except ValueError:
broken_packet_flag = True
pass
if secs >= 60:
mins += 1
secs = 0
broken_packet_flag = True
if mins >= 60:
hour += 1
mins = 0
broken_packet_flag = True
if hour >= 24:
dd += 1
hour = 0
broken_packet_flag = True
epoch_time = date_time_to_epoch(
yyyy, mm, dd, hour, mins, secs,
timezone_offset)
# dt_obj = datetime(yyyy,mm,dd,hour,mins,secs)
# time_tuple = dt_obj.timetuple()
# epoch_time = time.mktime(time_tuple)
epoch_timestamp_ship_prior = (epoch_time +
msec / 1000 +
timeoffset)
# get position
latitude_string = line_split_no_checksum[7]
latitude_degrees_ship_prior = int(
latitude_string[0:2])
latitude_minutes_ship_prior = float(
latitude_string[2:10])
latitude_prior = (
latitude_degrees_ship_prior +
latitude_minutes_ship_prior / 60.0)
if line_split_no_checksum[8] == "S":
latitude_prior = -latitude_prior
longitude_string = line_split_no_checksum[9]
longitude_degrees_ship_prior = int(
longitude_string[0:3])
longitude_minutes_ship_prior = float(
longitude_string[3:11])
longitude_prior = (
longitude_degrees_ship_prior +
longitude_minutes_ship_prior / 60.0)
if line_split_no_checksum[10] == "W":
longitude_prior = -longitude_prior
# flag raised to proceed
if flag_got_time < 2:
flag_got_time = flag_got_time + 1
elif flag_got_time >= 3:
if line_split_no_checksum[6] == "0":
# read in date
yyyy = int(line_split_no_checksum[5])
mm = int(line_split_no_checksum[4])
dd = int(line_split_no_checksum[3])
# read in time
time_string = str(line_split_no_checksum[2])
hour = int(time_string[0:2])
mins = int(time_string[2:4])
secs = int(time_string[4:6])
msec = int(time_string[7:10])
# calculate epoch time
epoch_time = date_time_to_epoch(
yyyy,
mm,
dd,
hour,
mins,
secs,
timezone_offset,
)
# dt_obj = datetime(yyyy,mm,dd,hour,mins,secs)
# time_tuple = dt_obj.timetuple()
# epoch_time = time.mktime(time_tuple)
epoch_timestamp_ship_posterior = (epoch_time +
msec / 1000 +
timeoffset)
# get position
latitude_string = line_split_no_checksum[7]
latitude_degrees_ship_posterior = int(
latitude_string[0:2])
latitude_minutes_ship_posterior = float(
latitude_string[2:10])
latitude_posterior = (
latitude_degrees_ship_posterior +
latitude_minutes_ship_posterior / 60.0)
if line_split_no_checksum[8] == "S":
latitude_posterior = -latitude_posterior
longitude_string = line_split_no_checksum[9]
longitude_degrees_ship_posterior = int(
longitude_string[0:3])
longitude_minutes_ship_posterior = float(
longitude_string[3:11])
longitude_posterior = (
longitude_degrees_ship_posterior +
longitude_minutes_ship_posterior / 60.0)
if line_split_no_checksum[10] == "W":
longitude_posterior = -longitude_posterior
# flag raised to proceed
flag_got_time = flag_got_time + 1
# line_split_no_checksum[0] == header_heading:
if header_heading in line_split_no_checksum[0]:
if flag_got_time < 3:
heading_ship_prior = float(line_split_no_checksum[1])
if flag_got_time < 2:
flag_got_time = flag_got_time + 1
else:
heading_ship_posterior = float(
line_split_no_checksum[1])
flag_got_time = flag_got_time + 1
if flag_got_time >= 5:
# interpolate for the ships location and heading
inter_time = (epoch_timestamp - epoch_timestamp_ship_prior
) / (epoch_timestamp_ship_posterior -
epoch_timestamp_ship_prior)
longitude_ship = (inter_time *
(longitude_posterior - longitude_prior) +
longitude_prior)
latitude_ship = (inter_time *
(latitude_posterior - latitude_prior) +
latitude_prior)
heading_ship = (
inter_time *
(heading_ship_posterior - heading_ship_prior) +
heading_ship_prior)
while heading_ship > 360:
heading_ship = heading_ship - 360
while heading_ship < 0:
heading_ship = heading_ship + 360
lateral_distance, bearing = latlon_to_metres(
latitude, longitude, latitude_ship, longitude_ship)
# determine range to input to uncertainty model
distance = math.sqrt(lateral_distance * lateral_distance +
depth * depth)
distance_std = distance_std_factor * distance + distance_std_offset
# determine uncertainty in terms of latitude and longitude
latitude_offset, longitude_offset = metres_to_latlon(
abs(latitude),
abs(longitude),
distance_std,
distance_std,
)
latitude_std = abs(abs(latitude) - latitude_offset)
longitude_std = abs(abs(longitude) - longitude_offset)
# calculate in metres from reference
lateral_distance_ship, bearing_ship = latlon_to_metres(
latitude_ship,
longitude_ship,
latitude_reference,
longitude_reference,
)
eastings_ship = (math.sin(bearing_ship * math.pi / 180.0) *
lateral_distance_ship)
northings_ship = (
math.cos(bearing_ship * math.pi / 180.0) *
lateral_distance_ship)
lateral_distance_target, bearing_target = latlon_to_metres(
latitude,
longitude,
latitude_reference,
longitude_reference,
)
eastings_target = (
math.sin(bearing_target * math.pi / 180.0) *
lateral_distance_target)
northings_target = (
math.cos(bearing_target * math.pi / 180.0) *
lateral_distance_target)
if not broken_packet_flag:
if ftype == "oplab" and category == Category.USBL:
data = {
"epoch_timestamp":
float(epoch_timestamp),
"class":
class_string,
"sensor":
sensor_string,
"frame":
frame_string,
"category":
category,
"data_ship": [
{
"latitude": float(latitude_ship),
"longitude": float(longitude_ship),
},
{
"northings": float(northings_ship),
"eastings": float(eastings_ship),
},
{
"heading": float(heading_ship)
},
],
"data_target": [
{
"latitude": float(latitude),
"latitude_std": float(latitude_std),
},
{
"longitude": float(longitude),
"longitude_std": float(longitude_std),
},
{
"northings": float(northings_target),
"northings_std": float(distance_std),
},
{
"eastings": float(eastings_target),
"eastings_std": float(distance_std),
},
{
"depth": float(depth),
"depth_std": float(distance_std),
},
{
"distance_to_ship": float(distance)
},
],
}
data_list.append(data)
elif ftype == "oplab" and category == Category.DEPTH:
data = {
"epoch_timestamp":
float(epoch_timestamp),
"epoch_timestamp_depth":
float(epoch_timestamp),
"class":
class_string,
"sensor":
sensor_string,
"frame":
"inertial",
"category":
Category.DEPTH,
"data": [{
"depth": float(depth),
"depth_std": float(distance_std),
}],
}
data_list.append(data)
if ftype == "acfr":
data = (
"SSBL_FIX: " + str(float(epoch_timestamp)) +
" ship_x: " + str(float(northings_ship)) +
" ship_y: " + str(float(eastings_ship)) +
" target_x: " + str(float(northings_target)) +
" target_y: " + str(float(eastings_target)) +
" target_z: " + str(float(depth)) +
" target_hr: " + str(float(lateral_distance)) +
" target_sr: " + str(float(distance)) +
" target_bearing: " + str(float(bearing)) +
"\n")
data_list += data
else:
Console.warn("Badly formatted packet (GAPS TIME)")
Console.warn(line)
# print(hour,mins,secs)
# reset flag
flag_got_time = 0
Console.info(" ...done parsing GAPS data.")
return data_list
def __new__(
self,
usbl_data,
dvl_imu_data,
N,
sensors_std,
dvl_noise_sigma_factor,
imu_noise_sigma_factor,
usbl_noise_sigma_factor,
measurement_update_flag=True,
):
# self.dvl_noise_sigma_factor = dvl_noise_sigma_factor
# self.imu_noise_sigma_factor = imu_noise_sigma_factor
# self.usbl_noise_sigma_factor = usbl_noise_sigma_factor
"""
def eval(r, p):
sum = 0.0
for i in range(len(p)): # calculate mean error
dx = (
p[i].eastings[-1] - r.eastings[-1] + (world_size / 2.0)
) % world_size - (world_size / 2.0)
dy = (
p[i].northings[-1] - r.northings[-1] + (world_size / 2.0)
) % world_size - (world_size / 2.0)
err = math.sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
"""
# ========== Start Noise models ========== #
def usbl_noise(usbl_datapoint): # measurement noise
# distance = usbl_datapoint.distance_to_ship # lateral_distance,bearing = latlon_to_metres(usbl_datapoint.latitude, usbl_datapoint.longitude, usbl_datapoint.latitude_ship, usbl_datapoint.longitude_ship) # noqa
# distance = math.sqrt(lateral_distance**2 + usbl_datapoint.depth**2) # noqa
# error = usbl_noise_sigma_factor*(usbl_noise_std_offset + usbl_noise_std_factor*distance) # 5 is for the differential GPS, and the distance std factor 0.01 is used as 0.006 is too sall and unrealistic # This is moved to parse_gaps and parse_usbl_dump # noqa
if usbl_datapoint.northings_std != 0:
error = usbl_datapoint.northings_std * usbl_noise_sigma_factor
else:
usbl_noise_std_offset = 5
usbl_noise_std_factor = 0.01
distance = math.sqrt(usbl_datapoint.northings**2 +
usbl_datapoint.eastings**2 +
usbl_datapoint.depth**2)
error = usbl_noise_sigma_factor * (
usbl_noise_std_offset + usbl_noise_std_factor * distance)
return error
def dvl_noise(dvl_imu_datapoint, mode="estimate"): # sensor1 noise
# Vinnay's dvl_noise model: velocity_std = (-0.0125*((velocity)**2)+0.2*(velocity)+0.2125)/100) assuming noise of x_velocity = y_velocity = z_velocity # noqa
velocity_std_factor = 0.001 # from catalogue rdi whn1200/600. # should read this in from somewhere else, e.g. json # noqa
velocity_std_offset = 0.002 # 0.02 # 0.2 #from catalogue rdi whn1200/600. # should read this in from somewhere else # noqa
x_velocity_std = (
abs(dvl_imu_datapoint.x_velocity) * velocity_std_factor +
velocity_std_offset
) # (-0.0125*((dvl_imu_datapoint.x_velocity)**2)+0.2*(dvl_imu_datapoint.x_velocity)+0.2125)/100 # noqa
y_velocity_std = (
abs(dvl_imu_datapoint.y_velocity) * velocity_std_factor +
velocity_std_offset
) # (-0.0125*((dvl_imu_datapoint.y_velocity)**2)+0.2*(dvl_imu_datapoint.y_velocity)+0.2125)/100 # noqa
z_velocity_std = (
abs(dvl_imu_datapoint.z_velocity) * velocity_std_factor +
velocity_std_offset
) # (-0.0125*((dvl_imu_datapoint.z_velocity)**2)+0.2*(dvl_imu_datapoint.z_velocity)+0.2125)/100 # noqa
if mode == "estimate":
x_velocity_estimate = random.gauss(
dvl_imu_datapoint.x_velocity,
dvl_noise_sigma_factor * x_velocity_std,
)
y_velocity_estimate = random.gauss(
dvl_imu_datapoint.y_velocity,
dvl_noise_sigma_factor * y_velocity_std,
)
z_velocity_estimate = random.gauss(
dvl_imu_datapoint.z_velocity,
dvl_noise_sigma_factor * z_velocity_std,
)
return (
x_velocity_estimate,
y_velocity_estimate,
z_velocity_estimate,
)
elif mode == "std":
return max([x_velocity_std, y_velocity_std, z_velocity_std])
def imu_noise(
previous_dvlimu_data_point,
current_dvlimu_data_point,
particle_list_data,
): # sensor2 noise
imu_noise = (
0.003 * imu_noise_sigma_factor
) # each time_step + 0.003. assuming noise of roll = pitch = yaw
if particle_list_data == 0: # for initiation
roll_estimate = random.gauss(current_dvlimu_data_point.roll,
imu_noise)
pitch_estimate = random.gauss(current_dvlimu_data_point.pitch,
imu_noise)
yaw_estimate = random.gauss(current_dvlimu_data_point.yaw,
imu_noise)
else: # for propagation
roll_estimate = particle_list_data.roll[-1] + random.gauss(
current_dvlimu_data_point.roll -
previous_dvlimu_data_point.roll,
imu_noise,
)
pitch_estimate = particle_list_data.pitch[-1] + random.gauss(
current_dvlimu_data_point.pitch -
previous_dvlimu_data_point.pitch,
imu_noise,
)
yaw_estimate = particle_list_data.yaw[-1] + random.gauss(
current_dvlimu_data_point.yaw -
previous_dvlimu_data_point.yaw,
imu_noise,
)
if yaw_estimate < 0:
yaw_estimate += 360
elif yaw_estimate > 360:
yaw_estimate -= 360
return roll_estimate, pitch_estimate, yaw_estimate
# ========== End Noise models ========== #
def initialize_particles(N, usbl_datapoint, dvl_imu_datapoint,
init_dvl_imu_datapoint):
particles = []
northings_estimate = (usbl_datapoint.northings -
dvl_imu_datapoint.northings +
init_dvl_imu_datapoint.northings)
eastings_estimate = (usbl_datapoint.eastings -
dvl_imu_datapoint.eastings +
init_dvl_imu_datapoint.eastings)
for i in range(N):
temp_particle = Particle()
roll_estimate, pitch_estimate, yaw_estimate = imu_noise(
0, init_dvl_imu_datapoint, 0)
(
x_velocity_estimate,
y_velocity_estimate,
z_velocity_estimate,
) = dvl_noise(init_dvl_imu_datapoint)
usbl_uncertainty = usbl_noise(usbl_datapoint)
# usbl_uncertainty = 0
temp_particle.set(
random.gauss(eastings_estimate, usbl_uncertainty),
random.gauss(northings_estimate, usbl_uncertainty),
init_dvl_imu_datapoint.epoch_timestamp,
x_velocity_estimate,
y_velocity_estimate,
z_velocity_estimate,
roll_estimate,
pitch_estimate,
yaw_estimate,
init_dvl_imu_datapoint.altitude,
init_dvl_imu_datapoint.depth,
)
temp_particle.set_weight(1)
particles.append(temp_particle)
# Normalize weights
weights_list = []
for i in particles:
weights_list.append(i.weight)
normalized_weights = normalize_weights(weights_list)
for index, particle_ in enumerate(particles):
particle_.weight = normalized_weights[index]
return particles
def normalize_weights(weights_list):
normalized_weights = []
for i in weights_list:
normalized_weights.append(i / sum(weights_list))
return normalized_weights
def propagate_particles(particles, previous_data_point,
current_data_point):
for i in particles:
# Propagation error model
time_difference = (current_data_point.epoch_timestamp -
previous_data_point.epoch_timestamp)
roll_estimate, pitch_estimate, yaw_estimate = imu_noise(
previous_data_point, current_data_point, i)
(
x_velocity_estimate,
y_velocity_estimate,
z_velocity_estimate,
) = dvl_noise(current_data_point)
[
north_velocity_estimate,
east_velocity_estimate,
down_velocity_estimate,
] = body_to_inertial(
roll_estimate,
pitch_estimate,
yaw_estimate,
x_velocity_estimate,
y_velocity_estimate,
z_velocity_estimate,
)
[
previous_north_velocity_estimate,
previous_east_velocity_estimate,
previous_down_velocity_estimate,
] = body_to_inertial(
i.roll[-1],
i.pitch[-1],
i.yaw[-1],
i.x_velocity[-1],
i.y_velocity[-1],
i.z_velocity[-1],
)
# DR motion model
northing_estimate = (0.5 * time_difference *
(north_velocity_estimate +
previous_north_velocity_estimate) +
i.northings[-1])
easting_estimate = (
0.5 * time_difference *
(east_velocity_estimate + previous_east_velocity_estimate)
+ i.eastings[-1])
i.set(
easting_estimate,
northing_estimate,
current_data_point.epoch_timestamp,
x_velocity_estimate,
y_velocity_estimate,
z_velocity_estimate,
roll_estimate,
pitch_estimate,
yaw_estimate,
current_data_point.altitude,
current_data_point.depth,
)
def measurement_update(
N,
usbl_measurement,
particles_list,
resample_flag=True
): # updates weights of particles and resamples them # USBL uncertainty follow the readings (0.06/100* depth)! assuming noise of northing = easting # noqa
# Update weights (particle weighting)
for i in particles_list[-1]:
weight = i.measurement_prob(usbl_measurement,
usbl_noise(usbl_measurement))
# weights.append(weight)
# i.weight.append(weight)
i.weight = i.weight * weight
# Normalize weights # this should be in particles...
weights_list = []
for i in particles_list[-1]:
weights_list.append(i.weight)
normalized_weights = normalize_weights(weights_list)
for index, particle_ in enumerate(particles_list[-1]):
particle_.weight = normalized_weights[index]
# calculate Neff
weights_list = []
for i in particles_list[-1]:
weights_list.append(i.weight)
effectiveParticleSize = 1 / sum([i**2 for i in weights_list])
if effectiveParticleSize < len(particles_list[-1]) / 2:
resample_flag = True
else:
resample_flag = False
if resample_flag:
# resampling wheel
temp_particles = []
index = int(random.random() * N)
beta = 0.0
mw = max(weights_list)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > weights_list[index]:
beta -= weights_list[index]
index = (index + 1) % N
temp_particle = Particle()
temp_particle.parentID = "{}-{}".format(
len(particles_list) - 1, index)
particles_list[-1][index].childIDList.append(
"{}-{}".format(len(particles_list),
len(temp_particles)))
temp_particle.set(
particles_list[-1][index].eastings[-1],
particles_list[-1][index].northings[-1],
particles_list[-1][index].timestamps[-1],
particles_list[-1][index].x_velocity[-1],
particles_list[-1][index].y_velocity[-1],
particles_list[-1][index].z_velocity[-1],
particles_list[-1][index].roll[-1],
particles_list[-1][index].pitch[-1],
particles_list[-1][index].yaw[-1],
particles_list[-1][index].altitude[-1],
particles_list[-1][index].depth[-1],
)
temp_particle.set_weight(
1 / N) # particles_list[-1][index].weight)
# temp_particle.set_error(usbl_measurement) # maybe can remove this? # noqa
temp_particles.append(temp_particle)
return (True, temp_particles)
else:
return (False, particles_list)
def extract_trajectory(final_particle):
northings_trajectory = final_particle.northings
eastings_trajectory = final_particle.eastings
timestamp_list = final_particle.timestamps
roll_list = final_particle.roll
pitch_list = final_particle.pitch
yaw_list = final_particle.yaw
altitude_list = final_particle.altitude
depth_list = final_particle.depth
parentID = final_particle.parentID
while parentID != "":
particle_list = int(parentID.split("-")[0])
element_list = int(parentID.split("-")[1])
northings_trajectory = (
particles_list[particle_list][element_list].northings[:-1]
+ northings_trajectory)
eastings_trajectory = (
particles_list[particle_list][element_list].eastings[:-1] +
eastings_trajectory)
timestamp_list = (
particles_list[particle_list][element_list].timestamps[:-1]
+ timestamp_list)
roll_list = (
particles_list[particle_list][element_list].roll[:-1] +
roll_list)
pitch_list = (
particles_list[particle_list][element_list].pitch[:-1] +
pitch_list)
yaw_list = (
particles_list[particle_list][element_list].yaw[:-1] +
yaw_list)
altitude_list = (
particles_list[particle_list][element_list].altitude[:-1] +
altitude_list)
depth_list = (
particles_list[particle_list][element_list].depth[:-1] +
depth_list)
parentID = particles_list[particle_list][element_list].parentID
return (
northings_trajectory,
eastings_trajectory,
timestamp_list,
roll_list,
pitch_list,
yaw_list,
altitude_list,
depth_list,
)
def mean_trajectory(particles):
x_list_ = []
y_list_ = []
for i in particles:
x_list_.append(i.weight * i.eastings[-1])
y_list_.append(i.weight * i.northings[-1])
x = sum(x_list_)
y = sum(y_list_)
return x, y
x_list = []
y_list = []
particles_list = []
usbl_datapoints = []
# print('Initializing particles around first point of dead reckoning solution offset by averaged usbl readings') # noqa
# Interpolate dvl_imu_data to usbl_data to initializing particles at first appropriate usbl timestamp. # noqa
# if dvl_imu_data[dvl_imu_data_index].epoch_timestamp > usbl_data[usbl_data_index].epoch_timestamp: # noqa
# while dvl_imu_data[dvl_imu_data_index].epoch_timestamp > usbl_data[usbl_data_index].epoch_timestamp: # noqa
# usbl_data_index += 1
# interpolate usbl_data to dvl_imu_data to initialize particles
usbl_data_index = 0
dvl_imu_data_index = 0
if (usbl_data[usbl_data_index].epoch_timestamp >
dvl_imu_data[dvl_imu_data_index].epoch_timestamp):
while (usbl_data[usbl_data_index].epoch_timestamp >
dvl_imu_data[dvl_imu_data_index].epoch_timestamp):
dvl_imu_data_index += 1
if (dvl_imu_data[dvl_imu_data_index].epoch_timestamp ==
usbl_data[usbl_data_index].epoch_timestamp):
particles = initialize_particles(
N,
usbl_data[usbl_data_index],
dvl_imu_data[dvl_imu_data_index],
dvl_imu_data[0],
) # *For now assume eastings_std = northings_std, usbl_data[usbl_data_index].eastings_std) # noqa
usbl_data_index += 1
dvl_imu_data_index += 1
elif (usbl_data[usbl_data_index].epoch_timestamp <
dvl_imu_data[dvl_imu_data_index].epoch_timestamp):
while (usbl_data[usbl_data_index + 1].epoch_timestamp <
dvl_imu_data[dvl_imu_data_index].epoch_timestamp):
if len(usbl_data) - 2 == usbl_data_index:
Console.warn(
"USBL data does not span to DVL data. Is your data right?" # noqa
)
break
usbl_data_index += 1
# interpolated_data = interpolate_data(usbl_data[usbl_data_index].epoch_timestamp, dvl_imu_data[dvl_imu_data_index], dvl_imu_data[dvl_imu_data_index+1]) # noqa
interpolated_data = interpolate_usbl(
dvl_imu_data[dvl_imu_data_index].epoch_timestamp,
usbl_data[usbl_data_index],
usbl_data[usbl_data_index + 1],
)
# dvl_imu_data.insert(dvl_imu_data_index+1, interpolated_data)
usbl_data.insert(usbl_data_index + 1, interpolated_data)
particles = initialize_particles(
N,
usbl_data[usbl_data_index],
dvl_imu_data[dvl_imu_data_index + 1],
dvl_imu_data[0],
) # *For now assume eastings_std = northings_std, usbl_data[usbl_data_index].eastings_std) # noqa
usbl_data_index += 1
dvl_imu_data_index += 1
else:
Console.quit(
"Check dvl_imu_data and usbl_data in particle_filter.py.")
usbl_datapoints.append(usbl_data[usbl_data_index - 1])
particles_list.append(particles)
# Force to start at DR init
dvl_imu_data_index = 0
x_, y_ = mean_trajectory(particles)
x_list.append(x_)
y_list.append(y_)
max_uncertainty = 0
usbl_uncertainty_list = []
n = 0
if measurement_update_flag is True: # perform resampling
last_usbl_flag = False
while dvl_imu_data[dvl_imu_data_index] != dvl_imu_data[-1]:
Console.progress(dvl_imu_data_index, len(dvl_imu_data))
time_difference = (
dvl_imu_data[dvl_imu_data_index + 1].epoch_timestamp -
dvl_imu_data[dvl_imu_data_index].epoch_timestamp)
max_uncertainty += (
dvl_noise(dvl_imu_data[dvl_imu_data_index], mode="std") *
time_difference) # * 2
if (dvl_imu_data[dvl_imu_data_index + 1].epoch_timestamp <
usbl_data[usbl_data_index].epoch_timestamp):
propagate_particles(
particles_list[-1],
dvl_imu_data[dvl_imu_data_index],
dvl_imu_data[dvl_imu_data_index + 1],
)
dvl_imu_data_index += 1
else:
if not last_usbl_flag:
# interpolate, insert, propagate, resample measurement_update, add new particles to list, check and assign parent id, check parents that have no children and delete it (skip this step for now) ### # noqa
interpolated_data = interpolate_dvl(
usbl_data[usbl_data_index].epoch_timestamp,
dvl_imu_data[dvl_imu_data_index],
dvl_imu_data[dvl_imu_data_index + 1],
)
dvl_imu_data.insert(dvl_imu_data_index + 1,
interpolated_data)
propagate_particles(
particles_list[-1],
dvl_imu_data[dvl_imu_data_index],
dvl_imu_data[dvl_imu_data_index + 1],
)
usbl_uncertainty_list.append(
usbl_noise(usbl_data[usbl_data_index]))
n += 1
resampled, new_particles = measurement_update(
N, usbl_data[usbl_data_index], particles_list)
if resampled:
particles_list.append(new_particles)
usbl_datapoints.append(usbl_data[usbl_data_index])
# reset usbl_uncertainty_list
usbl_uncertainty_list = []
else:
particles_list = new_particles
if usbl_data[usbl_data_index] == usbl_data[-1]:
last_usbl_flag = True
dvl_imu_data_index += 1
else:
usbl_data_index += 1
dvl_imu_data_index += 1
else:
propagate_particles(
particles_list[-1],
dvl_imu_data[dvl_imu_data_index],
dvl_imu_data[dvl_imu_data_index + 1],
)
dvl_imu_data_index += 1
x_, y_ = mean_trajectory(particles_list[-1])
x_list.append(x_)
y_list.append(y_)
# print (max_uncertainty)
# select particle trajectory with largest overall weight
# particles_weight_list = []
particles_error_list = []
for i in range(len(particles_list[-1])):
parentID = particles_list[-1][i].parentID
particles_error_list.append([])
if len(particles_list[-1][i].error) != 0:
particles_error_list[-1] += particles_list[-1][i].error
while parentID != "":
particle_list = int(parentID.split("-")[0])
element_list = int(parentID.split("-")[1])
parentID = particles_list[particle_list][
element_list].parentID
particles_error_list[-1] += particles_list[particle_list][
element_list].error
for i in range(len(particles_error_list)):
particles_error_list[i] = sum(particles_error_list[i])
selected_particle = particles_list[-1][particles_error_list.index(
min(particles_error_list))]
(
northings_trajectory,
eastings_trajectory,
timestamp_list,
roll_list,
pitch_list,
yaw_list,
altitude_list,
depth_list,
) = extract_trajectory(selected_particle)
else: # do not perform resampling, only propagate
while dvl_imu_data[dvl_imu_data_index] != dvl_imu_data[-1]:
propagate_particles(
particles_list[-1],
dvl_imu_data[dvl_imu_data_index],
dvl_imu_data[dvl_imu_data_index + 1],
)
dvl_imu_data_index += 1
## select particle trajectory with least average error (maybe assign weights without resampling and compare total or average weight? actually doesn't really matter because path won't be used anyway, main purpose of this is to see the std plot) # noqa
particles_error_list = []
for i in range(len(particles_list[-1])):
parentID = particles_list[-1][i].parentID
particles_error_list.append([])
particles_error_list[-1].append(particles_list[-1][i].error)
while parentID != "":
particle_list = int(parentID.split("-")[0])
element_list = int(parentID.split("-")[1])
parentID = particles_list[particle_list][
element_list].parentID
particles_error_list[-1].append(
particles_list[particle_list][element_list].error)
for i in range(len(particles_error_list)):
particles_error_list[i] = sum(particles_error_list[i]) / len(
particles_error_list[i])
selected_particle = particles_list[-1][particles_error_list.index(
min(particles_error_list))]
(
northings_trajectory,
eastings_trajectory,
timestamp_list,
roll_list,
pitch_list,
yaw_list,
altitude_list,
depth_list,
) = extract_trajectory(selected_particle)
# calculate northings std, eastings std, yaw std of particles
northings_std = []
eastings_std = []
yaw_std = []
arr_northings = []
arr_eastings = []
arr_yaw = []
for i in particles_list[0]:
arr_northings.append([])
arr_eastings.append([])
arr_yaw.append([])
for i in range(len(particles_list)):
for j in range(len(particles_list[i])):
if i != len(particles_list) - 1:
arr_northings[j] += particles_list[i][j].northings[:-1]
arr_eastings[j] += particles_list[i][j].eastings[:-1]
arr_yaw[j] += particles_list[i][j].yaw[:-1]
else:
arr_northings[j] += particles_list[i][j].northings
arr_eastings[j] += particles_list[i][j].eastings
arr_yaw[j] += particles_list[i][j].yaw
arr_northings = numpy.array(arr_northings)
arr_eastings = numpy.array(arr_eastings)
arr_yaw = numpy.array(arr_yaw)
for i in numpy.std(arr_northings, axis=0):
northings_std.append(i)
for i in numpy.std(arr_eastings, axis=0):
eastings_std.append(i)
# yaw_std step check for different extreme values around 0 and 360. not sure if this method below is robust. # noqa
arr_std_yaw = numpy.std(arr_yaw, axis=0)
arr_yaw_change = []
for i in range(len(arr_std_yaw)):
if (
arr_std_yaw[i] > 30
): # if std is more than 30 deg, means there's two extreme values, so minus 360 for anything above 180 deg. # noqa
arr_yaw_change.append(i)
# yaw_std.append(i)
for i in arr_yaw:
for j in arr_yaw_change:
if i[j] > 180:
i[j] -= 360
arr_std_yaw = numpy.std(arr_yaw, axis=0)
for i in arr_std_yaw:
yaw_std.append(i)
# numpy.mean(arr, axis=0)
pf_fusion_dvl_list = []
for i in range(len(timestamp_list)):
pf_fusion_dvl = SyncedOrientationBodyVelocity()
pf_fusion_dvl.epoch_timestamp = timestamp_list[i]
pf_fusion_dvl.northings = northings_trajectory[i]
pf_fusion_dvl.eastings = eastings_trajectory[i]
pf_fusion_dvl.depth = depth_list[i]
pf_fusion_dvl.roll = roll_list[i]
pf_fusion_dvl.pitch = pitch_list[i]
pf_fusion_dvl.yaw = yaw_list[i]
pf_fusion_dvl.altitude = altitude_list[i]
pf_fusion_dvl_list.append(pf_fusion_dvl)
# plt.scatter(x_list, y_list)
return (
pf_fusion_dvl_list,
usbl_datapoints,
particles_list,
northings_std,
eastings_std,
yaw_std,
)
def create_output_directories(self):
"""Handle the creation of output directories for each camera"""
self.output_dir_path = self.path_processed / "image"
self.output_dir_path /= self.camera_name
# Create output directories depending on the correction method
parameters_folder_str = "params_"
developed_folder_str = "corrected_"
if self.correction_method == "colour_correction":
parameters_folder_str += self.distance_metric
developed_folder_str += self.distance_metric
developed_folder_str += (
"_mean_" + str(int(self.brightness)) + "_std_" + str(int(self.contrast))
)
elif self.correction_method == "manual_balance":
parameters_folder_str += "manual"
developed_folder_str += "manual"
developed_folder_str += (
"_gain_"
+ str(self.color_gain_matrix_rgb[0])
+ "_"
+ str(self.color_gain_matrix_rgb[4])
+ "_"
+ str(self.color_gain_matrix_rgb[8])
+ "_sub_"
+ str(self.subtractors_rgb[0])
+ "_"
+ str(self.subtractors_rgb[1])
+ "_"
+ str(self.subtractors_rgb[2])
)
# Accept suffixes for the output directories
if self.suffix:
parameters_folder_str += "_" + self.suffix
developed_folder_str += "_" + self.suffix
self.attenuation_parameters_folder = (
self.output_dir_path / parameters_folder_str
)
self.output_images_folder = self.output_dir_path / developed_folder_str
if not self.attenuation_parameters_folder.exists():
self.attenuation_parameters_folder.mkdir(parents=True)
else:
file_list = list(self.attenuation_parameters_folder.glob("*.npy"))
if len(file_list) > 0:
if not self.force:
Console.quit(
"Parameters exist for current configuration.",
"Run parse with Force (-F flag)...",
)
else:
Console.warn(
"Code will overwrite existing parameters for ",
"current configuration...",
)
if not self.output_images_folder.exists():
self.output_images_folder.mkdir(parents=True)
else:
file_list = list(self.output_images_folder.glob("*.*"))
if len(file_list) > 0:
if not self.force:
Console.quit(
"Corrected images exist for current configuration. ",
"Run process with Force (-F flag)...",
)
else:
Console.warn(
"Code will overwrite existing corrected images for ",
"current configuration...",
)
def generate_attenuation_correction_parameters(self):
"""Generates image stats and attenuation coefficients and saves the
parameters for process"""
if len(self.altitude_list) < 3:
Console.quit(
"Insufficient number of images to compute attenuation ",
"parameters...",
)
# create empty matrices to store image correction parameters
self.image_raw_mean = np.empty(
(self.image_channels, self.image_height, self.image_width),
dtype=np.float32,
)
self.image_raw_std = np.empty(
(self.image_channels, self.image_height, self.image_width),
dtype=np.float32,
)
self.image_attenuation_parameters = np.empty(
(self.image_channels, self.image_height, self.image_width, 3),
dtype=np.float32,
)
self.image_corrected_mean = np.empty(
(self.image_channels, self.image_height, self.image_width),
dtype=np.float32,
)
self.image_corrected_std = np.empty(
(self.image_channels, self.image_height, self.image_width),
dtype=np.float32,
)
self.correction_gains = np.empty(
(self.image_channels, self.image_height, self.image_width),
dtype=np.float32,
)
image_size_gb = (
self.image_channels
* self.image_height
* self.image_width
* 4.0
/ (1024.0 ** 3)
)
max_bin_size_gb = 50.0
max_bin_size = int(max_bin_size_gb / image_size_gb)
self.bin_band = 0.1
hist_bins = np.arange(self.altitude_min, self.altitude_max, self.bin_band)
# Watch out: need to substract 1 to get the correct number of bins
# because the last bin is not included in the range
images_fn, images_map = open_memmap(
shape=(
len(hist_bins) - 1,
self.image_height * self.image_width,
self.image_channels,
),
dtype=np.float32,
)
distances_fn, distances_map = open_memmap(
shape=(len(hist_bins) - 1, self.image_height * self.image_width),
dtype=np.float32,
)
distance_vector = None
if self.depth_map_list and self.distance_metric == "depth_map":
Console.info("Computing depth map histogram with", hist_bins.size, " bins")
distance_vector = np.zeros((len(self.depth_map_list), 1))
for i, dm_file in enumerate(self.depth_map_list):
dm_np = depth_map.loader(dm_file, self.image_width, self.image_height)
distance_vector[i] = dm_np.mean(axis=1)
elif self.altitude_list and self.distance_metric == "altitude":
Console.info("Computing altitude histogram with", hist_bins.size, " bins")
distance_vector = np.array(self.altitude_list)
if distance_vector is not None:
idxs = np.digitize(distance_vector, hist_bins) - 1
# Display histogram in console
for idx_bin in range(hist_bins.size - 1):
tmp_idxs = np.where(idxs == idx_bin)[0]
Console.info(
" Bin",
format(idx_bin, "02d"),
"(",
round(hist_bins[idx_bin], 1),
"m < x <",
round(hist_bins[idx_bin + 1], 1),
"m):",
len(tmp_idxs),
"images",
)
with tqdm_joblib(
tqdm(desc="Computing altitude histogram", total=hist_bins.size - 1,)
):
joblib.Parallel(n_jobs=-2, verbose=0)(
joblib.delayed(self.compute_distance_bin)(
idxs,
idx_bin,
images_map,
distances_map,
max_bin_size,
max_bin_size_gb,
distance_vector,
)
for idx_bin in range(hist_bins.size - 1)
)
# calculate attenuation parameters per channel
self.image_attenuation_parameters = corrections.calculate_attenuation_parameters(
images_map,
distances_map,
self.image_height,
self.image_width,
self.image_channels,
)
# delete memmap handles
del images_map
os.remove(images_fn)
del distances_map
os.remove(distances_fn)
# Save attenuation parameter results.
np.save(
self.attenuation_params_filepath, self.image_attenuation_parameters,
)
corrections.save_attenuation_plots(
self.attenuation_parameters_folder,
attn=self.image_attenuation_parameters,
)
# compute correction gains per channel
target_altitude = distance_vector.mean()
Console.info(
"Computing correction gains for target altitude", target_altitude,
)
self.correction_gains = corrections.calculate_correction_gains(
target_altitude,
self.image_attenuation_parameters,
self.image_height,
self.image_width,
self.image_channels,
)
Console.warn("Saving correction gains")
# Save correction gains
np.save(self.correction_gains_filepath, self.correction_gains)
corrections.save_attenuation_plots(
self.attenuation_parameters_folder, gains=self.correction_gains,
)
# Useful if fails, to reload precomputed numpyfiles.
# TODO: offer as a new step.
# self.image_attenuation_parameters = np.load(
# self.attenuation_params_filepath)
# self.correction_gains = np.load(self.correction_gains_filepath)
# apply gains to images
Console.info("Applying attenuation corrections to images...")
image_properties = [
self.image_height,
self.image_width,
self.image_channels,
]
runner = RunningMeanStd(image_properties)
memmap_filename, memmap_handle = open_memmap(
shape=(
len(self.camera_image_list),
self.image_height,
self.image_width,
self.image_channels,
),
dtype=np.float32,
)
# DEBUG: can be removed
# Console.error("depth_map_list size", len(self.depth_map_list))
# Console.error("camera_image_list size", len(self.camera_image_list))
###################################################################################################
for i in trange(len(self.camera_image_list)):
# Load the image
img = self.loader(self.camera_image_list[i])
# Load the distance matrix
if not self.depth_map_list:
# TODO: Show the depth_map creation
# if self.depth_map_list is None:
# Generate matrices on the fly
distance = distance_vector[i]
distance_mtx = np.empty((self.image_height, self.image_width))
distance_mtx.fill(distance)
else:
distance_mtx = depth_map.loader(
self.depth_map_list[i], self.image_width, self.image_height,
)
# TODO: Show the size of the produced distance_mtx
# Correct the image
corrected_img = corrections.attenuation_correct(
img,
distance_mtx,
self.image_attenuation_parameters,
self.correction_gains,
)
# TODO: Inspect the corrected image after attenuation correction
# Before calling compute, let's show the corrected_img dimensions
# Console.error("corrected_img.shape", corrected_img.shape)
runner.compute(corrected_img)
memmap_handle[i] = corrected_img.reshape(
self.image_height, self.image_width, self.image_channels
)
image_corrected_mean = runner.mean.reshape(
self.image_height, self.image_width, self.image_channels
)
image_corrected_std = runner.std.reshape(
self.image_height, self.image_width, self.image_channels
)
# save parameters for process
np.save(
self.corrected_mean_filepath, image_corrected_mean
) # TODO: make member
np.save(
self.corrected_std_filepath, image_corrected_std
) # TODO: make member
corrections.save_attenuation_plots(
self.attenuation_parameters_folder,
img_mean=image_corrected_mean,
img_std=image_corrected_std,
)
else:
Console.info(
"No altitude or depth maps available.", "Computing raw mean and std.",
)
image_raw_mean, image_raw_std = running_mean_std(
self.camera_image_list, self.loader
)
np.save(self.raw_mean_filepath, image_raw_mean)
np.save(self.raw_std_filepath, image_raw_std)
# image_raw_mean = np.load(self.raw_mean_filepath)
# image_raw_std = np.load(self.raw_std_filepath)
ch = image_raw_mean.shape[0]
if ch == 3:
image_raw_mean = image_raw_mean.transpose((1, 2, 0))
image_raw_std = image_raw_std.transpose((1, 2, 0))
imageio.imwrite(
Path(self.attenuation_parameters_folder) / "image_raw_mean.png",
image_raw_mean,
)
imageio.imwrite(
Path(self.attenuation_parameters_folder) / "image_raw_std.png",
image_raw_std,
)
corrections.save_attenuation_plots(
self.attenuation_parameters_folder,
img_mean=image_raw_mean,
img_std=image_raw_std,
)
Console.info("Correction parameters saved...")
def get_imagelist(self):
"""Generate list of source images"""
# Store a copy of the currently stored image list in the Corrector object
_original_image_list = self.camera_image_list
_original_altitude_list = self.altitude_list
# Replaces Corrector object's image_list with the camera image list
# OLD: self.camera_image_list = self.camera.image_list # <---- Now this is done at the end (else condition)
self.camera_image_list = []
self.altitude_list = []
# If using colour_correction, we need to read in the navigation
if self.correction_method == "colour_correction":
if self.distance_path == "json_renav_*":
Console.info(
"Picking first JSON folder as the default path to auv_nav",
"csv files...",
)
dir_ = self.path_processed
json_list = list(dir_.glob("json_*"))
if len(json_list) == 0:
Console.quit(
"No navigation solution could be found. Please run ",
"auv_nav parse and process first",
)
self.distance_path = json_list[0]
metric_path = self.path_processed / self.distance_path
# Try if ekf exists:
full_metric_path = metric_path / "csv" / "ekf"
metric_file = "auv_ekf_" + self.camera_name + ".csv"
if not full_metric_path.exists():
full_metric_path = metric_path / "csv" / "dead_reckoning"
metric_file = "auv_dr_" + self.camera_name + ".csv"
self.altitude_csv_path = full_metric_path / metric_file
# Check if file exists
if not self.altitude_csv_path.exists():
Console.quit(
"The navigation CSV file is not present. Run auv_nav first."
)
# read dataframe for corresponding distance csv path
dataframe = pd.read_csv(self.altitude_csv_path)
# Deal with bagfiles:
# if the extension is bag, the list is a list of bagfiles
if self.camera.extension == "bag":
self.camera.bagfile_list = copy.deepcopy(self.camera.image_list)
self.loader.set_bagfile_list_and_topic(
self.camera.bagfile_list, self.camera.topic
)
# get imagelist for given camera object
if self.user_specified_image_list != "none":
path_file_list = Path(self.path_config) / self.user_specified_image_list
trimmed_csv_file = "trimmed_csv_" + self.camera_name + ".csv"
self.trimmed_csv_path = Path(self.path_config) / trimmed_csv_file
if not self.altitude_csv_path.exists():
message = "Path to " + metric_file + " does not exist..."
Console.quit(message)
else:
# create trimmed csv based on user's list of images
dataframe = trim_csv_files(
path_file_list, self.altitude_csv_path, self.trimmed_csv_path,
)
# Check images exist:
valid_idx = []
for idx, entry in enumerate(dataframe["relative_path"]):
im_path = self.path_raw / entry
if im_path.exists() or self.camera.extension == "bag":
valid_idx.append(idx)
filtered_dataframe = dataframe.iloc[valid_idx]
filtered_dataframe.reset_index(drop=True)
# WARNING: if the column does not contain any 'None' entry, it will be parsed as float, and the .str() accesor will fail
filtered_dataframe["altitude [m]"] = filtered_dataframe[
"altitude [m]"
].astype("string")
filtered_dataframe = filtered_dataframe[
~filtered_dataframe["altitude [m]"].str.contains("None")
] # drop rows with None altitude
distance_list = filtered_dataframe["altitude [m]"].tolist()
for _, row in filtered_dataframe.iterrows():
alt = float(row["altitude [m]"])
if alt > self.altitude_min and alt < self.altitude_max:
if self.camera.extension == "bag":
timestamp_from_filename = Path(row["relative_path"]).stem
if "\\" in timestamp_from_filename:
timestamp_from_filename = timestamp_from_filename.split(
"\\"
)[-1]
self.camera_image_list.append(timestamp_from_filename)
else:
self.camera_image_list.append(
self.path_raw / row["relative_path"]
)
self.altitude_list.append(alt)
if len(distance_list) == 0:
Console.error("No images exist / can be found!")
Console.error(
"Check the file",
self.altitude_csv_path,
"and make sure that the 'relative_path' column points to",
"existing images relative to the raw mission folder (e.g.",
self.path_raw,
")",
)
Console.error("You may need to reprocess the dive with auv_nav")
Console.quit("No images were found.")
# WARNING: what happens in a multidive setup when the current dive has no images (but the rest of the dive does)?
Console.info(
len(self.altitude_list),
"/",
len(distance_list),
"Images filtered as per altitude range...",
)
if len(self.altitude_list) < 3:
Console.quit(
"Insufficient number of images to compute attenuation ",
"parameters...",
)
else:
# Copy the images list from the camera
self.camera_image_list = self.camera.image_list
# Join the current image list with the original image list (copy)
self.camera_image_list.extend(_original_image_list)
# Show size of the extended image list
Console.warn(
">> The camera image list is now", len(self.camera_image_list)
) # JC: I'm leaving this as it is informative for multidive
# Join the current image list with the original image list (copy)
self.altitude_list.extend(_original_altitude_list)
def fit_and_save(self, cloud, processed_folder):
"""Fit mean plane and uncertainty bounding planes to point cloud
Parameters
----------
cloud : ndarray of shape (nx3)
Point cloud
processed_folder : Path
Path of the processed folder where outputs are written
Returns
-------
String
Plane parameters of the mean plane and a set of uncertainty
bounding planes of the point cloud in yaml-file format.
"""
total_no_points = len(cloud)
# Fit mean plane
Console.info("Fitting a plane to", total_no_points, "points...")
p = Plane([1, 0, 0, 1.5])
mean_plane, self.inliers_cloud_list = p.fit(cloud, self.mdt)
# p.plot(cloud=cloud)
filename = time.strftime("pointclouds_and_best_model_%Y%m%d_%H%M%S.html")
plot_pointcloud_and_planes(
[np.array(cloud), np.array(self.inliers_cloud_list)],
[np.array(mean_plane)],
str(processed_folder / filename),
)
scale = 1.0 / mean_plane[0]
mean_plane = np.array(mean_plane) * scale
mean_plane = mean_plane.tolist()
Console.info("Least squares found", len(self.inliers_cloud_list), "inliers")
if len(self.inliers_cloud_list) < 0.5 * len(cloud) * self.gip:
Console.warn("The number of inliers found are off from what you expected.")
Console.warn(" * Expected inliers:", len(cloud) * self.gip)
Console.warn(" * Found inliers:", len(self.inliers_cloud_list))
Console.warn(
"Check the output cloud to see if the found plane makes sense."
)
Console.warn("Try to increase your distance threshold.")
inliers_cloud = np.array(self.inliers_cloud_list)
mean_x = np.mean(inliers_cloud[:, 0])
mean_y = np.mean(inliers_cloud[:, 1])
mean_z = np.mean(inliers_cloud[:, 2])
mean_xyz = np.array([mean_x, mean_y, mean_z])
# Determine minimum distance between points as function of inlier
# point cloud size
std_y = np.std(inliers_cloud[:, 1])
std_z = np.std(inliers_cloud[:, 2])
# print("Min y: " + str(np.min(inliers_cloud[:, 1])))
# print("Max y: " + str(np.max(inliers_cloud[:, 1])))
# print("Std y: " + str(std_y))
# print("Min z: " + str(np.min(inliers_cloud[:, 2])))
# print("Max z: " + str(np.max(inliers_cloud[:, 2])))
# print("Std z: " + str(std_z))
min_dist = 2 * math.sqrt(std_y ** 2 + std_z ** 2)
Console.info("Minimum distance for poisson disc sampling: {}".format(min_dist))
min_sin_angle = 0.866 # = sin(60°)
# Append 1 to the points, so they can be multiplied (dot product) with
# plane paramters to find out if they are in front, behind or on a
# plane.
self.inliers_1 = np.concatenate(
[inliers_cloud, np.ones((inliers_cloud.shape[0], 1))], axis=1
)
planes_enclose_inliers = False
Console.info("Generating uncertainty planes...")
max_uncertainty_planes = 300
tries = 0
failed_distance = 0
failed_angle = 0
while (
planes_enclose_inliers is False
and len(self.uncertainty_planes) < max_uncertainty_planes
):
tries += 1
point_cloud_local = random.sample(self.inliers_cloud_list, 3)
# Check if the points are sufficiently far apart and not aligned
p0p1 = point_cloud_local[1][1:3] - point_cloud_local[0][1:3]
p0p2 = point_cloud_local[2][1:3] - point_cloud_local[0][1:3]
p1p2 = point_cloud_local[2][1:3] - point_cloud_local[1][1:3]
p0p1_norm = np.linalg.norm(p0p1)
p0p2_norm = np.linalg.norm(p0p2)
p1p2_norm = np.linalg.norm(p1p2)
# Poisson disc sampling: reject points that are too close together
if p0p1_norm < min_dist or p0p2_norm < min_dist or p1p2_norm < min_dist:
failed_distance += 1
if failed_distance % 100000 == 0:
Console.info(
"Combinations rejected due to distance criterion",
"(Poisson disk sampling):",
failed_distance,
"times,",
"due to angle criterion:",
failed_angle,
"times",
)
continue
# Reject points that are too closely aligned
if abs(np.cross(p0p1, p0p2)) / (p0p1_norm * p0p2_norm) < min_sin_angle:
failed_angle += 1
if failed_angle % 100000 == 0:
print(
"Combinations rejected due to distance criterion",
"(Poisson disk sampling):",
failed_distance,
"times,",
"due to angle criterion:",
failed_angle,
"times",
)
continue
# Compute plane through the 3 points and append to list
self.triples.append(np.array(point_cloud_local))
self.uncertainty_planes.append(plane_through_3_points(point_cloud_local))
Console.info(
"Number of planes: ",
len(self.uncertainty_planes),
", " "Number of tries so far: ",
tries,
".",
"Combinations rejected due to distance criterion",
"(Poisson disk sampling):",
failed_distance,
"times,",
"due to angle criterion:",
failed_angle,
"times",
)
planes_enclose_inliers = self._do_planes_enclose_inliers()
Console.info(
"... finished generating {} uncertainty planes.".format(
len(self.uncertainty_planes)
)
)
if len(self.uncertainty_planes) >= max_uncertainty_planes:
Console.warn("Stopped due to reaching max_uncertainty_planes")
filename = time.strftime(
"pointclouds_and_uncertainty_planes_all_" "%Y%m%d_%H%M%S.html"
)
plot_pointcloud_and_planes(
self.triples + [np.array(cloud), inliers_cloud],
self.uncertainty_planes,
str(processed_folder / filename),
)
# uncomment to save for debugging
# np.save('inliers_cloud.npy', inliers_cloud)
# for i, plane in enumerate(self.uncertainty_planes):
# np.save('plane' + str(i) + '.npy', plane)
# Console.info("Removing uncertainty planes that do not contribute...")
# self._remove_unnecessary_uncertainty_planes()
# Console.info("... finished removing non-contributing planes. Reduced"
# "number of uncertainty planes to {}."
# .format(len(self.uncertainty_planes)))
# assert self._check_if_planes_enclose_inliers() # Sanity check
filename = time.strftime(
"pointclouds_and_uncertainty_planes_%Y%m%d_" "%H%M%S.html"
)
plot_pointcloud_and_planes(
self.triples + [np.array(cloud), inliers_cloud],
self.uncertainty_planes,
str(processed_folder / filename),
)
yaml_msg = (
"mean_xyz_m: "
+ str(mean_xyz.tolist())
+ "\n"
+ "mean_plane: "
+ str(mean_plane)
+ "\n"
)
if len(self.uncertainty_planes) > 0:
uncertainty_planes_str = "uncertainty_planes:\n"
for i, up in enumerate(self.uncertainty_planes):
uncertainty_planes_str += " - " + str(up.tolist()) + "\n"
yaml_msg += uncertainty_planes_str
yaml_msg += (
'date: "'
+ Console.get_date()
+ '" \n'
+ 'user: "******" \n'
+ 'host: "'
+ Console.get_hostname()
+ '" \n'
+ 'version: "'
+ Console.get_version()
+ '" \n'
)
return yaml_msg
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 call_parse(args):
"""Perform parsing of configuration yaml files and generate image
correction parameters
Parameters
-----------
args : parse_args object
User provided arguments for path of source images
"""
# Now args.path is a list of paths (str / os.PathLike objects)
path_list = [Path(path).resolve() for path in args.path]
if len(path_list) == 1:
path = path_list[0]
Console.info("Single path provided, normal single dive mode...")
else:
Console.warn(
"Multiple paths provided [{}]. Checking each path...".format(
len(path_list)))
for path in path_list:
# chec if path is valid
if not path.exists():
Console.error("Path", path,
"does not exist! Exiting...") # quit
sys.exit(1)
else:
Console.info("\t", path, " [OK]")
# Populating the configuration and camerasystem lists for each dive path
# The camera system is pulled first from <config> folder if available, if not from <raw> folder
correct_config_list, camerasystem_list = zip(*[
load_configuration_and_camera_system(path, args.suffix)
for path in path_list
])
# correct_config <--- from correct_images.yaml (<config> folder)
# camerasystem <--- from camera.yaml (<config> folder or from <raw> folder)
# Let's check that both lists have the same length and are not empty (same number of dives)
if len(correct_config_list) != len(camerasystem_list):
Console.error("Number of [camerasystem] and [configuration] differ!")
sys.exit(1)
if len(correct_config_list) == 0:
Console.error("No valid camerasystem/configuration found!")
sys.exit(1)
# When in multidive mode, check if all camerasystem are the same. For this we test camera_system.camera_system
if len(
camerasystem_list
) > 1: # this test is still valid for single dive mode, so we could remove this [if] sentence
camera_system = camerasystem_list[0]
for cs in camerasystem_list:
# the first entry will be repeated, no problem with that
# TODO: Extend is_equivalent() method allowing checking cameras in different orders
# WARNING: We decide not to use equivalent() here, because it is not robust enough. Enforce same camera order
if not camera_system.camera_system == cs.camera_system:
Console.error("Camera systems differ!")
Console.error("\tFirst camera system (reference) ",
camera_system.camera_system)
Console.error("\tWrong camera system (current) ",
cs.camera_system)
sys.exit(1)
Console.warn(
"Camera systems are the same for all dives.") # so far so good
# Check if the correct_config_lists elements are the same (equivalent)
if len(correct_config_list) > 1:
correct_config = correct_config_list[0]
for cc in correct_config_list:
# Check if the relevant fields of the configuration are the same (including per-dive camera setup)
if not correct_config.is_equivalent(cc):
Console.error("Configurations [correct_config] do not match!")
sys.exit(1)
Console.warn("Configurations are equivalent for all dives.")
camerasystem = camerasystem_list[0]
for camera in camerasystem.cameras:
# check if the camera also exists in the configuration
if camera.name not in [
c.camera_name for c in correct_config.configs.camera_configs
]: # ignore if not present
Console.warn(
"Camera [", camera.name,
"] defined in <camera.yaml> but not found in configuration. Skipping..."
)
else:
Console.info("Parsing for camera", camera.name)
# Create a Corrector object for each camera with empty configuration
# The configuration and the paths will be populated later on a per-dive basis
corrector = Corrector(args.force,
args.suffix,
camera,
correct_config=None)
# call new list-compatible implementation of parse()
corrector.parse(path_list, correct_config_list)
Console.info(
"Parse completed for all cameras. Please run process to develop ",
"corrected images...",
)
def warn_if_zero(val, name):
if val == 0:
Console.warn("The value for", name, "is zero. Is this expected?")
def load_configuration_and_camera_system(path, suffix=None):
"""Generate correct_config and camera system objects from input config
yaml files
Parameters
-----------
path : Path
User provided Path of source images
"""
Console.warn("Parsing multipaths with suffix:", suffix)
# resolve paths to raw, processed and config folders
path_raw_folder = get_raw_folder(path)
path_config_folder = get_config_folder(path)
# resolve path to mission.yaml
path_mission = path_raw_folder / "mission.yaml"
# find mission and correct_images yaml files
if path_mission.exists():
Console.info("File mission.yaml found at", path_mission)
else:
Console.quit("File mission.yaml file not found at", path_raw_folder)
# load mission parameters
mission = Mission(path_mission)
# resolve path to camera.yaml file
camera_yaml_raw_path = path_raw_folder / "camera.yaml"
camera_yaml_config_path = path_config_folder / "camera.yaml"
camera_yaml_path = None
default_file_path_correct_config = None
if not camera_yaml_raw_path.exists(
) and not camera_yaml_config_path.exists():
Console.info(
"Not found camera.yaml file in /raw folder...Using default ",
"camera.yaml file...",
)
# find out default yaml paths
root = Path(__file__).resolve().parents[1]
acfr_std_camera_file = "auv_nav/default_yaml/ts1/SSK17-01/camera.yaml"
sx3_camera_file = "auv_nav/default_yaml/ae2000/YK17-23C/camera.yaml"
biocam_camera_file = "auv_nav/default_yaml/as6/DY109/camera.yaml"
biocam4000_15c_camera_file = "auv_nav/default_yaml/alr/jc220/camera.yaml"
hybis_camera_file = "auv_nav/default_yaml/hybis/camera.yaml"
ntnu_camera_file = "auv_nav/default_yaml/ntnu_stereo/tautra21/camera.yaml"
rosbag_camera_file = "auv_nav/default_yaml/rosbag/grassmap/camera.yaml"
acfr_std_correct_config_file = (
"correct_images/default_yaml/acfr/correct_images.yaml")
sx3_std_correct_config_file = (
"correct_images/default_yaml/sx3/correct_images.yaml")
biocam_std_correct_config_file = (
"correct_images/default_yaml/biocam/correct_images.yaml")
biocam4000_15c_std_correct_config_file = (
"correct_images/default_yaml/biocam4000_15c/correct_images.yaml")
hybis_std_correct_config_file = (
"correct_images/default_yaml/hybis/correct_images.yaml")
ntnu_std_correct_config_file = (
"correct_images/default_yaml/ntnu_stereo/correct_images.yaml")
rosbag_std_correct_config_file = (
"correct_images/default_yaml/rosbag/correct_images.yaml")
Console.info("Image format:", mission.image.format)
if mission.image.format == "acfr_standard":
camera_yaml_path = root / acfr_std_camera_file
default_file_path_correct_config = root / acfr_std_correct_config_file
elif mission.image.format == "seaxerocks_3":
camera_yaml_path = root / sx3_camera_file
default_file_path_correct_config = root / sx3_std_correct_config_file
elif mission.image.format == "biocam":
camera_yaml_path = root / biocam_camera_file
default_file_path_correct_config = root / biocam_std_correct_config_file
elif mission.image.format == "biocam4000_15c":
camera_yaml_path = root / biocam4000_15c_camera_file
default_file_path_correct_config = (
root / biocam4000_15c_std_correct_config_file)
elif mission.image.format == "hybis":
camera_yaml_path = root / hybis_camera_file
default_file_path_correct_config = root / hybis_std_correct_config_file
elif mission.image.format == "ntnu_stereo":
camera_yaml_path = root / ntnu_camera_file
default_file_path_correct_config = root / ntnu_std_correct_config_file
elif mission.image.format == "rosbag":
camera_yaml_path = root / rosbag_camera_file
default_file_path_correct_config = root / rosbag_std_correct_config_file
camera_yaml_path.copy(camera_yaml_config_path)
Console.info(
"Copied camera.yaml file to config folder. Please edit it.")
Console.info("The file is located at", camera_yaml_config_path)
else:
Console.quit(
"Image system in camera.yaml does not match with mission.yaml",
"Provide correct camera.yaml in /raw folder... ",
)
elif camera_yaml_raw_path.exists(
) and not camera_yaml_config_path.exists():
Console.info("Found camera.yaml file in /raw folder")
camera_yaml_path = camera_yaml_raw_path
elif not camera_yaml_raw_path.exists() and camera_yaml_config_path.exists(
):
Console.info("Found camera.yaml file in /config folder")
camera_yaml_path = camera_yaml_config_path
elif camera_yaml_raw_path.exists() and camera_yaml_config_path.exists():
Console.info("Found camera.yaml both in /raw and in /config folder")
Console.info("Using camera.yaml from /config folder")
camera_yaml_path = camera_yaml_config_path
else:
Console.quit(
"rosbag image type requires a camera.yaml file in /config folder",
"Please provide camera.yaml file in /config folder",
)
# instantiate the camera system and setup cameras from mission and
# config files / auv_nav
camera_system = CameraSystem(camera_yaml_path, path_raw_folder)
if camera_system.camera_system != mission.image.format:
Console.quit(
"Image system in camera.yaml does not match with mission.yaml",
"Provide correct camera.yaml in /raw folder",
)
# check for correct_config yaml path
path_correct_images = None
if suffix == "" or suffix is None:
path_correct_images = path_config_folder / "correct_images.yaml"
else:
path_correct_images = path_config_folder / ("correct_images_" +
suffix + ".yaml")
if path_correct_images.exists():
Console.info(
"Configuration file correct_images.yaml file found at",
path_correct_images,
)
else:
default_file_path_correct_config.copy(path_correct_images)
Console.warn(
"Configuration file not found, copying a default one at",
path_correct_images,
)
# load parameters from correct_config.yaml
correct_config = CorrectConfig(path_correct_images)
return correct_config, camera_system
def compute_distance_bin(
self,
idxs,
idx_bin,
images_map,
distances_map,
max_bin_size,
max_bin_size_gb,
distance_vector,
):
dimensions = [self.image_height, self.image_width, self.image_channels]
tmp_idxs = np.where(idxs == idx_bin)[0]
if len(tmp_idxs) > 2:
bin_images = [self.camera_image_list[i] for i in tmp_idxs]
bin_distances_sample = None
bin_images_sample = None
# Random sample if memmap has to be created
if self.smoothing != "mean" and len(bin_images) > max_bin_size:
Console.info(
"Random sampling altitude bin to fit in", max_bin_size_gb, "Gb",
)
bin_images = random.sample(bin_images, max_bin_size)
if not self.depth_map_list:
# Generate matrices on the fly
distance_bin = distance_vector[tmp_idxs]
distance_bin_sample = distance_bin.mean()
if distance_bin_sample <= 0 or np.isnan(distance_bin_sample):
Console.warn("The mean distance is equal or lower than zero!")
Console.warn("Printing the entire vector:", distance_bin)
Console.warn("Printing the mean:", distance_bin_sample)
distance_bin_sample = self.altitude_min + self.bin_band * idx_bin
bin_distances_sample = np.empty((self.image_height, self.image_width))
bin_distances_sample.fill(distance_bin_sample)
else:
bin_distances = [self.depth_map_list[i] for i in tmp_idxs]
bin_distances_sample = running_mean_std(
bin_distances, loader=self.loader
)[0]
if self.smoothing == "mean":
bin_images_sample = running_mean_std(bin_images, loader=self.loader)[0]
elif self.smoothing == "mean_trimmed":
memmap_filename, memmap_handle = create_memmap(
bin_images, dimensions, loader=self.loader,
)
bin_images_sample = image_mean_std_trimmed(memmap_handle)[0]
del memmap_handle
os.remove(memmap_filename)
elif self.smoothing == "median":
memmap_filename, memmap_handle = create_memmap(
bin_images, dimensions, loader=self.loader,
)
bin_images_sample = median_array(memmap_handle)
del memmap_handle
os.remove(memmap_filename)
base_path = Path(self.attenuation_parameters_folder)
fig = plt.figure()
plt.imshow(bin_images_sample)
plt.colorbar()
plt.title("Image bin " + str(idx_bin))
fig_name = base_path / (
"bin_images_sample_" + str(distance_bin_sample) + "m.png"
)
# Console.info("Saved figure at", fig_name)
plt.savefig(fig_name, dpi=600)
plt.close(fig)
fig = plt.figure()
plt.imshow(bin_distances_sample)
plt.colorbar()
plt.title("Distance bin " + str(idx_bin))
fig_name = base_path / (
"bin_distances_sample_" + str(distance_bin_sample) + "m.png"
)
# Console.info("Saved figure at", fig_name)
plt.savefig(fig_name, dpi=600)
plt.close(fig)
images_map[idx_bin] = bin_images_sample.reshape(
[self.image_height * self.image_width, self.image_channels]
)
distances_map[idx_bin] = bin_distances_sample.reshape(
[self.image_height * self.image_width]
)
def load_configuration(self, correct_config=None):
if correct_config is None: # nothing to do here, we expect an explicit call
Console.warn("No correct_config provided. Skipping load_configuration()")
return
self.correct_config = correct_config
"""Load general configuration parameters"""
self.correction_method = self.correct_config.method
if self.correction_method == "colour_correction":
self.distance_metric = self.correct_config.color_correction.distance_metric
self.distance_path = self.correct_config.color_correction.metric_path
self.altitude_max = self.correct_config.color_correction.altitude_max
self.altitude_min = self.correct_config.color_correction.altitude_min
self.smoothing = self.correct_config.color_correction.smoothing
self.window_size = self.correct_config.color_correction.window_size
self.outlier_rejection = self.correct_config.color_correction.outlier_reject
self.cameraconfigs = self.correct_config.configs.camera_configs
self.undistort = self.correct_config.output_settings.undistort_flag
self.output_format = self.correct_config.output_settings.compression_parameter
# Load camera parameters
cam_idx = self.get_camera_idx()
self.camera_found = False
if cam_idx is None:
Console.info(
"Camera not included in correct_images.yaml. No",
"processing will be done for this camera.",
)
return
else:
self.camera_found = True
self.user_specified_image_list_parse = self.cameraconfigs[
cam_idx
].imagefilelist_parse
self.user_specified_image_list_process = self.cameraconfigs[
cam_idx
].imagefilelist_process
if self.correction_method == "colour_correction":
# Brighness and contrast are percentages of 255
# e.g. brightness of 30 means 30% of 255 = 77
self.brightness = float(self.cameraconfigs[cam_idx].brightness)
self.contrast = float(self.cameraconfigs[cam_idx].contrast)
elif self.correction_method == "manual_balance":
self.subtractors_rgb = np.array(self.cameraconfigs[cam_idx].subtractors_rgb)
self.color_gain_matrix_rgb = np.array(
self.cameraconfigs[cam_idx].color_gain_matrix_rgb
)
# Create output directories and needed attributes
self.create_output_directories()
# Define basic filepaths
if self.correction_method == "colour_correction":
p = Path(self.attenuation_parameters_folder)
self.attenuation_params_filepath = p / "attenuation_parameters.npy"
self.correction_gains_filepath = p / "correction_gains.npy"
self.corrected_mean_filepath = p / "image_corrected_mean.npy"
self.corrected_std_filepath = p / "image_corrected_std.npy"
self.raw_mean_filepath = p / "image_raw_mean.npy"
self.raw_std_filepath = p / "image_raw_std.npy"
# Define image loader
# Use default loader
self.loader = loader.Loader()
self.loader.bit_depth = self.camera.bit_depth
if self.camera.extension == "raw":
self.loader.set_loader("xviii")
elif self.camera.extension == "bag":
self.loader.set_loader("rosbag")
tz_offset_s = (
read_timezone(self.mission.image.timezone) * 60
+ self.mission.image.timeoffset
)
self.loader.tz_offset_s = tz_offset_s
else:
self.loader.set_loader("default")
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 rescale_camera(path, camera_system, camera):
name = camera.camera_name
distance_path = camera.distance_path
interpolate_method = camera.interpolate_method
image_path = camera.path
target_pixel_size = camera.target_pixel_size
maintain_pixels = bool(camera.maintain_pixels)
output_folder = camera.output_folder
idx = [
i for i, camera in enumerate(camera_system.cameras)
if camera.name == name
]
if len(idx) > 0:
Console.info("Camera found in camera.yaml file...")
else:
Console.warn(
"Camera not found in camera.yaml file. Please provide a relevant \
camera.yaml file...")
return False
# obtain images to be rescaled
path_processed = get_processed_folder(path)
image_path = path_processed / image_path
# obtain path to distance / altitude values
full_distance_path = path_processed / distance_path
full_distance_path = full_distance_path / "csv" / "ekf"
distance_file = "auv_ekf_" + name + ".csv"
distance_path = full_distance_path / distance_file
# obtain focal lengths from calibration file
camera_params_folder = path_processed / "calibration"
camera_params_filename = "mono_" + name + ".yaml"
camera_params_file_path = camera_params_folder / camera_params_filename
if not camera_params_file_path.exists():
Console.quit("Calibration file not found...")
else:
Console.info("Calibration file found...")
monocam = MonoCamera(camera_params_file_path)
focal_length_x = monocam.K[0, 0]
focal_length_y = monocam.K[1, 1]
# create output path
output_directory = path_processed / output_folder
if not output_directory.exists():
output_directory.mkdir(parents=True)
# call rescale function
dataframe = pd.read_csv(Path(distance_path))
imagenames_list = [
filename for filename in os.listdir(image_path)
if filename[-4:] == ".jpg" or filename[-4:] == ".png"
or filename[-4:] == ".tif"
]
Console.info("Distance values loaded...")
rescale_images(
imagenames_list,
image_path,
interpolate_method,
target_pixel_size,
dataframe,
output_directory,
focal_length_x,
focal_length_y,
maintain_pixels,
)
return True
