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 main(args=None): """ Main entry point for project. Args: args : list of arguments as if they were input in the command line. Leave it None to use sys.argv. This notation makes it possible to call the module from the command line as well as from a different python module. When called from the command line args defaults to None, and parse_args() defaults to using sys.argv[1:]. When called from a python script or module, pass the arguments as list, e.g. main(["parse", "-h"]). This will populate the args parameter. """ # enable VT100 Escape Sequence for WINDOWS 10 for Console outputs # https://stackoverflow.com/questions/16755142/how-to-make-win32-console-recognize-ansi-vt100-escape-sequences # noqa os.system("") Console.banner() Console.info("Running auv_nav version " + str(Console.get_version())) parser = argparse.ArgumentParser() subparsers = parser.add_subparsers(dest="which", ) """ Subparsers for the 3 targets 'parse', 'convert' and 'process' double-dash arguments (optionally completed with a single-dash, single letter abbreviation) are optional. Arguments without the double-dash prefix are positional arguments and therefore required """ subparser_parse = subparsers.add_parser( "parse", help="Parse raw data and converting it to an intermediate \ dataformat for further processing. Type auv_nav parse -h for help on \ this target.", ) subparser_parse.add_argument( "path", default=".", nargs="+", help="Folderpath where the (raw) input data is. Needs to be a \ subfolder of 'raw' and contain the mission.yaml configuration file.", ) # prefixing the argument with -- means it's optional subparser_parse.add_argument( "-F", "--Force", dest="force", action="store_true", help="Force file \ overwite", ) subparser_parse.add_argument( "--merge", dest="merge", action="store_true", help="Merge multiple dives into a single JSON file. Requires more \ than one dive PATH.", ) subparser_parse.set_defaults(func=call_parse_data) subparser_process = subparsers.add_parser( "process", help="Process and/or convert data. Data needs to be saved in \ the intermediate data format generated using auv_nav.py parse. Type \ auv_nav process -h for help on this target.", ) subparser_process.add_argument( "path", default=".", help="Path to folder where the data to process is. The folder \ has to be generated using auv_nav parse.", ) subparser_process.add_argument( "-F", "--Force", dest="force", action="store_true", help="Force file \ overwite", ) subparser_process.add_argument( "-s", "--start", dest="start_datetime", default="", help="Start date & \ time in YYYYMMDDhhmmss from which data will be processed. If not set, \ start at beginning of dataset.", ) subparser_process.add_argument( "-e", "--end", dest="end_datetime", default="", help="End date & time \ in YYYYMMDDhhmmss up to which data will be processed. If not set \ process to end of dataset.", ) subparser_process.add_argument( "-v", "--verbose", dest="verbose", action="store_true", help="Enable verbose output to terminal", ) subparser_process.add_argument( "-r", "--relative_pose_uncertainty", dest="relative_pose_uncertainty", action="store_true", help="Enable relative_pose_uncertainty", ) subparser_process.add_argument( "--start_image_identifier", dest="start_image_identifier", default=None, help= "Identifier (path) from which onwards states are loaded. Required if relative_pose_uncertainty is True.", ) subparser_process.add_argument( "--end_image_identifier", dest="end_image_identifier", default=None, help= "Identifier (path) up to which states are loaded. Required if relative_pose_uncertainty is True.", ) subparser_process.set_defaults(func=call_process_data) subparser_convert = subparsers.add_parser( "convert", help="Converts data.", ) subparser_convert.set_defaults(func=show_help) # CONVERT subparsers subsubparsers = subparser_convert.add_subparsers(dest="which") # ACFR to OPLAB CSV subparser_oplab_to_acfr = subsubparsers.add_parser( "oplab_to_acfr", help="Converts an already processed dive to ACFR format", ) subparser_oplab_to_acfr.add_argument( "-d", "--dive-folder", dest="dive_folder", help="Input dive path.", ) subparser_oplab_to_acfr.add_argument( "-o", "--output-folder", dest="output_folder", help="Path where results will be written.", ) subparser_oplab_to_acfr.add_argument( "-F", "--Force", dest="force", action="store_true", help="Force file overwite", ) subparser_oplab_to_acfr.set_defaults(func=oplab_to_acfr) # OPLAB to ACFR subparser_acfr_to_oplab = subsubparsers.add_parser( "acfr_to_oplab", help="Converts a VehiclePosEst.data and/or a StereoPosEst.data to \ OPLAB csv format", ) subparser_acfr_to_oplab.add_argument( "--vehicle-pose", dest="vehicle_pose", help="vehicle_pose_est.data filepath.", ) subparser_acfr_to_oplab.add_argument( "--stereo-pose", dest="stereo_pose", help="stereo_pose_est.data filepath.", ) subparser_acfr_to_oplab.add_argument( "-o", "--output-folder", dest="output_folder", help="Path where results will be written.", ) subparser_acfr_to_oplab.add_argument( "-d", "--dive-folder", dest="dive_folder", help="Optional path of an existing processed dive to interpolate \ laser timestamps to ACFR navigation.", ) subparser_acfr_to_oplab.add_argument( "-F", "--Force", dest="force", action="store_true", help="Force file overwite", ) subparser_acfr_to_oplab.set_defaults(func=acfr_to_oplab) # HYBIS to OPLAB CSV subparser_hybis_to_oplab = subsubparsers.add_parser( "hybis_to_oplab", help="Converts a hybis navigation file to oplab CSV format", ) subparser_hybis_to_oplab.add_argument( "-i", "--navigation-file", dest="navigation_file", help="Input navigation file.", ) subparser_hybis_to_oplab.add_argument( "-d", "--image-path", dest="image_path", help="Input image path.", ) subparser_hybis_to_oplab.add_argument( "-o", "--output-folder", dest="output_folder", help="Path where results will be written.", ) subparser_hybis_to_oplab.add_argument( "--reference-lat", dest="reference_lat", help="Reference latitude for northing/easting.", ) subparser_hybis_to_oplab.add_argument( "--reference-lon", dest="reference_lon", help="Reference longitude for northing/easting.", ) subparser_hybis_to_oplab.add_argument( "-F", "--Force", dest="force", action="store_true", help="Force file overwite", ) subparser_hybis_to_oplab.set_defaults(func=hybis_to_oplab) if len(sys.argv) == 1 and args is None: # Show help if no args provided parser.print_help(sys.stderr) sys.exit(1) argcomplete.autocomplete(parser) args = parser.parse_args() args.func(args)
def main(args=None): # enable VT100 Escape Sequence for WINDOWS 10 for Console outputs # https://stackoverflow.com/questions/16755142/how-to-make-win32-console-recognize-ansi-vt100-escape-sequences # noqa os.system("") Console.banner() Console.info("Running correct_images version " + str(Console.get_version())) parser = argparse.ArgumentParser() subparsers = parser.add_subparsers() # subparser correct subparser_correct = subparsers.add_parser( "correct", help= "Correct images for attenuation / distortion / gamma and debayering", # noqa ) subparser_correct.add_argument("path", help="Path to raw directory till dive.") subparser_correct.add_argument( "-F", "--Force", dest="force", action="store_true", help="Force overwrite if correction parameters already exist.", ) subparser_correct.add_argument( "--suffix", dest="suffix", default="", help= "Expected suffix for correct_images configuration and output folders.", ) subparser_correct.set_defaults(func=call_correct) # subparser parse subparser_parse = subparsers.add_parser( "parse", help="Compute the correction parameters") # subparser_parse.add_argument("path", help="Path to raw directory till dive.") subparser_parse.add_argument( "path", # default=".", nargs="+", help="Folderpath where the (raw) input data is. Needs to be a \ subfolder of 'raw' and contain the mission.yaml configuration file.", ) subparser_parse.add_argument( "-F", "--Force", dest="force", action="store_true", help="Force overwrite if correction parameters already exist.", ) subparser_parse.add_argument( "--suffix", dest="suffix", default="", help= "Expected suffix for correct_images configuration and output folders.", ) subparser_parse.set_defaults(func=call_parse) # subparser process subparser_process = subparsers.add_parser("process", help="Process image correction") subparser_process.add_argument("path", help="Path to raw directory till dive.") subparser_process.add_argument( "-F", "--Force", dest="force", action="store_true", help="Force overwrite if correction parameters already exist.", ) subparser_process.add_argument( "--suffix", dest="suffix", default="", help= "Expected suffix for correct_images configuration and output folders.", ) subparser_process.set_defaults(func=call_process) # subparser rescale image subparser_rescale = subparsers.add_parser("rescale", help="Rescale processed images") subparser_rescale.add_argument("path", help="Path to raw folder") subparser_rescale.add_argument( "--suffix", dest="suffix", default="", help= "Expected suffix for correct_images configuration and output folders.", ) subparser_rescale.set_defaults(func=call_rescale) if len(sys.argv) == 1 and args is None: # Show help if no args provided parser.print_help(sys.stderr) elif len(sys.argv) == 2 and not sys.argv[1] == "-h": args = parser.parse_args(["correct", sys.argv[1]]) print(args) if hasattr(args, "func"): args.func(args) else: args = parser.parse_args() # Check suffix is only text, digits, dash and underscores allowed_chars = string.ascii_letters + "-" + "_" + string.digits if all([c in allowed_chars for c in args.suffix]): args.func(args) else: Console.error( "Suffix must only contain letters, digits, dash and underscores" )
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