Skip to content

Commit 2604129

Browse files
MunozBanonMunozBanon
committed
refactor: Main file added to loc benchmark
1 parent 888bf99 commit 2604129

File tree

3 files changed

+106
-85
lines changed

3 files changed

+106
-85
lines changed

Diff for: ‎config/loc_benchmark.yaml

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
localisation_benchmark:
2+
# package_dir: "/home/mice85/oxford-lab/labrobotica/algorithms/oxford_spires_dataset"
3+
dataset_folder: "/home/miguelangel/data/Oxford-Spires-Dataset/"
4+
flag_is_all: false # If true it rund all the sequences in the 'dataset_dir'. If false it runs the sequences in 'list_sec'
5+
6+
list_sec: [
7+
"2024-03-12-keble-college-02",
8+
"2024-03-12-keble-college-03",
9+
# "2024-03-12-keble-college-04",
10+
# "2024-03-12-keble-college-05",
11+
# "2024-03-13-observatory-quarter-01",
12+
# "2024-03-13-observatory-quarter-02",
13+
# "2024-03-14-blenheim-palace-01",
14+
# "2024-03-14-blenheim-palace-02",
15+
# "2024-03-14-blenheim-palace-05",
16+
# "2024-03-18-christ-church-01",
17+
# "2024-03-18-christ-church-02",
18+
# "2024-03-18-christ-church-03",
19+
# "2024-03-20-christ-church-05",
20+
# "2024-05-20-bodleian-library-02"
21+
]
22+
23+
run_fast_lio_slam: true

Diff for: ‎scripts/localisation_benchmark/fast_lio_slam.py

+1-85
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,6 @@
99
from evo.core.trajectory import PoseTrajectory3D
1010
from evo.tools import file_interface
1111

12-
sys.path.append("/home/mice85/oxford-lab/labrobotica/algorithms/oxford_spires_dataset")
13-
14-
from oxford_spires_utils.bash_command import run_command
15-
1612

1713
def run_fast_lio_slam (path_to_rosbag, path_to_output):
1814

@@ -94,84 +90,4 @@ def convert_to_tum (path_to_output, path_to_sec):
9490

9591
print("PROCESS FINISHED!!")
9692
print("Output in: {}/output_slam/fast_lio_tum.txt".format(path_to_sec))
97-
print("*********************************************************")
98-
99-
100-
def eval_fast_lio_slam (path_to_gt, path_to_output, package_dir, path_to_sec, dataset_dir):
101-
102-
path_traj = path_to_sec + "/output_slam" + "/fast_lio_tum.txt"
103-
output = run_command("evo_ape tum {} {} --align --t_max_diff 0.01".format(path_to_gt, path_traj), print_output=False)
104-
105-
rmse = -1
106-
for line in output.stdout:
107-
print(line, end="")
108-
if "rmse" in line:
109-
numbers = re.findall('\d+\.\d+|\d+', line)
110-
rmse = numbers[0]
111-
112-
logging.basicConfig(filename=dataset_dir + "results.log", filemode="a", level=logging.INFO)
113-
logging.info(path_to_sec)
114-
logging.info("APE - RMSE result (Fast_LIO-SLAM): {}".format(rmse))
115-
print("RMSE added to log: {}".format(rmse))
116-
117-
def create_output_folder (path_to_sec):
118-
path_to_output = path_to_sec + "/output_slam" + "/fastlio_raw_output/"
119-
if not os.path.exists(path_to_output):
120-
os.makedirs(path_to_output)
121-
return path_to_output
122-
123-
def get_sec_list (dataset_dir, flag_is_all=True):
124-
if flag_is_all:
125-
list_sec = os.listdir(dataset_dir)
126-
else:
127-
list_sec = [
128-
"2024-03-12-keble-college-02",
129-
"2024-03-12-keble-college-03",
130-
"2024-03-12-keble-college-04",
131-
"2024-03-12-keble-college-05",
132-
"2024-03-13-observatory-quarter-01",
133-
"2024-03-13-observatory-quarter-02",
134-
"2024-03-14-blenheim-palace-01",
135-
"2024-03-14-blenheim-palace-02",
136-
"2024-03-14-blenheim-palace-05",
137-
"2024-03-18-christ-church-01",
138-
"2024-03-18-christ-church-02",
139-
"2024-03-18-christ-church-03",
140-
"2024-03-20-christ-church-05",
141-
"2024-05-20-bodleian-library-02",
142-
"2024-05-20-bodleian-library-03",
143-
"2024-05-20-bodleian-library-04",
144-
"2024-05-20-bodleian-library-05"
145-
]
146-
return list_sec
147-
148-
if __name__ == "__main__":
149-
150-
# -------------------------------------------------------------------------------- #
151-
# TODO: get path from arg an define folders in the future class.
152-
package_dir = "/home/mice85/oxford-lab/labrobotica/algorithms/oxford_spires_dataset"
153-
dataset_dir = "/media/mice85/blackdrive1/oxford_spires_dataset/data/"
154-
flag_is_all = False
155-
# -------------------------------------------------------------------------------- #
156-
157-
list_sec = get_sec_list (dataset_dir, flag_is_all)
158-
159-
print('Total sequence folders: ' + str(len(list_sec)))
160-
161-
# Print list of sequences
162-
for sec in list_sec:
163-
164-
path_to_sec = dataset_dir + sec
165-
path_to_rosbag = "/home/mice85/data/" + sec + "/rosbag/" #path_to_sec + "/rosbag/"
166-
path_to_gt = path_to_sec + "/ground_truth_traj/gt_lidar.txt"
167-
path_to_output = create_output_folder (path_to_sec)
168-
169-
run_fast_lio_slam (path_to_rosbag, path_to_output)
170-
171-
time.sleep(5)
172-
173-
convert_to_tum (path_to_output, path_to_sec)
174-
175-
eval_fast_lio_slam (path_to_gt, path_to_output, package_dir, path_to_sec, dataset_dir)
176-
177-
# break
93+
print("*********************************************************")

