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
