17 """This script allows conversions from ROS occupancy grid maps to NDT maps."""
19 from pathlib
import Path
28 import matplotlib.pyplot
as plt
32 arg_parser = argparse.ArgumentParser(usage=__doc__)
33 arg_parser.add_argument(
37 help=
"Path to the map's .yaml file.",
40 arg_parser.add_argument(
44 help=
"Path to the directory where outputs will be dumped to. \
45 Will be created if it doesn't exist.",
48 arg_parser.add_argument(
53 help=
"Cell side length in meters of the final NDT map.",
55 return arg_parser.parse_args()
59 """Script entrypoint."""
62 yaml_file: Path = args.input
63 out_dir: Path = args.output_dir
64 out_dir.mkdir(parents=
True, exist_ok=
True)
65 grid = OccupancyGrid.load_from_file(args.input)
69 f
"Extracted a pointcloud of {pc.shape[1]} points from the occupancy grid... \n"
73 f
"Constructed a NDT representation of the point cloud with {len(ndt.grid)} cells.\n"
77 output_plot_name = out_dir / f
"{yaml_file.stem}.png"
78 plt.savefig(output_plot_name.absolute())
79 output_hdf5_name = out_dir / f
"{yaml_file.stem}.hdf5"
80 ndt.to_hdf5(output_hdf5_name)
82 print(f
"Saved output artifacts to {out_dir.absolute()}\n\n")
84 f
"Loading the map from its serialized form ({output_hdf5_name}) "
85 "and verifying its integrity...\n"
88 NDTMap.from_hdf5(output_hdf5_name)
89 ),
"Reading the NDT map from disk produced a different map from the serialized one,\
92 print(
"Integrity check OK!")
95 if __name__ ==
"__main__":