単眼カメラとセマンティックセグメンテーションを使用した占有グリッドの作成

この例では次を使用します。

この例では、セマンティックセグメンテーションと深層学習を使用して車両周りの自由空間を推定し、占有グリッドを作成する方法を説明します。その後、この占有グリッドを使用してパス計画に使用可能な車両コストマップを作成します。

自由空間の推定について

自由空間の推定では、環境内で歩行者、縁石、他の車両などの障害物に衝突することなく自車が走行できる領域を特定します。車両は、自由空間を推定するために、レーダー、LiDAR、カメラなどのさまざまなセンサーを使用できます。この例では、セマンティックセグメンテーションを使用してイメージセンサーから自由空間を推定する方法に焦点を当てています。

この例では、次を行う方法を学びます。

セマンティックイメージセグメンテーションを使用した自由空間の推定。
自由空間の推定を使用した占有グリッドの作成。
鳥瞰図プロットでの占有グリッドの可視化。
占有グリッドを使用した車両コストマップの作成。
ワールドの位置が占有されているか空いているかのチェック。

事前学習済みのネットワークのダウンロード

この例では、事前学習済みのセマンティックセグメンテーションネットワークを使用します。このネットワークは、ピクセルを 11 個の異なるクラス (Road、Pedestrian、Car、Sky など) に分類できます。イメージ内の自由空間は、Road として分類されたイメージピクセルを自由空間として定義することで推定できます。その他のクラスすべては、非自由空間または障害物として定義されます。

このネットワークに学習させる手順の詳細は、深層学習を使用したセマンティックセグメンテーション (Computer Vision Toolbox)の例に示されています。事前学習済みのネットワークをダウンロードします。

% Download and load the pretrained network.
data = downloadPretrainedNetwork('vision/data','segnetVGG16CamVid');
net = data.net;

メモ: データのダウンロードにかかる時間はインターネット接続の速度によって異なります。上記で使用したコマンドは、ダウンロードが完了するまで MATLAB® をブロックします。別の方法として、Web ブラウザーを使用して、このデータセットをローカルディスクにまずダウンロードしておくことができます。この場合、Web からダウンロードしたファイルを使用するには、上記の変数 pretrainedFolder の値を、ダウンロードしたファイルの場所に変更します。

自由空間の推定

ダウンロードしたセマンティックセグメンテーションネットワークを使用してイメージを処理し、自由空間を推定します。このネットワークは、イメージ内の各イメージピクセルの分類を返します。自由空間は、Road として分類されたイメージピクセルとして識別されます。

この例で使用されているイメージは、CamVid データセット [1] に含まれるイメージシーケンスからの単一フレームです。この例で示す手順をフレームのシーケンスに適用して、車両の走行中に自由空間を推定することができます。ただし、この例では非常に深い畳み込みニューラルネットワークアーキテクチャを使用しているため (VGG-16 符号化器を使用した SegNet)、各フレームを処理するのに約 1 秒かかります。そのため、便宜上、単一フレームを処理します。

% Read the image.
I = imread('seq05vd_snap_shot.jpg');

% Segment the image.
[C,scores,allScores] = semanticseg(I,net);

% Overlay free space onto the image.
B = labeloverlay(I,C,'IncludedLabels',"Road");

% Display free space and image.
figure
imshow(B)

自由空間の推定における信頼度を把握するため、各ピクセルの Road クラスの出力スコアを表示します。信頼度の値を使用し、下流のアルゴリズムに推定の妥当性を通知できます。たとえば、ネットワークがあるピクセルを Road として分類したとしても、信頼度スコアが低いために安全上の理由でその分類を無視する場合があります。

% Use the network's output score for Road as the free space confidence.
roadClassIdx = 4;
freeSpaceConfidence = allScores(:,:,roadClassIdx);
    
% Display the free space confidence.
figure
imagesc(freeSpaceConfidence)
title('Free Space Confidence Scores')
colorbar

Road ピクセルに対する初期セグメンテーションの結果が、道路上のほとんどのピクセルが正しく分類されたことを示している場合でも、スコアを可視化することで、それらの分類における分類器の信頼度をより詳細に確認できます。たとえば、自動車の境界に近づくにつれて信頼度は低下します。

