MATLAB环境下利用单目摄像头和语义分割创建占位栅格

准备写一些关于自动驾驶方面的东西，以前研究过一些，也比较感兴趣。

本文简单讲解如何使用深度学习语义分割估计车辆可行驶区域(free space)，并创建占位栅格（occupancy grid），然后使用占位栅格创建车辆代价图（vehicle costmap），代码运行环境为MATLAB R2018a。

代码如下

https://mianbaoduo.com/o/bread/Y5mXmJxy

可行驶区域估计

可行驶区域估计，即识别环境中车辆在不撞到任何障碍物（如行人、路缘或其他车辆）的情况下行驶的区域。车辆可以使用各种传感器来估计可行驶区域，例如雷达、激光雷达或摄像头。本文重点介绍如何使用语义分割来估计可行驶区域。

在本例中，可以学到

1.使用语义图像分割来估计可行驶区域。

2.使用可行驶区域估计值创建占位栅格。

3.在鸟瞰图上可视化占位栅格。

4.使用占位栅格创建车辆代价图。

5.检查位置是否被占用。

为了方便，本文使用一个预训练的语义分割网络，该网络可以将像素划分为11个不同的类别，包括道路、行人、汽车等。

加载训练好的网络

data = load(pretrainedSegNet);
net = data.net;

估计可行驶区域

网络将返回图像中每个像素的类别，可行驶区域被标记为类别Road的像素。本例中使用的图像是来自CamVid 数据集中图像序列的单个帧。本例中的处理过程可应用于一系列帧，以估计车辆行驶时的可行驶区域。但是，由于本例中使用了非常深的卷积神经网络结构（带有 VGG-16编码器的 SegNet），处理每帧大约需要 1 秒钟。因此，为方便起见，只处理单个帧。

读取图像

I = imread('seq05vd_snap_shot.jpg');

分割图像

[C,scores,allScores] = semanticseg(I,net);

将可行驶区域叠加到图像上

B = labeloverlay(I,C,'IncludedLabels',"Road");

显示可行驶区域和图像

figure
imshow(B)

要理解可行驶区域估计的置信度，显示每个像素Road类别的分数。即使网络将像素分类为Road类别，置信度分数也可能足够低，出于安全原因而忽略该分类。

使用网络对Road的输出分数作为可行驶区域置信度

roadClassIdx = 4;
freeSpaceConfidence = allScores(:,:,roadClassIdx);

显示可行驶区域置信度

figure
imagesc(freeSpaceConfidence)
title('Free Space Confidence Scores')
colorbar

尽管Road像素的初始分割结果显示道路上的大多数像素都已正确分类，但可视化分数能提供更丰富的信息，说明分类器对这些分类的置信度。例如，当靠近汽车的边界时，置信度会降低。

创建鸟瞰图

为了便于生成利于导航的占位栅格，需要将可行驶区域的估计值转换为车辆坐标系。此过程可以通过将可行驶区域估计值转换为鸟瞰图来完成。要创建鸟瞰图，首先定义相机传感器配置，使用函数camvidMonoCameraSensor完成。

为CamVid数据创建单目摄像头

sensor = camvidMonoCameraSensor();

给定相机设置后， birdsEyeView 函数将原始图像转换为鸟瞰视图

定义鸟瞰图变换参数

distAheadOfSensor = 20; % 以米为单位，指定单目摄像头高度
spaceToOneSide    = 3;  % 向右和向左看3米
bottomOffset      = 0;  
outView = [bottomOffset, distAheadOfSensor, -spaceToOneSide, spaceToOneSide];
outImageSize = [NaN, 256]; % 输出图像宽度（以像素为单位）;
birdsEyeConfig = birdsEyeView(sensor,outView,outImageSize);

为图像生成鸟瞰图和可行驶区域置信度

根据CamVid传感器的大小调整图像大小和可行驶区域估计值

imageSize = sensor.Intrinsics.ImageSize;
I = imresize(I,imageSize);
freeSpaceConfidence = imresize(freeSpaceConfidence,imageSize);

将图像和可行驶区域置信度分数转换为鸟瞰图

imageBEV = transformImage(birdsEyeConfig,I);
freeSpaceBEV = transformImage(birdsEyeConfig,freeSpaceConfidence); 

以鸟瞰图显示图像框

figure
imshow(imageBEV)

将图像转换为鸟瞰图，并生成可行驶区域置信度

figure
imagesc(freeSpaceBEV)
title('Free Space Confidence')

