Semantic Terrain Classification for Off-Road Autonomous Driving代码学习
github:https://github.com/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函数 https://www.runoob.com/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 range
continue
for 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:
continue
assert 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) // 2
key_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).T
tpoints[:, 3] = past["remissions"]
tpoints = tpoints.reshape((-1))
npoints = past["points"].shape[0]
end = start + npoints
assert(npoints == past["labels"].shape[0])
concated_points[4 * start:4 * end] = tpoints
concated_labels[start:end] = past["labels"]
pc_ids[start:end] = i
start = 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位置上标记为free and low-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-traversable
We start off by estimating ground elavation by locally averaging z values
of the traversable points. Then, we compute each points distance to the estimated
ground 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
然后投影,得到图的具体位置坐标
## projection
j_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) * 255
for 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 scan
if 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] = 5
return costmap, key_labels.astype(np.uint32), costmap_pose