鳥瞰ビューイメージの作成

自由空間の推定はイメージ空間で生成されます。ナビゲーションに役立つ占有グリッドの生成を容易にするには、自由空間の推定を車両座標系に変換する必要があります。これは、自由空間の推定を鳥瞰ビューイメージに変換することによって行えます。

鳥瞰ビューイメージを作成するには、まず、カメラセンサーの構成を定義します。この例の最後にリストされているサポート関数 camvidMonoCameraSensor は、CamVid [1] のデータを収集するのに使用する単眼カメラを表す monoCamera (Automated Driving Toolbox) オブジェクトを返します。monoCamera (Automated Driving Toolbox) を構成するには、カメラの内部パラメーターと外部パラメーターが必要です。これらは、CamVid データセットで提供されているデータを使用して推定されたものです。この関数は、カメラの内部パラメーターを推定するために、CamVid のチェッカーボードのキャリブレーションイメージと、カメラキャリブレーター (Computer Vision Toolbox) アプリを使用しました。高さやピッチといったカメラの外部パラメーターの推定は、CamVid データセットの作者によって推定された外部パラメーターから導出されました。

% Create monoCamera for CamVid data.
sensor = camvidMonoCameraSensor();

birdsEyeView (Automated Driving Toolbox) オブジェクトは、カメラの設定が与えられると、元のイメージを鳥瞰ビューに変換します。このオブジェクトにより、変換する領域を車両座標を使用して指定できます。車両座標の単位は、カメラの取り付け高さをメートル単位で指定したときに、monoCamera (Automated Driving Toolbox) オブジェクトによって確立されていることに注意してください。たとえば、高さをミリメートル単位で指定した場合、残りのシミュレーションではミリメートルが使用されます。

% Define bird's-eye-view transformation parameters.
distAheadOfSensor = 20; % in meters, as previously specified in monoCamera height input
spaceToOneSide    = 3;  % look 3 meters to the right and left
bottomOffset      = 0;  
outView = [bottomOffset, distAheadOfSensor, -spaceToOneSide, spaceToOneSide];

outImageSize = [NaN, 256]; % output image width in pixels; height is chosen automatically to preserve units per pixel ratio

birdsEyeConfig = birdsEyeView(sensor,outView,outImageSize);

イメージと自由空間の信頼度を示す鳥瞰ビューイメージを生成します。

% Resize image and free space estimate to size of CamVid sensor. 
imageSize = sensor.Intrinsics.ImageSize;
I = imresize(I,imageSize);
freeSpaceConfidence = imresize(freeSpaceConfidence,imageSize);

% Transform image and free space confidence scores into bird's-eye view.
imageBEV = transformImage(birdsEyeConfig,I);
freeSpaceBEV = transformImage(birdsEyeConfig,freeSpaceConfidence); 

% Display image frame in bird's-eye view.
figure
imshow(imageBEV)

イメージを鳥瞰ビューに変換し、自由空間の信頼度を生成します。

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

センサーから離れた領域ほどピクセル数が少なくなり、より不鮮明になるため、さらに多くの内挿が必要です。

自由空間の推定に基づく占有グリッドの作成

占有グリッドは、車両座標の離散グリッドとして車両の周囲を表すために使用され、パス計画に使用されます。占有グリッドの各セルは、そのセルの占有確率を表す値をもちます。推定された自由空間を使用して占有グリッドの値を埋めることができます。

自由空間の推定を使用して占有グリッドを埋める手順は以下のとおりです。

車両座標における占有グリッドの次元を定義します。
各グリッドセルについて、一連の (X,Y) 点を生成します。これらの点は車両座標系にあります。
vehicleToImage (Automated Driving Toolbox) の変換を使用し、点を車両の座標空間 (X,Y) から鳥瞰ビューイメージの座標空間 (x,y) に変換します。
griddedInterpolant を使用して (x,y) に位置する自由空間の信頼度の値をサンプリングし、厳密にイメージ内のピクセルの中心ではない自由空間の信頼度の値を内挿します。
占有グリッドセルに対応するすべての (x,y) 点について、自由空間の信頼度の平均値でそのグリッドセルを埋めます。