Diff for: ‎scripts/localisation_benchmark/main.py

+82
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
import argparse
2+
import os
3+
from pathlib import Path
4+
5+
import yaml
6+
from fast_lio_slam import run_fast_lio_slam, convert_to_tum
7+
8+
# TODO: loggin
9+
# def setup_logging():
10+
11+
class LocalisationBenchmark:
12+
def __init__(self, loc_config):
13+
self.loc_config = loc_config
14+
self.gt_file = "/processed/trajectory/gt-tum.txt"
15+
16+
def process_fast_lio_slam (self):
17+
print("run_fast_lio_slam here")
18+
print("convert_to_tum here")
19+
20+
def evaluation_ape_rmse (path_to_gt, path_traj, dataset_dir, method):
21+
output = run_command("evo_ape tum {} {} --align --t_max_diff 0.01".format(path_to_gt, path_traj), print_output=False)
22+
23+
rmse = -1
24+
for line in output.stdout:
25+
print(line, end="")
26+
if "rmse" in line:
27+
numbers = re.findall('\d+\.\d+|\d+', line)
28+
rmse = numbers[0]
29+
30+
# logging.basicConfig(filename=dataset_dir + "results.log", filemode="a", level=logging.INFO)
31+
# logging.info(path_to_sec)
32+
# logging.info("APE - RMSE result ({}): {}".format(method, rmse))
33+
print("RMSE added to log: {}".format(rmse))
34+
35+
return rmse
36+
37+
def create_output_folder (path_to_sec, method_folder):
38+
path_to_output = path_to_sec + "/output_slam" + method_folder
39+
if not os.path.exists(path_to_output):
40+
os.makedirs(path_to_output)
41+
return path_to_output
42+
43+
def get_sec_list (loc_config):
44+
if loc_config["flag_is_all"]:
45+
list_sec = os.listdir(loc_config["dataset_folder"])
46+
else:
47+
list_sec = loc_config["list_sec"]
48+
return list_sec
49+
50+
def get_args():
51+
parser = argparse.ArgumentParser(description="Localisation Benchmark")
52+
default_loc_config_file = Path(__file__).parent.parent.parent / "config" / "loc_benchmark.yaml"
53+
parser.add_argument(
54+
"--config-file",
55+
type=str,
56+
default=str(default_loc_config_file),
57+
)
58+
return parser.parse_args()
59+
60+
if __name__ == "__main__":
61+
# setup_logging()
62+
loc_config_file = get_args().config_file
63+
# logger.info("Starting Reconstruction Benchmark")
64+
with open(loc_config_file, "r") as f:
65+
loc_config = yaml.safe_load(f)["localisation_benchmark"]
66+
67+
list_sec = get_sec_list(loc_config)
68+
print('Total sequence folders: ' + str(len(list_sec)))
69+
70+
loc_benchmark = LocalisationBenchmark(loc_config)
71+
72+
for sec in list_sec:
73+
74+
path_to_sec = loc_benchmark.loc_config["dataset_folder"] + sec
75+
path_to_gt = path_to_sec + loc_benchmark.gt_file
76+
77+
print("SEQUENCE: " + path_to_sec)
78+
79+
if loc_benchmark.loc_config["run_fast_lio_slam"]:
80+
print("RUNING FAST-LIO-SLAM")
81+
path_to_output = create_output_folder (path_to_sec, "/fastlio_raw_output/")
82+
loc_benchmark.process_fast_lio_slam

0 commit comments

Comments
 (0)