-
Notifications
You must be signed in to change notification settings - Fork 25
/
cost_builder.py
53 lines (44 loc) · 1.68 KB
/
cost_builder.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
# Copyright (c) 2023-2024, ETH Zurich (Robotics Systems Lab)
# Author: Pascal Roth
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
# imperative-cost-map
from viplanner.config import CostMapConfig
from viplanner.cost_maps import CostMapPCD, SemCostMap, TsdfCostMap
def main(cfg: CostMapConfig, final_viz: bool = True):
assert any([cfg.semantics, cfg.geometry]), "no cost map type selected"
# create semantic cost map
if cfg.semantics:
print("============ Creating Semantic Map from cloud ===============")
sem_cost_map = SemCostMap(cfg.general, cfg.sem_cost_map, visualize=cfg.visualize)
sem_cost_map.pcd_init()
data, coord = sem_cost_map.create_costmap()
# create tsdf cost map
elif cfg.geometry:
print("============== Creating tsdf Map from cloud =================")
tsdf_cost_map = TsdfCostMap(cfg.general, cfg.tsdf_cost_map)
tsdf_cost_map.ReadPointFromFile()
data, coord = tsdf_cost_map.CreateTSDFMap()
(tsdf_cost_map.VizCloud(tsdf_cost_map.obs_pcd) if cfg.visualize else None)
else:
raise ValueError("no cost map type selected")
# set coords in costmap config
cfg.x_start, cfg.y_start = coord
# construct final cost map as pcd and save parameters
print("======== Generate and Save costmap as Point-Cloud ===========")
cost_mapper = CostMapPCD(
cfg=cfg,
tsdf_array=data[0],
viz_points=data[1],
ground_array=data[2],
load_from_file=False,
)
cost_mapper.SaveTSDFMap()
if final_viz:
cost_mapper.ShowTSDFMap(cost_map=True)
return
if __name__ == "__main__":
cfg = CostMapConfig()
main(cfg)
# EoF