説明を簡潔にするために、上記の手順は、この例の最後にリストされているサポート関数 createOccupancyGridFromFreeSpaceEstimate で実装されています。鳥瞰ビューの構成を使用して占有グリッドの次元を定義し、createOccupancyGridFromFreeSpaceEstimate を呼び出して占有グリッドを作成します。

% Define dimensions and resolution of the occupancy grid.
gridX = distAheadOfSensor;
gridY = 2 * spaceToOneSide;
cellSize = 0.25; % in meters to match units used by CamVid sensor

% Create the occupancy grid from the free space estimate.
occupancyGrid = createOccupancyGridFromFreeSpaceEstimate(...
    freeSpaceBEV, birdsEyeConfig, gridX, gridY, cellSize);

birdsEyePlot (Automated Driving Toolbox) を使用して占有グリッドを可視化します。birdsEyePlot (Automated Driving Toolbox) を作成し、pcolor を使用してその上に占有グリッドを追加します。

% Create bird's-eye plot.
bep = birdsEyePlot('XLimits',[0 distAheadOfSensor],'YLimits', [-5 5]);

% Add occupancy grid to bird's-eye plot.
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)

% Make the occupancy grid visualization transparent and remove grid lines.
h.FaceAlpha = 0.5;
h.LineStyle = 'none';

鳥瞰図プロットは、複数のセンサーからのデータを表示することもできます。たとえば、coverageAreaPlotter (Automated Driving Toolbox) を使用してレーダーのカバレッジ領域を追加できます。

% Add coverage area to plot.
caPlotter = coverageAreaPlotter(bep, 'DisplayName', 'Coverage Area');

% Update it with a field of view of 35 degrees and a range of 60 meters
mountPosition = [0 0];
range = 15;
orientation = 0;
fieldOfView = 35;
plotCoverageArea(caPlotter, mountPosition, range, orientation, fieldOfView);
hold off

複数のセンサーからのデータを表示すると、自律型車両が行う判定の診断やデバッグに役立ちます。

占有グリッドを使用した車両コストマップの作成

vehicleCostmap (Automated Driving Toolbox) は、車両座標またはワールド座標において位置が占有されているか空いているかをチェックするための機能を提供します。あらゆるパス計画アルゴリズムまたは意思決定アルゴリズムでこのチェックが必要です。生成された occupancyGrid を使用して vehicleCostmap (Automated Driving Toolbox) を作成します。

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

% Display the costmap.
figure
plot(costmap,'Inflation','off')
colormap(parula)
colorbar
title('Vehicle Costmap')

% Orient the costmap so that it lines up with the vehicle coordinate
% system, where the X-axis points in front of the ego vehicle and the
% Y-axis points to the left.
view(gca,-90,90)

vehicleCostmap (Automated Driving Toolbox) の使用方法を説明するために、ワールド座標における一連の位置を作成します。これらの位置は、車両が通行できるパスを表します。

% Create a set of locations in vehicle coordinates.
candidateLocations = [
    8 0.375
    10 0.375
    12 2
    14 0.375   
    ];

checkOccupied (Automated Driving Toolbox) を使用し、それぞれの位置が占有されているか空いているかをチェックします。この結果によると、パスの候補の 1 つは costmap で定義された障害物と衝突するため、通行は不可能です。

% Check if locations are occupied.
isOccupied = checkOccupied(costmap,candidateLocations);

% Partition locations into free and occupied for visualization purposes.
occupiedLocations = candidateLocations(isOccupied,:);
freeLocations = candidateLocations(~isOccupied,:);

% Display free and occupied points on top of costmap.
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

上に示した occupancyGrid、vehicleCostmap (Automated Driving Toolbox)、checkOccupied (Automated Driving Toolbox)の使用方法は、pathPlannerRRT (Automated Driving Toolbox)などのパスプランナーで使用される基本的な操作を示しています。パス計画の詳細については、Automated Parking Valet (Automated Driving Toolbox)の例を参照してください。

参考文献

