Semantic Terrain Classification for Off-Road Autonomous Driving代码学习

github：/JHLee0513/semantic_bevnet

论文：Semantic Terrain Classification for Off-Road Autonomous Driving

数据生成部分

generate_bev_gt\kitti_4class_100x100_center_cfg.yaml 这个文件夹下可以更改标注对不同风险地区的映射，以及costmap的大小

generate_bev_gt\generate_parallel.py生成数据集

make_color_map函数将标签转换为costmap

第一步 得到每次扫描的colormap 对应图1

注 python zip函数 /python/python-func-zip.html 用于将元素打包为一个个元组或解压出来

原文：对于每次扫描，我们将其与过去的t扫描和未来的t扫描进行聚合，并使用步长s构建一个更大的点集。我们将t设置为足够大的数字（例如71），以获得机器人周围大面积的密集可遍历性信息。这些参数可根据车辆速度和激光雷达点的密度进行调整。 对应步骤1

其中参数

sequence_length 为t 过去的t次扫描

stride 步长

for i in range(len(scan_files)):start_idx = i - (sequence_length // 2 * stride)history = []data_idxs = [start_idx + _ * stride for _ in range(sequence_length)]if data_idxs[0] < 0 or data_idxs[-1] >= len(scan_files):# Out of rangecontinuefor data_idx in data_idxs:scan_file = scan_files[data_idx]basename = os.path.splitext(scan_file)[0]scan_path = os.path.join(input_folder, "velodyne", scan_file)label_path = os.path.join(input_folder, "labels", basename + ".label")history.append((scan_path, label_path, poses[data_idx]))history = history[::-1]if len(history) < sequence_length:continueassert len(history) == sequence_length

将所有的信息存储到history中得到一个元数组 存储着

包括colormap cmap，点云文件的路径hist_scan_files，标签的路径hist_label_files和poe的路径hist_poses

将所有的信息传递给job_args

job_args.append({'cfg': cfg,'cmap': cmap,'scan_files': hist_scan_files,'label_files': hist_label_files,'poses': hist_poses,})

gen_costmap这个函数用于生成costmap

async_results = [pool.apply_async(gen_costmap, (job, FLAGS.mem_frac)) for job in job_args]#利用多进程得到cosmap

scan 存储点云x，y，z，f

label存储标签

remissions 存储f

scan_ground_frame存储高度y

信息存储到history中

history.appendleft({"scan": scan.copy(),"scan_ground_frame": scan_ground_frame,"points": points,"labels": labels,"remissions": remissions,"pose": poses[i].copy(),"filename": scan_files[i]})

keyscan表示当前的那一帧 即我们要将所有的转换给这一帧

key_scan_id = len(history) // 2key_scan = history[key_scan_id]

首先移除掉会影响判别的移动的物体 remove_moving_objects

new_history = remove_moving_objects(history,cfg["moving_classes"],key_scan_id)

join_pointclouds 将点云聚合起来

利用左边转换 将上一帧的点云转换到这一帧的坐标下

for i, past in enumerate(history):diff = np.matmul(key_pose_inv, past["pose"])tpoints = np.matmul(diff, past["points"].T).Ttpoints[:, 3] = past["remissions"]tpoints = tpoints.reshape((-1))npoints = past["points"].shape[0]end = start + npointsassert(npoints == past["labels"].shape[0])concated_points[4 * start:4 * end] = tpointsconcated_labels[start:end] = past["labels"]pc_ids[start:end] = istart = end

得到聚合后的点云cat_points，标签cat_labels，以及是第几帧的点pc_ids

cat_points, cat_labels, pc_ids = join_pointclouds(new_history, key_scan["pose"])

然后生成cotmap create_costmap

label利用映射转换为cost_label

利用这个构建map

map_cfg = cfg["costmap"]h = int(np.floor((map_cfg["maxy"] - map_cfg["miny"])/map_cfg["gridh"]))w = int(np.floor((map_cfg["maxx"] - map_cfg["minx"])/map_cfg["gridw"]))

利用potprocess处理

def postprocess(points, labels, cfg):

得到costmap地图

论文原文

点云堆叠和地面高度估计

对于聚合扫描中的每个点，我们进行向下投影，以找到其在可遍历性地图上的位置x，y。因此，地图的每个x、y位置都包含一个点柱。我们通过在地图中每个x，y位置上标记为freeandlow-cost的点的最低z坐标上运行平均滤波核来估计地面高度地图。此高度图用作最终可通过性投影的参考。

其中BinningPostprocess 类为

def process_pc(self, points, preds):''' We assume the input prediction has 4 classes (pred.shape == nx4):+ class 0 and 1 are traversable+ class 2 and 3 are non-traversableWe start off by estimating ground elavation by locally averaging z valuesof the traversable points. Then, we compute each points distance to the estimatedground elavation value and correct the predictions as follows:+ (LOGIC 0) Any point outside the 2d map boundary is classified as unknown.+ (LOGIC 1) Any point above the sky_threshold is classified as sky (a new class).+ (LOGIC 2) Any point in class 2 that is lower than g2_threshold is labeld as class 1.+ (LOGIC 3) Any point in class 3 that it lower than g3_threshold is labeld as class 1.The output is a nx5 dimensional prediction tensor.

这样处理的到分类

cu_labels = postprocess(points, mapped_labels, cfg["postprocessing"])

新的标签存储在cu_labels

转化为tensor数组 mapped_labels

然后投影，得到图的具体位置坐标

## projectionj_inds = np.floor((points[:, 0] - map_cfg["minx"]) / map_cfg["gridw"]).astype(np.int32)i_inds = np.floor((points[:, 1] - map_cfg["miny"]) / map_cfg["gridh"]).astype(np.int32)inrange = (i_inds >= 0) & (i_inds < h) & (j_inds >= 0) & (j_inds < w)i_inds, j_inds, mapped_labels = [x[inrange] for x in [i_inds, j_inds,mapped_labels]]

利用规则填充costmap

# Assume that cost of class j > cost of class i if j > i# Project the points down. Higher-cost classes overwrite low-cost classes.costmap = np.ones((h, w), np.uint8) * 255for l in [0, 1, 2, 3]:hist, _, _ = np.histogram2d(i_inds[mapped_labels == l],j_inds[mapped_labels == l],bins=(h, w),range=((0, h), (0, w)))costmap[hist != 0] = l

得到key_can的costmap

#### extract postprocessed labels only for the key scanif pc_ids is not None and key_scan_id >= 0:key_labels_ = cu_labels[pc_ids[not_void] == key_scan_id]scan_len = (pc_ids == key_scan_id).sum()key_labels = np.full(scan_len, -1)key_labels[not_void[pc_ids == key_scan_id]] = key_labels_.cpu().numpy()# 0,1,2,3,4 are reserved so use 5 for unknown?key_labels[key_labels < 0] = 5return costmap, key_labels.astype(np.uint32), costmap_pose