def __init__(self, stereo_camera_model, config, overwrite=False): self.data = [] self.sc = stereo_camera_model self.camera_name = self.sc.left.name self.config = config self.overwrite = overwrite if config.get("filter") is not None: Console.quit( "The node 'filter' is no longer used in calibration.yaml. " "'stratify' is used instead." ) detection = config.get("detection", {}) stratify = config.get("stratify", {}) ransac = config.get("ransac", {}) # uncertainty_generation = config.get("uncertainty_generation", {}) self.k = detection.get("window_size", 5) self.min_greenness_ratio = detection.get("min_greenness_ratio", 0.01) self.num_columns = detection.get("num_columns", 1024) self.remap = detection.get("remap", True) self.start_row = detection.get("start_row", 0) self.end_row = detection.get("end_row", -1) self.start_row_b = detection.get("start_row_b", 0) self.end_row_b = detection.get("end_row_b", -1) self.two_lasers = detection.get("two_lasers", False) self.min_z_m = stratify.get("min_z_m", 1) self.max_z_m = stratify.get("max_z_m", 20) self.number_of_bins = stratify.get("number_of_bins", 5) self.max_points_per_bin = stratify.get("max_bin_elements", 300) self.max_point_cloud_size = ransac.get("max_cloud_size", 10000) self.mdt = ransac.get("min_distance_threshold", 0.002) self.ssp = ransac.get("sample_size_ratio", 0.8) self.gip = ransac.get("goal_inliers_ratio", 0.999) self.max_iterations = ransac.get("max_iterations", 5000) self.left_maps = cv2.initUndistortRectifyMap( self.sc.left.K, self.sc.left.d, self.sc.left.R, self.sc.left.P, (self.sc.left.image_width, self.sc.left.image_height), cv2.CV_32FC1, ) self.right_maps = cv2.initUndistortRectifyMap( self.sc.right.K, self.sc.right.d, self.sc.right.R, self.sc.right.P, (self.sc.right.image_width, self.sc.right.image_height), cv2.CV_32FC1, ) self.inliers_1 = None self.triples = [] self.uncertainty_planes = [] self.in_front_or_behind = []
def call_rescale(args): path = Path(args.path).resolve() time_string = time.strftime("%Y%m%d_%H%M%S", time.localtime()) Console.set_logging_file( get_processed_folder(path) / ("log/" + time_string + "_correct_images_rescale.log")) correct_config, camerasystem = load_configuration_and_camera_system( path, args.suffix) # install freeimage plugins if not installed imageio.plugins.freeimage.download() if correct_config.camerarescale is None: Console.error("Camera rescale configuration not found") Console.error( "Please populate the correct_images.yaml file with a rescale configuration" ) Console.quit("Malformed correct_images.yaml file") # obtain parameters for rescale from correct_config rescale_cameras = correct_config.camerarescale.rescale_cameras for camera in rescale_cameras: corrections.rescale_camera(path, camerasystem, camera) Console.info("Rescaling completed for all cameras ...")
def get_altitude_and_depth_maps(self): """Generate distance matrix numpy files and save them""" # read altitude / depth map depending on distance_metric if self.distance_metric == "uniform": Console.info("Null distance matrix created") self.depth_map_list = [] return elif self.distance_metric == "altitude": Console.info("Null distance matrix created") self.depth_map_list = [] return elif self.distance_metric == "depth_map": path_depth = self.path_processed / "depth_map" if not path_depth.exists(): Console.quit("Depth maps not found...") else: Console.info("Path to depth maps found...") depth_map_list = list(path_depth.glob("*.npy")) self.depth_map_list = [ Path(item) for item in depth_map_list for image_path in self.camera_image_list if Path(image_path).stem in Path(item).stem ] if len(self.camera_image_list) != len(self.depth_map_list): Console.quit( "The number of images does not coincide with the ", "number of depth maps.", )
def parse_interlacer(outpath, filename): data = array("f") value = [] data_ordered = [] filepath = outpath / filename try: with filepath.open("r") as json_file: data = json.load(json_file) for i, data_packet in enumerate(data): value.append(str(float(data_packet["epoch_timestamp"]))) except ValueError: Console.quit("Error: no data in JSON file") # sort data in order of epoch_timestamp sorted_index, sorted_items = sort_values(value) # store interlaced data in order of time for i in range(len(data)): data_ordered.append((data[sorted_index[i]])) # write out interlaced json file with filepath.open("w") as fileout: json.dump(data_ordered, fileout, indent=2) fileout.close()
def memmap_loader( image_list, memmap_handle, idx, loader=default.loader, new_width=None, new_height=None, ): if image_list[idx] is None: Console.error("Image at idx", idx, "is None") Console.error( "Please check your navigation CSV for any missing values") Console.quit("Image list is malformed") np_im = loader(image_list[idx]).astype(np.float32) dimensions = np_im.shape if new_height is not None and new_width is not None: same_dimensions = (new_width == dimensions[1]) and (new_height == dimensions[0]) if not same_dimensions: im2 = cv2.resize(np_im, (new_width, new_height), cv2.INTER_CUBIC) memmap_handle[idx, ...] = im2 else: memmap_handle[idx, ...] = np_im else: memmap_handle[idx, ...] = np_im
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 debayer(image: np.ndarray, pattern: str) -> np.ndarray: """Perform debayering of input image Parameters ----------- image : numpy.ndarray image data to be debayered pattern : string bayer pattern Returns ------- numpy.ndarray Debayered image """ # Make use of 16 bit debayering image16_float = image.astype(np.float32) * (2**16 - 1) image16 = image16_float.clip(0, 2**16 - 1).astype(np.uint16) corrected_rgb_img = None if pattern == "rggb" or pattern == "RGGB": corrected_rgb_img = cv2.cvtColor(image16, cv2.COLOR_BAYER_BG2RGB_EA) elif pattern == "grbg" or pattern == "GRBG": corrected_rgb_img = cv2.cvtColor(image16, cv2.COLOR_BAYER_GB2RGB_EA) elif pattern == "bggr" or pattern == "BGGR": corrected_rgb_img = cv2.cvtColor(image16, cv2.COLOR_BAYER_RG2RGB_EA) elif pattern == "gbrg" or pattern == "GBRG": corrected_rgb_img = cv2.cvtColor(image16, cv2.COLOR_BAYER_GR2RGB_EA) else: Console.quit("Bayer pattern not supported (", pattern, ")") # Scale down to unitary corrected_rgb_img = corrected_rgb_img.astype(np.float32) * (2**(-16)) return corrected_rgb_img
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 set_loader(self, loader_name): if loader_name == "xviii": self._loader = xviii.loader self._loader_name = "xviii" elif loader_name == "default": self._loader = default.loader self._loader_name = "default" elif loader_name == "rosbag": self._loader = rosbag.loader self._loader_name = "rosbag" else: Console.quit("The loader type", loader_name, "is not implemented.") Console.info("Loader set to", loader_name)
def check_pattern(config): if config["pattern"] == "Circles" or config["pattern"] == "circles": pattern = Patterns.Circles elif config["pattern"] == "ACircles" or config["pattern"] == "acircles": pattern = Patterns.ACircles elif config["pattern"] == "Chessboard" or config["pattern"] == "chessboard": pattern = Patterns.Chessboard else: Console.quit( "The available patterns are: Circles, Chessboard or ACircles.", "Please check you did not misspell the pattern type.", ) return pattern
def __call__(self, img_file): if self.bit_depth is not None: if self._loader_name == "rosbag": return self._loader( img_file, self.topic, self.bagfile_list, self.tz_offset_s, self.bit_depth, ) else: return self._loader(img_file, src_bit=self.bit_depth) else: Console.quit("Set the bit_depth in the loader first.")
def __init__(self, filename=None): """__init__ is the constructor function Parameters ----------- filename : Path path to the correct_images.yaml file """ if filename is None: return with filename.open("r") as stream: data = yaml.safe_load(stream) self.color_correction = None self.camerarescale = None if "version" not in data: Console.error( "It seems you are using an old correct_images.yaml.", "You will have to delete it and run this software again.", ) Console.error("Delete the file with:") Console.error(" rm ", filename) Console.quit("Wrong correct_images.yaml format") self.version = data["version"] valid_methods = ["manual_balance", "colour_correction"] self.method = data["method"] if self.method not in valid_methods: Console.quit( "The method requested (", self.method, ") is not supported or implemented. The valid methods", "are:", " ".join(m for m in valid_methods), ) if "colour_correction" in data: self.color_correction = ColourCorrection(data["colour_correction"]) node = data["cameras"] self.configs = CameraConfigs(node) node = data["output_settings"] self.output_settings = OutputSettings(node) if "rescale" in data: self.camerarescale = CameraRescale(data["rescale"])
def find_navigation_csv(base, distance_path="json_renav_*", solution="ekf", camera="Cam51707923"): base_processed = get_processed_folder(base) json_list = list(base_processed.glob(distance_path)) if len(json_list) == 0: Console.quit("No navigation solution could be found at", base_processed) nav_csv_filepath = json_list[0] / ("csv/" + solution) nav_csv_filepath = nav_csv_filepath / ( "auv_" + solution + "_" + camera + ".csv") # noqa if not nav_csv_filepath.exists(): Console.quit("No navigation solution could be found at", nav_csv_filepath) return nav_csv_filepath
def calibrate_mono( name, filepaths, extension, config, output_file, fo, foa, target_width=None, target_height=None, ): if not check_dirs_exist(filepaths): filepaths = get_raw_folders(filepaths) Console.info("Looking for {} calibration images in {}".format( extension, str(filepaths))) image_list = collect_image_files(filepaths, extension) Console.info("Found " + str(len(image_list)) + " images.") if len(image_list) < 8: Console.quit("Too few images. Try to get more.") mc = MonoCalibrator( boards=[ ChessboardInfo(config["rows"], config["cols"], config["size"]) ], pattern=check_pattern(config), invert=config["invert"], name=name, target_width=target_width, target_height=target_height, ) try: image_list_file = output_file.with_suffix(".json") if foa or not image_list_file.exists(): mc.cal(image_list) with image_list_file.open("w") as f: Console.info("Writing JSON to " "'{}'" "".format(image_list_file)) json.dump(mc.json, f) else: mc.cal_from_json(image_list_file, image_list) mc.report() Console.info("Writing calibration to " "'{}'" "".format(output_file)) with output_file.open("w") as f: f.write(mc.yaml()) except CalibrationException: Console.error("The calibration pattern was not found in the images.")
def merge_json_files(json_file_list): # Check that all origins are the same origin_lat = None origin_lon = None data_list = [] for fn in json_file_list: filepath = Path(fn) data = [] with filepath.open("r") as json_file: data = json.load(json_file) # Origins are by default at the top of the json file lat = data[0]["data"][0]["latitude"] lon = data[0]["data"][0]["longitude"] if origin_lat is None: origin_lat = lat origin_lon = lon data_list.append(data[0]) elif origin_lat != lat or origin_lon != lon: Console.error( "The datasets you want to merge do not belong to the same", "origin.", ) Console.error( "Change the origins to be identical and parse them again.") Console.quit("Invalid origins for merging datasets.") # Get dive name # json_file_list = .../DIVE_NAME/nav/nav_standard.json dive_prefix = filepath.parents[1].name + "/" # Preceed all filenames with the dive name # And skip the origin for item in data[1:]: if "camera1" in item: item["camera1"][0]["filename"] = ( dive_prefix + item["camera1"][0]["filename"]) if "camera2" in item: item["camera2"][0]["filename"] = ( dive_prefix + item["camera2"][0]["filename"]) if "camera3" in item: item["camera3"][0]["filename"] = ( dive_prefix + item["camera3"][0]["filename"]) if item["category"] == "laser": item["filename"] = dive_prefix + item["filename"] data_list.append(item) return data_list
def laser(self): if "laser_calibration" not in self.calibration_config["cameras"][0]: Console.quit( 'There is no field "laser_calibration" for the first', "camera in the calibration.yaml", ) if "laser_calibration" not in self.calibration_config["cameras"][1]: Console.quit( 'There is no field "laser_calibration" for the second', "camera in the calibration.yaml", ) c0 = self.calibration_config["cameras"][0] c1 = self.calibration_config["cameras"][1] self.laser_imp(c1, c0) if len(self.calibration_config["cameras"]) > 2: c1 = self.calibration_config["cameras"][2] self.laser_imp(c0, c1)
def find_image_path( base, correct_images="attenuation_correction/developed_*", correction="altitude_corrected/m30_std10", ): correct_images_list = list(base.glob(correct_images)) if len(correct_images_list) == 0: Console.quit("No correct_images solution could be found at", base) image_folder = correct_images_list[0] / correction if not image_folder.exists(): Console.quit( "No correct_images solution could \ be found at", image_folder, ) return image_folder
def rosbag_topic_worker(bagfile_list, wanted_topic, data_object, data_list, output_format, output_dir): """Process a topic from a rosbag calling a method from an object Parameters ---------- bagfile_list : list list of paths to rosbags wanted_topic : str Wanted topic data_object : object Object that has the data_method implemented data_method : std Name of the method to call (e.g. data_object.data_method(msg)) data_list : list List of data output output_format : str Output format output_dir: str Output directory Returns ------- list Processed data list """ if wanted_topic is None: Console.quit("data_method for bagfile is not specified for topic", wanted_topic) for bagfile in bagfile_list: bag = rosbag.Bag(bagfile, "r") for topic, msg, _ in bag.read_messages(topics=[wanted_topic]): if topic == wanted_topic: func = getattr(data_object, "from_ros") type_str = str(type(msg)) # rosbag library does not store a clean message type, # so we need to make it ourselves from a dirty string msg_type = type_str.split(".")[1][1:-2].replace("__", "/") func(msg, msg_type, output_dir) if data_object.valid(): data = data_object.export(output_format) data_list.append(data) return data_list
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 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 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 acfr_to_oplab(args): csv_filepath = Path(args.output_folder) force_overwite = args.force if not args.vehicle_pose and not args.stereo_pose: Console.error("Please provide at least one file") Console.quit("Missing comandline arguments") if args.vehicle_pose: Console.info("Vehicle pose provided! Converting it to DR CSV...") vpp = AcfrVehiclePoseParser(args.vehicle_pose) dr = vpp.get_dead_reckoning() csv_filename = "auv_acfr_centre.csv" if (csv_filepath / csv_filename).exists() and not force_overwite: Console.error( "The DR file already exists. Use -F to force overwrite it") Console.quit("Default behaviour: not to overwrite results") else: write_csv(csv_filepath, dr, csv_filename, csv_flag=True) Console.info("Output vehicle pose written to", csv_filepath / csv_filename) if args.stereo_pose: Console.info("Stereo pose provided! Converting it to camera CSVs...") spp = AcfrStereoPoseParser(args.stereo_pose) cam1, cam2 = spp.get_cameras() if (csv_filepath / "auv_dr_LC.csv").exists() and not force_overwite: Console.error( "The camera files already exists. Use -F to force overwrite it" ) Console.quit("Default behaviour: not to overwrite results") else: write_csv(csv_filepath, cam1, "auv_acfr_LC", csv_flag=True) write_csv(csv_filepath, cam2, "auv_acfr_RC", csv_flag=True) Console.info("Output camera files written at", csv_filepath) if args.dive_folder is None: return # If the user provides a dive path, we can interpolate the ACFR navigation # to the laser timestamps path_processed = get_processed_folder(args.dive_folder) nav_standard_file = path_processed / "nav" / "nav_standard.json" nav_standard_file = get_processed_folder(nav_standard_file) Console.info("Loading json file {}".format(nav_standard_file)) start_datetime = "" finish_datetime = "" file3 = csv_filepath / "auv_acfr_laser.csv" if file3.exists() and not force_overwite: Console.error( "The laser file already exists. Use -F to force overwrite it") Console.quit("Default behaviour: not to overwrite results") with nav_standard_file.open("r") as nav_standard: parsed_json_data = json.load(nav_standard) if start_datetime == "": epoch_start_time = epoch_from_json(parsed_json_data[1]) start_datetime = epoch_to_datetime(epoch_start_time) else: epoch_start_time = string_to_epoch(start_datetime) if finish_datetime == "": epoch_finish_time = epoch_from_json(parsed_json_data[-1]) finish_datetime = epoch_to_datetime(epoch_finish_time) else: epoch_finish_time = string_to_epoch(finish_datetime) Console.info("Interpolating laser to ACFR stereo pose data...") fileout3 = file3.open( "w" ) # ToDo: Check if file exists and only overwrite if told ('-F') fileout3.write(cam1[0].get_csv_header()) for i in range(len(parsed_json_data)): Console.progress(i, len(parsed_json_data)) epoch_timestamp = parsed_json_data[i]["epoch_timestamp"] if (epoch_timestamp >= epoch_start_time and epoch_timestamp <= epoch_finish_time): if "laser" in parsed_json_data[i]["category"]: filename = parsed_json_data[i]["filename"] c3_interp = interpolate_camera(epoch_timestamp, cam1, filename) fileout3.write(c3_interp.to_csv_row()) Console.info("Done! Laser file available at", str(file3))
def hybis_to_oplab(args): if args.navigation_file is None: Console.error("Please provide a navigation file") Console.quit("Missing comandline arguments") if args.image_path is None: Console.error("Please provide an image path") Console.quit("Missing comandline arguments") if args.reference_lat is None: Console.error("Please provide a reference_lat") Console.quit("Missing comandline arguments") if args.reference_lon is None: Console.error("Please provide a reference_lon") Console.quit("Missing comandline arguments") if args.output_folder is None: Console.error("Please provide an output path") Console.quit("Missing comandline arguments") parser = HybisParser( args.navigation_file, args.image_path, args.reference_lat, args.reference_lon, ) force_overwite = args.force filepath = get_processed_folder(args.output_folder) start_datetime = epoch_to_datetime(parser.start_epoch) finish_datetime = epoch_to_datetime(parser.finish_epoch) # make path for processed outputs json_filename = ("json_renav_" + start_datetime[0:8] + "_" + start_datetime[8:14] + "_" + finish_datetime[0:8] + "_" + finish_datetime[8:14]) renavpath = filepath / json_filename if not renavpath.is_dir(): try: renavpath.mkdir() except Exception as e: print("Warning:", e) elif renavpath.is_dir() and not force_overwite: # Check if dataset has already been processed Console.error( "It looks like this dataset has already been processed for the", "specified time span.", ) Console.error( "The following directory already exist: {}".format(renavpath)) Console.error( "To overwrite the contents of this directory rerun auv_nav with", "the flag -F.", ) Console.error("Example: auv_nav process -F PATH") Console.quit("auv_nav process would overwrite json_renav files") output_dr_centre_path = renavpath / "csv" / "dead_reckoning" if not output_dr_centre_path.exists(): output_dr_centre_path.mkdir(parents=True) camera_name = "hybis_camera" output_dr_centre_path = output_dr_centre_path / ("auv_dr_" + camera_name + ".csv") parser.write(output_dr_centre_path) parser.write(output_dr_centre_path) Console.info("Output written to", output_dr_centre_path)
def oplab_to_acfr(args): if not args.dive_folder: Console.error("No dive folder provided. Exiting...") Console.quit("Missing comandline arguments") if not args.output_folder: Console.error("No output folder provided. Exiting...") Console.quit("Missing comandline arguments") output_folder = get_processed_folder(args.output_folder) vf = output_folder / "vehicle_pose_est.data" sf = output_folder / "stereo_pose_est.data" if (vf.exists() or sf.exists()) and not args.force: Console.error( "Output folder already exists. Use -F to overwrite. Exiting...") Console.quit("Default behaviour: not to overwrite results") Console.info("Loading mission.yaml") path_processed = get_processed_folder(args.dive_folder) mission_file = path_processed / "mission.yaml" mission = Mission(mission_file) vehicle_file = path_processed / "vehicle.yaml" vehicle = Vehicle(vehicle_file) origin_lat = mission.origin.latitude origin_lon = mission.origin.longitude json_list = list(path_processed.glob("json_*")) if len(json_list) == 0: Console.quit( "No navigation solution could be found. Please run ", "auv_nav parse and process first", ) navigation_path = path_processed / json_list[0] # Try if ekf exists: full_navigation_path = navigation_path / "csv" / "ekf" vpw = AcfrVehiclePoseWriter(vf, origin_lat, origin_lon) vehicle_navigation_file = full_navigation_path / "auv_ekf_centre.csv" dataframe = pd.read_csv(vehicle_navigation_file) dr_list = [] for _, row in dataframe.iterrows(): sb = SyncedOrientationBodyVelocity() sb.from_df(row) dr_list.append(sb) vpw.write(dr_list) Console.info("Vehicle pose written to", vf) if not mission.image.cameras: Console.warn("No cameras found in the mission file.") return if len(mission.image.cameras) == 1: Console.error("Only one camera found in the mission file. Exiting...") Console.quit("ACFR stereo pose est requires two cameras.") camera0_name = mission.image.cameras[0].name camera1_name = mission.image.cameras[1].name camera0_nav_file = full_navigation_path / ("auv_ekf_" + camera0_name + ".csv") dataframe = pd.read_csv(camera0_nav_file) cam1_list = [] for _, row in dataframe.iterrows(): sb = Camera() sb.from_df(row) cam1_list.append(sb) camera1_nav_file = full_navigation_path / ("auv_ekf_" + camera1_name + ".csv") dataframe = pd.read_csv(camera1_nav_file) cam2_list = [] for _, row in dataframe.iterrows(): sb = Camera() sb.from_df(row) cam2_list.append(sb) spw = AcfrStereoPoseWriter(sf, origin_lat, origin_lon) spw.write(cam1_list, cam2_list) Console.info("Stereo pose written to", sf) Console.info("Generating combined.RAW") nav_standard_file = path_processed / "nav" / "nav_standard.json" nav_standard_file = get_processed_folder(nav_standard_file) Console.info("Loading json file {}".format(nav_standard_file)) with nav_standard_file.open("r") as nav_standard: parsed_json_data = json.load(nav_standard) start_datetime = "" finish_datetime = "" # setup start and finish date time if start_datetime == "": epoch_start_time = epoch_from_json(parsed_json_data[1]) start_datetime = epoch_to_datetime(epoch_start_time) else: epoch_start_time = string_to_epoch(start_datetime) if finish_datetime == "": epoch_finish_time = epoch_from_json(parsed_json_data[-1]) finish_datetime = epoch_to_datetime(epoch_finish_time) else: epoch_finish_time = string_to_epoch(finish_datetime) sensors_std = { "usbl": { "model": "sensor" }, "dvl": { "model": "sensor" }, "depth": { "model": "sensor" }, "orientation": { "model": "sensor" }, } exporter = AcfrCombinedRawWriter(mission, vehicle, output_folder) # read in data from json file # i here is the number of the data packet for i in range(len(parsed_json_data)): Console.progress(i, len(parsed_json_data)) epoch_timestamp = parsed_json_data[i]["epoch_timestamp"] if epoch_timestamp >= epoch_start_time and epoch_timestamp <= epoch_finish_time: if "velocity" in parsed_json_data[i]["category"]: if "body" in parsed_json_data[i]["frame"]: # to check for corrupted data point which have inertial # frame data values if "epoch_timestamp_dvl" in parsed_json_data[i]: # confirm time stamps of dvl are aligned with main # clock (within a second) if (abs(parsed_json_data[i]["epoch_timestamp"] - parsed_json_data[i]["epoch_timestamp_dvl"]) ) < 1.0: velocity_body = BodyVelocity() velocity_body.from_json(parsed_json_data[i], sensors_std["dvl"]) exporter.add(velocity_body) if "inertial" in parsed_json_data[i]["frame"]: velocity_inertial = InertialVelocity() velocity_inertial.from_json(parsed_json_data[i]) exporter.add(velocity_inertial) if "orientation" in parsed_json_data[i]["category"]: orientation = Orientation() orientation.from_json(parsed_json_data[i], sensors_std["orientation"]) exporter.add(orientation) if "depth" in parsed_json_data[i]["category"]: depth = Depth() depth.from_json(parsed_json_data[i], sensors_std["depth"]) exporter.add(depth) if "altitude" in parsed_json_data[i]["category"]: altitude = Altitude() altitude.from_json(parsed_json_data[i]) exporter.add(altitude) if "usbl" in parsed_json_data[i]["category"]: usbl = Usbl() usbl.from_json(parsed_json_data[i], sensors_std["usbl"]) exporter.add(usbl) if "image" in parsed_json_data[i]["category"]: camera1 = Camera() # LC camera1.from_json(parsed_json_data[i], "camera1") exporter.add(camera1) camera2 = Camera() camera2.from_json(parsed_json_data[i], "camera2") exporter.add(camera2)
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 __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 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 rescale( image_array: np.ndarray, interpolate_method: str, target_pixel_size_m: float, altitude: float, f_x: float, f_y: float, maintain_pixels: bool, ) -> np.ndarray: image_shape = image_array.shape image_height = image_shape[0] image_width = image_shape[1] pixel_height = altitude / f_y pixel_width = altitude / f_x vertical_rescale = pixel_height / target_pixel_size_m horizontal_rescale = pixel_width / target_pixel_size_m method = None if interpolate_method == "bicubic": method = Image.BICUBIC elif interpolate_method == "bilinear": method = Image.BILINEAR elif interpolate_method == "nearest_neighbour": method = Image.NEAREST elif interpolate_method == "lanczos": method = Image.LANCZOS else: Console.error("The requested rescaling method is not implemented.") Console.error("Valid methods are: ") Console.error(" * bicubic") Console.error(" * bilinear") Console.error(" * nearest_neighbour") Console.error(" * lanczos") Console.quit("Rescaling method not implemented.") image_rgb = Image.fromarray(image_array, "RGB") if maintain_pixels: if vertical_rescale < 1 or horizontal_rescale < 1: size = ( int(image_width * horizontal_rescale), int(image_height * vertical_rescale), ) image_rgb = image_rgb.resize(size, resample=method) size = (image_width, image_height) image_rgb = image_rgb.resize(size, resample=method) else: crop_width = int((1 / horizontal_rescale) * image_width) crop_height = int((1 / vertical_rescale) * image_height) # find crop box dimensions box_left = int((image_width - crop_width) / 2) box_upper = int((image_height - crop_height) / 2) box_right = image_width - box_left box_lower = image_height - box_upper # crop the image to the center box = (box_left, box_upper, box_right, box_lower) cropped_image = image_rgb.crop(box) # resize the cropped image to the size of original image size = (image_width, image_height) image_rgb = cropped_image.resize(size, resample=method) else: size = ( int(image_width * horizontal_rescale), int(image_height * vertical_rescale), ) image_rgb = image_rgb.resize(size, resample=method) image = np.array(image_rgb, dtype=np.uint8) return image
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
def parse_ntnu_dvl(mission, vehicle, category, output_format, outpath): # Get your data from a file using mission paths, for example filepath = mission.velocity.filepath log_file_path = get_raw_folder(outpath / ".." / filepath) category_str = None if category == Category.VELOCITY: category_str = "velocity" elif category == Category.ALTITUDE: category_str = "altitude" velocity_std_factor = mission.velocity.std_factor velocity_std_offset = mission.velocity.std_offset heading_offset = vehicle.dvl.yaw body_velocity = BodyVelocity( velocity_std_factor, velocity_std_offset, heading_offset, ) altitude = Altitude(altitude_std_factor=mission.altitude.std_factor) data_list = [] num_entries = 0 num_valid_entries = 0 # For each log file for log_file in log_file_path.glob("*.log"): # Open the log file df = pd.read_csv(log_file, sep="\t", skiprows=(0, 1), names=header_list) if category == Category.VELOCITY: Console.info("... parsing velocity") # For each row in the file for index, row in df.iterrows(): body_velocity.from_ntnu_dvl(str(log_file.name), row) num_entries += 1 if body_velocity.valid(): # DVL provides -32 when no bottom lock data = body_velocity.export(output_format) num_valid_entries += 1 data_list.append(data) elif category == Category.ALTITUDE: Console.info("... parsing altitude") # For each row in the file for index, row in df.iterrows(): num_entries += 1 altitude.from_ntnu_dvl(str(log_file.name), row) if altitude.valid(): num_valid_entries += 1 data = altitude.export(output_format) data_list.append(data) elif category == Category.ORIENTATION: Console.quit("NTNU DVL parser has no ORIENTATION available") elif category == Category.DEPTH: Console.quit("NTNU DVL parser has no DEPTH available") elif category == Category.USBL: Console.quit("NTNU DVL parser has no USBL available") else: Console.quit("NTNU DVL parser has no category", category, "available") Console.info( "... parsed", num_entries, "entries, of which", num_valid_entries, "are valid for category", category_str, ) return data_list