occupancy_grid_to_ndt.py
Go to the documentation of this file.
1 #!/usr/bin/env python3
2 
3 # Copyright 2024 Ekumen, Inc.
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 
17 """This script allows conversions from ROS occupancy grid maps to NDT maps."""
18 import argparse
19 from pathlib import Path
20 
21 from beluga_ros.conversion_utils import (
22  grid_to_point_cloud,
23  OccupancyGrid,
24  point_cloud_to_ndt,
25  NDTMap,
26 )
27 
28 import matplotlib.pyplot as plt
29 
30 
31 def _parse_args() -> argparse.Namespace:
32  arg_parser = argparse.ArgumentParser(usage=__doc__)
33  arg_parser.add_argument(
34  '--input',
35  '-i',
36  type=Path,
37  help="Path to the map's .yaml file.",
38  required=True,
39  )
40  arg_parser.add_argument(
41  '--output_dir',
42  '-o',
43  type=Path,
44  help="Path to the directory where outputs will be dumped to. \
45  Will be created if it doesn't exist.",
46  required=True,
47  )
48  arg_parser.add_argument(
49  '--cell_size',
50  '-c',
51  type=float,
52  default=1.0,
53  help="Cell side length in meters of the final NDT map.",
54  )
55  return arg_parser.parse_args()
56 
57 
58 def main():
59  """Script entrypoint."""
60  args = _parse_args()
61 
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)
66  pc = grid_to_point_cloud(grid)
67 
68  print(
69  f"Extracted a pointcloud of {pc.shape[1]} points from the occupancy grid... \n"
70  )
71  ndt = point_cloud_to_ndt(pc, args.cell_size)
72  print(
73  f"Constructed a NDT representation of the point cloud with {len(ndt.grid)} cells.\n"
74  )
75 
76  ndt.plot()
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)
81 
82  print(f"Saved output artifacts to {out_dir.absolute()}\n\n")
83  print(
84  f"Loading the map from its serialized form ({output_hdf5_name}) "
85  "and verifying its integrity...\n"
86  )
87  assert ndt.is_close(
88  NDTMap.from_hdf5(output_hdf5_name)
89  ), "Reading the NDT map from disk produced a different map from the serialized one,\
90  this is a bug."
91 
92  print("Integrity check OK!")
93 
94 
95 if __name__ == "__main__":
96  main()
beluga_ros.conversion_utils.grid_to_point_cloud
np.ndarray grid_to_point_cloud(OccupancyGrid occupancy_grid)
Definition: conversion_utils.py:225
beluga_ros.conversion_utils
Definition: conversion_utils.py:1
beluga_ros.conversion_utils.point_cloud_to_ndt
NDTMap point_cloud_to_ndt(np.ndarray pc, cell_size=1.0)
Definition: conversion_utils.py:269
beluga_ros.occupancy_grid_to_ndt.main
def main()
Definition: occupancy_grid_to_ndt.py:58
beluga_ros.occupancy_grid_to_ndt._parse_args
argparse.Namespace _parse_args()
Definition: occupancy_grid_to_ndt.py:31


beluga_ros
Author(s):
autogenerated on Tue Jul 16 2024 03:00:02