由于像素较少，因此离传感器较远的区域更加模糊，因此需要更多的插值。

基于可行驶区域创建占位栅格

占位栅格用于将车辆周围环境表示为车辆坐标中的离散栅格，并用于路径规划。占位栅格中的每个像元（cell ）都有一个值，表示该像元占用的概率。

使用可行驶区域估计值填充占位栅格的过程如下：

1.在车辆坐标中定义占位栅格的尺寸。

2.为每个网格单元生成一组 （X，Y） 点，这些点位于车辆的坐标系中。

3.使用 vehicleToImage vehicleToImage函数将车辆坐标空间 （X，Y） 中的点转换为鸟瞰图像坐标空间 （x，y）。

4. 使用griddedInterpolant函数对 （x，y） 位置的可行驶区域置信度值进行采样。

5. 使用与该网格像元对应的所有 （x，y） 点的平均可行驶区域置信度值填充占位栅格像元。

为简单起见，上面显示的过程在函数
createOccupancyGridFromFreeSpaceEstimate中实现

定义占位栅格的尺寸和分辨率

gridX = distAheadOfSensor;
gridY = 2 * spaceToOneSide;
cellSize = 0.25; % 以米为单位，匹配CamVid传感器使用的单位

根据可行驶区域估计值创建占位栅格

occupancyGrid = createOccupancyGridFromFreeSpaceEstimate(...
    freeSpaceBEV, birdsEyeConfig, gridX, gridY, cellSize);

使用 birdsEyePlot函数可视化占位栅格

创建鸟瞰图

bep = birdsEyePlot('XLimits',[0 distAheadOfSensor],'YLimits', [-5 5]);

将占位栅格添加到鸟瞰图。

hold on
[numCellsY,numCellsX] = size(occupancyGrid);
X = linspace(0, gridX, numCellsX);
Y = linspace(-gridY/2, gridY/2, numCellsY);
h = pcolor(X,Y,occupancyGrid);
title('Occupancy Grid (probability)')
colorbar
delete(legend)

占位栅格可视化并删除网格线

h.FaceAlpha = 0.5;
h.LineStyle = 'none';

鸟瞰图还可以显示来自多个传感器的数据。例如，使用coverageAreaPlotter函数添加雷达覆盖区域。

将覆盖区域添加到绘图中

caPlotter = coverageAreaPlotter(bep, 'DisplayName', 'Coverage Area');

更新为35度和60米范围的视野

mountPosition = [0 0];
range = 15;
orientation = 0;
fieldOfView = 35;
plotCoverageArea(caPlotter, mountPosition, range, orientation, fieldOfView);
hold off

显示来自多个传感器的数据对于诊断和调试自动驾驶车辆做出的决策非常有用

使用占位栅格创建车辆代价图vehicle costmap

vehicleCostmap 函数提供了检查车辆坐标或全局坐标中的位置是否被占用，几乎任何路径规划或决策算法都需要此项检查。使用生成的占位栅格创建车辆代价图。

创建车辆代价图

costmap = vehicleCostmap(flipud(occupancyGrid), ...
    'CellSize',cellSize, ...
    'MapLocation',[0,-spaceToOneSide], ...
    'InflationRadius',0.01);

显示代价图

figure
plot(costmap,'Inflation','off')
colormap(parula)
colorbar
title('Vehicle Costmap')

view(gca,-90,90)

要说明如何使用 vehicleCostmap，需要在全局坐标系中创建一组位置，这些位置表示车辆可以穿过的路径。

创建一组位置

candidateLocations = [
    8 0.375
    10 0.375
    12 2
    14 0.375   
    ];

使用 checkOccupied 函数检查每个位置是否被占用

检查位置是否被占用

isOccupied = checkOccupied(costmap,candidateLocations);

将位置划分为可行驶区域和已占用区域，以便进行可视化。

occupiedLocations = candidateLocations(isOccupied,:);
freeLocations = candidateLocations(~isOccupied,:);

可视化

hold on
markerSize = 100;
scatter(freeLocations(:,1),freeLocations(:,2),markerSize,'g','filled')
scatter(occupiedLocations(:,1),occupiedLocations(:,2),markerSize,'r','filled');
legend(["Free" "Occupied"])
hold off

参考文献

[1] Brostow, Gabriel J., Julien Fauqueur, and Roberto Cipolla. "Semantic Object Classes in Video: A high-definition ground truth database." Pattern Recognition Letters. Vol. 30, Issue 2, 2009, pp. 88-97.