Python example: rbpf_slam.py

Builds an occupancy grid map from a 2D lidar dataset with Rao-Blackwellized particle filter SLAM.

Modules: mrpt.config, mrpt.io, mrpt.obs, mrpt.slam

 1#!/usr/bin/env python3
 2r"""
 3Builds an occupancy grid map from a 2D lidar dataset with Rao-Blackwellized particle filter SLAM.
 4
 5Usage (from the MRPT source tree):
 6
 7  . install/setup.bash
 8  ./mrpt_examples_py/rbpf_slam.py \
 9      -c modules/mrpt_data/config_files/rbpf-slam/gridmapping_optimal_sampling.ini
10
11The rawlog is read from the config file ([MappingApplication] rawlog_file)
12unless one is given as argument. Use --max-steps N to stop early.
13"""
14# mrpt-example: video=XiKTWNOQlcU
15
16import argparse
17import os
18import sys
19
20from mrpt.config import CConfigFile
21from mrpt.io import CCompressedInputStream, archiveFrom
22from mrpt.obs import CRawlog
23from mrpt.slam import CMetricMapBuilderRBPF
24
25DEFAULT_CONFIG = os.path.join(
26    os.path.dirname(os.path.abspath(__file__)),
27    "../modules/mrpt_data/config_files/rbpf-slam/gridmapping_optimal_sampling.ini")
28
29parser = argparse.ArgumentParser()
30parser.add_argument("rawlog", nargs="?", help="Rawlog file (default: the one in the config file)")
31parser.add_argument("-c", "--config", default=DEFAULT_CONFIG, help="Config file (.ini)")
32parser.add_argument("-o", "--output", default="final_map",
33                    help="Prefix of the output map files (default: final_map)")
34parser.add_argument("--max-steps", type=int, default=0,
35                    help="Stop after this many rawlog entries (default: 0 = all)")
36args = parser.parse_args()
37
38config_filename = os.path.abspath(args.config)
39if not os.path.exists(config_filename):
40    sys.exit(f"Error: config file not found: {config_filename}")
41section = "MappingApplication"
42
43rawlog_filename = args.rawlog
44if not rawlog_filename:
45    rawlog_filename = os.path.join(
46        os.path.dirname(config_filename),
47        CConfigFile(config_filename).read_string(section, "rawlog_file", ""))
48if not os.path.exists(rawlog_filename):
49    sys.exit(f"Error: rawlog file not found: {rawlog_filename}")
50
51# RBPF options, including the definition of the maps to build:
52options = CMetricMapBuilderRBPF.TConstructionOptions()
53options.loadFromConfigFileName(config_filename, section)
54
55map_builder = CMetricMapBuilderRBPF(options)
56map_builder.initialize()
57
58# Process the rawlog as a stream of (action, sensory frame) pairs:
59rawlog_arch = archiveFrom(CCompressedInputStream(rawlog_filename))
60entry = 0
61steps = 0
62while True:
63    read_ok, entry, actions, sf, obs = CRawlog.ReadFromArchive(rawlog_arch, entry)
64    if not read_ok:
65        break
66    if actions is None or sf is None:
67        continue  # RBPF-SLAM needs rawlogs in the (action, sensory frame) format
68
69    map_builder.processActionObservation(actions, sf)
70    print(f"Entry {entry}: pose={map_builder.getCurrentPoseEstimation().getMean()}, "
71          f"keyframes={map_builder.getCurrentlyBuiltMapSize()}")
72
73    steps += 1
74    if args.max_steps and steps >= args.max_steps:
75        break
76
77# Save the map of the most likely particle, as a simplemap (keyframes) and in
78# a format for inspection (e.g. an image for gridmaps):
79print(f'Saving final map to: "{args.output}*"')
80if not map_builder.getCurrentlyBuiltMap().saveToFile(args.output + ".simplemap"):
81    sys.exit("Error saving the simplemap")
82map_builder.getCurrentlyBuiltMetricMap().saveMetricMapRepresentationToFile(args.output)