[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.

サポート関数

function sensor = camvidMonoCameraSensor()
% Return a monoCamera camera configuration based on data from the CamVid 
% data set[1].
%
% The cameraCalibrator app was used to calibrate the camera using the
% calibration images provided in CamVid:
%
% http://web4.cs.ucl.ac.uk/staff/g.brostow/MotionSegRecData/data/CalibrationSeq_and_Files_0010YU.zip
%
% Calibration pattern grid size is 28 mm. 
%
% Camera pitch is computed from camera pose matrices [R t] stored here:
%
% http://web4.cs.ucl.ac.uk/staff/g.brostow/MotionSegRecData/data/EgoBoost_trax_matFiles.zip

% References
% ----------
% [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.

calibrationData = load('camera_params_camvid.mat');

% Describe camera configuration.
focalLength    = calibrationData.cameraParams.FocalLength;
principalPoint = calibrationData.cameraParams.PrincipalPoint;
imageSize      = calibrationData.cameraParams.ImageSize;

% Camera height estimated based on camera setup pictured in [1].
height = 0.5;  % height in meters from the ground

% Camera pitch was computed using camera extrinsics provided in data set.
pitch = 0;  % pitch of the camera, towards the ground, in degrees

camIntrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);
sensor = monoCamera(camIntrinsics,height,'Pitch',pitch);
end

function occupancyGrid = createOccupancyGridFromFreeSpaceEstimate(...
    freeSpaceBEV,birdsEyeConfig,gridX,gridY,cellSize)
% Return an occupancy grid that contains the occupancy probability over
% a uniform 2-D grid.

% Number of cells in occupancy grid.
numCellsX = ceil(gridX / cellSize);
numCellsY = ceil(gridY / cellSize);

% Generate a set of (X,Y) points for each grid cell. These points are in
% the vehicle's coordinate system. Start by defining the edges of each grid
% cell.

% Define the edges of each grid cell in vehicle coordinates.
XEdges = linspace(0,gridX,numCellsX);
YEdges = linspace(-gridY/2,gridY/2,numCellsY);

% Next, specify the number of sample points to generate along each
% dimension within a grid cell. Use these to compute the step size in the
% X and Y direction. The step size will be used to shift the edge values of
% each grid to produce points that cover the entire area of a grid cell at
% the desired resolution.

% Sample 20 points from each grid cell. Sampling more points may produce
% smoother estimates at the cost of additional computation.
numSamplePoints = 20;

% Step size needed to sample number of desired points.
XStep = (XEdges(2)-XEdges(1)) / (numSamplePoints-1);
YStep = (YEdges(2)-YEdges(1)) / (numSamplePoints-1);

% Finally, slide the set of points across both dimensions of the grid
% cells. Sample the occupancy probability along the way using
% griddedInterpolant.

% Create griddedInterpolant for sampling occupancy probability. Use 1
% minus the free space confidence to represent the probability of occupancy.
occupancyProb = 1 - freeSpaceBEV;
sz = size(occupancyProb);
[y,x] = ndgrid(1:sz(1),1:sz(2));
F = griddedInterpolant(y,x,occupancyProb);

% Initialize the occupancy grid to zero.
occupancyGrid = zeros(numCellsY*numCellsX,1);

% Slide the set of points XEdges and YEdges across both dimensions of the
% grid cell. 
for j = 1:numSamplePoints
    
    % Increment sample points in the X-direction
    X = XEdges + (j-1)*XStep;
   
    for i = 1:numSamplePoints
        
        % Increment sample points in the Y-direction
        Y = YEdges + (i-1)*YStep;
        
        % Generate a grid of sample points in bird's-eye-view vehicle coordinates
        [XGrid,YGrid] = meshgrid(X,Y);
        
        % Transform grid of sample points to image coordinates
        xy = vehicleToImage(birdsEyeConfig,[XGrid(:) YGrid(:)]);
        
        % Clip sample points to lie within image boundaries
        xy = max(xy,1);
        xq = min(xy(:,1),sz(2));        
        yq = min(xy(:,2),sz(1));
        
        % Sample occupancy probabilities using griddedInterpolant and keep
        % a running sum.
        occupancyGrid = occupancyGrid + F(yq,xq);  
    end
    
end

% Determine mean occupancy probability.
occupancyGrid = occupancyGrid / numSamplePoints^2;
occupancyGrid = reshape(occupancyGrid,numCellsY,numCellsX);
end

単眼カメラとセマンティック セグメンテーションを使用した占有グリッドの作成