GPU での SLAM の使用による LiDAR データからのマップ作成

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

この例では、NVIDIA® GPU で 3 次元自己位置推定と環境地図作成 (SLAM) を実行する方法を示します。

この例では、車両に取り付けられたセンサーからの 3 次元 LiDAR データを使用して、マップを段階的に作成し、SLAM アプローチを使用して車両の軌跡を推定します。この例は「SLAM を使用した LiDAR データからのマップの作成」の例に基づいています。詳細については、SLAM を使用した LiDAR データからのマップの作成 (Computer Vision Toolbox)を参照してください。

記録データの読み込み

この例では、Velodyne SLAM データセット [1] のデータを使用します。これは、約 6 分間の記録データです。データを一時ディレクトリにダウンロードします。データセットのサイズは 153 MB です。このダウンロードには数分かかる場合があります。

baseDownloadURL = 'https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip';
dataFolder      = fullfile(tempdir, 'kit_velodyneslam_data_scenario1', filesep);
options         = weboptions('Timeout', Inf);

zipFileName  = dataFolder + "scenario1.zip";

% Get the full file path to the PNG files in the scenario1 folder.
pointCloudFilePattern = fullfile(dataFolder, 'scenario1', 'scan*.png');
numExpectedFiles = 2513;

folderExists = exist(dataFolder, 'dir');
if ~folderExists
    % Create a folder in a temporary directory to save the downloaded zip
    % file.
    mkdir(dataFolder);

    disp('Downloading scenario1.zip (153 MB) ...')
    websave(zipFileName, baseDownloadURL, options);

    % Unzip downloaded file
    unzip(zipFileName, dataFolder);

elseif folderExists && numel(dir(pointCloudFilePattern)) < numExpectedFiles
    % Redownload the data if it got reduced in the temporary directory.
    disp('Downloading scenario1.zip (153 MB) ...')
    websave(zipFileName, baseDownloadURL, options);

    % Unzip downloaded file.
    unzip(zipFileName, dataFolder)
end

関数 helperReadDataset を使用して、作成したフォルダーから timetable オブジェクトにデータを読み取ります。LiDAR データには、PNG イメージファイル形式の点群が含まれています。点群ファイル名のリストを変数 pointCloudTable に抽出します。

datasetTable = helperReadDataset(dataFolder, pointCloudFilePattern);

pointCloudTable = datasetTable(:, 1);
insDataTable = datasetTable(:, 2:end);

点群データをエントリポイント関数に渡すには、データを点群から行列にコピーします。イメージファイルから点群データを読み取るには、関数 helperReadPointCloudFromFile を使用します。この関数は、イメージファイル名を受け取り、pointCloud オブジェクトを返します。すべての点群のサイズは 64×870×3 で、2513 個の点群があります。行列のサイズは 64×670×3×2513 です。

pointCloudCount = height(pointCloudTable);
numRows= 64;
numColumns = 870;
location = zeros(numRows, numColumns, 3, 'single');
for idx = 1 : pointCloudCount
    filename = pointCloudTable.PointCloudFileName{idx};
    ptCloud = helperReadPointCloudFromFile(filename);
    location(:,:,:,idx) = ptCloud.Location;
end

オドメトリを使用したマップの作成

マップを作成するには、SLAM を使用した LiDAR データからのマップの作成 (Computer Vision Toolbox)の例の方法を使用します。このアプローチは次のステップで構成されています。

LiDAR スキャンの位置合わせ: 点群のレジストレーション手法を使用して、連続的な LiDAR スキャンを整列させます。この例では、関数 pcregisterndt を使用してスキャンをレジストレーションします。これらの変換を連続して構成することで、各点群を最初の点群の基準座標系に変換します。
位置合わせされたスキャンの結合: 変換したすべての点群を組み合わせることで、マップを生成します。

インクリメンタルにマップを作成し、車両の軌跡を推定するこのアプローチは、"オドメトリ" と呼ばれます。

エントリポイント関数の確認

この例では、エントリポイント関数 ndtSLAM の GPU コードを生成します。ndtSLAM は、点群の位置と INS データを入力引数として受け取ります。for ループ内で、エントリポイント関数は 1 回の反復につき 2 つの連続する点群セットをレジストレーションします。最初の 2 つの点群を for ループの前にレジストレーションし、必要に応じて最後の 2 つの点群を for ループの後にレジストレーションします。

type ndtSLAM.m

function absTformOut = ndtSLAM(locations, insDataTable)
% ndtSLAM register multiple pointclouds and returns the absolute
% transformation for each of the pointcloud. locations is matrix of
% location of every pointcloud with size R x C x 3 x N, where N is
% number of pointcloud, and R x C x 3 is size of individual pointcloud.
% insDataTable is a table of INS data. absTformOut returns transformations
% as a matrix shaped 4 x 4 x N.

% Set random seed to ensure reproducibility
rng(0);

% Initialize point cloud processing parameters
gridSize = coder.const(0.8);

% Initialize transformations
absTform   = rigidtform3d(eye(4, 'single'));  % Absolute transformation to reference frame
relTform   = rigidtform3d(eye(4, 'single'));  % Relative transformation between successive scans

skipFrames  = coder.const(5);
numFrames   = size(locations, 4);

% allocate output variables
numTransforms = ceil(numFrames / skipFrames);
absTformOut = coder.nullcopy(zeros(4,4,numTransforms, 'single'));
outIdx = 1;

% If input locations are empty, return
if isempty(locations)
    return;
end

% Read point cloud
ptCloudFirstOrig = pointCloud(locations(:,:,:,1));

% Process point cloud
%   - Segment and remove ground plane
%   - Segment and remove ego vehicle
ptCloudFirst = helperProcessPointCloud(ptCloudFirstOrig, "rangefloodfill");

% Downsample the processed point cloud
ptCloudFirst = pcdownsample(ptCloudFirst, "gridAverage", gridSize);

% Add first point cloud scan as a view to the view set
absTformOut(:,:,outIdx) = absTform.A;
outIdx = outIdx + 1;

ptCloudPrev = ptCloudFirst;

for n = 1 + skipFrames : skipFrames + skipFrames : numFrames - skipFrames
    % If locations are empty skip present iteration and continue to next
    % iteration.
    if isempty(locations(:,:,:,n)) || isempty(locations(:,:,:,n + skipFrames))
        continue;
    end
    %% even iteration
    % Read point cloud
    ptCloudOrig = pointCloud(locations(:,:,:,n));
    insData = insDataTable(n - skipFrames: n, :);
    [absTform, relTform, ptCloudPrev] = processFrame(ptCloudOrig, ptCloudPrev, ...
        insData, gridSize, relTform, absTform);

    % update output
    absTformOut(:,:,outIdx) = absTform.A;
    outIdx = outIdx + 1;

    %% odd iteration
    % Read point cloud
    ptCloudOrig = pointCloud(locations(:,:,:,n + skipFrames));
    insData = insDataTable(n: n + skipFrames, :);
    [absTform, relTform, ptCloudPrev] = processFrame(ptCloudOrig, ptCloudPrev, ...
        insData, gridSize, relTform, absTform);

    % update output
    absTformOut(:,:,outIdx) = absTform.A;
    outIdx = outIdx + 1;
end

if mod(numTransforms, 2) == 0
    % last even iteration, if required.
    ptIdx = 1 + skipFrames * (numTransforms - 1);
    % Read point cloud
    ptCloudOrig = pointCloud(locations(:,:,:,ptIdx));
    ptCloudPrev = pointCloud(locations(:,:,:,ptIdx - skipFrames));
    insData = insDataTable(ptIdx-skipFrames:ptIdx, :);
    [absTform, ~, ~] = processFrame(ptCloudOrig, ptCloudPrev, ...
        insData, gridSize, relTform, absTform);
    % update output
    absTformOut(:,:,outIdx) = absTform.A;
end
end

エントリポイント関数 ndtSLAM は processFrame を呼び出して、2 つの点群の処理とレジストレーションを実行します。

type processFrame.m

function [absTform, relTform, ptCloudPrev] = processFrame(ptCloudOrig, ptCloudPrev, ...
    insData, gridSize, relTform, absTform)
% processFrame Process and register two pointclouds and return the
% transformations. 

regGridSize = coder.const(2.5);

% Process point cloud
%   - Segment and remove ground plane
%   - Segment and remove ego vehicle
ptCloud = helperProcessPointCloud(ptCloudOrig, "rangefloodfill");

% Downsample the processed point cloud
moving = pcdownsample(ptCloud, 'gridAverage', gridSize);

% Use INS to estimate an initial transformation for registration
initTform = helperComputeInitialEstimateFromINS(relTform, insData);

% Compute rigid transformation that registers current point cloud with
% previous point cloud
relTform = pcregisterndt(moving, ptCloudPrev, regGridSize, ...
    "InitialTransform", initTform);

% Update absolute transformation to reference frame (first point cloud)
absTform = rigidtform3d(absTform.A * relTform.A);

% update prev point cloud
ptCloudPrev = moving;
end

helperProcessPointCloud は、グランドプレーンとエゴビークルに属する点を削除することで、pointCloud オブジェクトを処理します。

type helperProcessPointCloud.m

function ptCloud = helperProcessPointCloud(ptCloudIn, method)
%helperProcessPointCloud Process pointCloud to remove ground and ego vehicle
%   ptCloud = helperProcessPointCloud(ptCloudIn, method) processes
%   ptCloudIn by removing the ground plane and the ego vehicle.
%   method can be "planefit" or "rangefloodfill".
%
%   See also pcfitplane, pointCloud/findNeighborsInRadius.

isOrganized = ~ismatrix(ptCloudIn.Location);

if (method=="rangefloodfill" && isOrganized)
    elevationAngleDelta = coder.const(11);
    % Segment ground using floodfill on range image
    groundFixedIdx = segmentGroundFromLidarData(ptCloudIn, ...
        "ElevationAngleDelta", elevationAngleDelta);
else
    % Segment ground as the dominant plane with reference normal
    % vector pointing in positive z-direction
    maxDistance         = 0.4;
    maxAngularDistance  = 5;
    referenceVector     = [0 0 1];

    [~, groundFixedIdx] = pcfitplane(ptCloudIn, maxDistance, ...
        referenceVector, maxAngularDistance);
end

if isOrganized
    groundFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2));
else
    groundFixed = false(ptCloudIn.Count, 1);
end
groundFixed(groundFixedIdx) = true;

% Segment ego vehicle as points within a given radius of sensor
sensorLocation = coder.const([0 0 0]);
radius = coder.const(3.5);
egoFixedIdx = findNeighborsInRadius(ptCloudIn, sensorLocation, radius);

if isOrganized
    egoFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2));
else
    egoFixed = false(ptCloudIn.Count, 1);
end
egoFixed(egoFixedIdx) = true;

% Retain subset of point cloud without ground and ego vehicle
if isOrganized
    indices = ~groundFixed & ~egoFixed;
else
    indices = find(~groundFixed & ~egoFixed);
end

ptCloud = select(ptCloudIn, indices);
end

helperComputeInitialEstimateFromINS は、INS データから初期変換推定値を計算します。

type helperComputeInitialEstimateFromINS.m

function initTform = helperComputeInitialEstimateFromINS(initTform, insData)
% helperComputeInitialEstimateFromINS Compute estimate for transformation
% from INS data.

% If no INS readings are available, return
if isempty(insData)
    return;
end

% The INS readings are provided with X pointing to the front, Y to the left
% and Z up. Translation below accounts for transformation into the lidar
% frame.
insToLidarOffset = [0 -0.79 -1.73]; % See DATAFORMAT.txt
tNow = [-insData.Y(end), insData.X(end), insData.Z(end)].' + insToLidarOffset';
tBefore = [-insData.Y(1)  , insData.X(1)  , insData.Z(1)].' + insToLidarOffset';

% Since the vehicle is expected to move along the ground, changes in roll
% and pitch are minimal. Ignore changes in roll and pitch, use heading only.
Rnow = rotmat(quaternion([insData.Heading(end) 0 0], 'euler', 'ZYX', 'point'), 'point');
Rbef = rotmat(quaternion([insData.Heading(1)   0 0], 'euler', 'ZYX', 'point'), 'point');

tformMatrix = [Rbef tBefore;0 0 0 1] \ [Rnow tNow;0 0 0 1];

initTform = rigidtform3d(cast(tformMatrix, 'like', initTform.A));
end

CUDA® MEX の生成

エントリポイント関数 ndtSLAM の CUDA MEX ファイルを生成します。パフォーマンスを向上させるために、次のようにします。

メモリマネージャーを有効にする。
システムの GPU でサポートされている最高のコンピューティング能力を設定する。
スレッドごとのスタック制限を増やす。この例では、最大の整数値を使用します。intmax がエラーを返す場合は、より低い値を使用します。

config = coder.gpuConfig();
config.GpuConfig.EnableMemoryManager = true;
config.GpuConfig.ComputeCapability =  gpuDevice().ComputeCapability;
config.GpuConfig.StackLimitPerThread = intmax;

codegen -config config -args {location, insDataTable} ndtSLAM

Code generation successful: View report

マップのプロット

関数 ndtSLAM は、マップの作成に使用される各フレームの絶対変換を返します。

tforms = ndtSLAM_mex(location, insDataTable);

pcviewset オブジェクトを作成して、一連のビューおよびビュー間のペアワイズ接続として点群オドメトリと SLAM データを保存します。vSet の Views テーブルで、変数 AbsolutePose は、最初のビューに対する各ビューの絶対姿勢を指定します。vSet の Connections テーブルでは、接続されたビュー間の相対的な制約を変数 RelativePose で指定し、接続に関連付けられた不確かさをエッジごとに変数 InformationMatrix で指定します。

vSet = pcviewset();

マップをプロットするには、変換行列を rigidtform3d オブジェクトに変換し、点群と rigidtform3d オブジェクトを pcviewset オブジェクトに追加します。

skipFrames = 5;
viewId = 1;
for idx = 1: skipFrames: 2513
    ptCloud = pointCloud(location(:,:,:,idx));
    absTforms = rigidtform3d(tforms(:,:,viewId));
    vSet = addView(vSet, viewId, absTforms, "PointCloud", ptCloud);
    if viewId > 1
        vSet = addConnection(vSet, viewId-1, viewId);
    end
    viewId = viewId + 1;
end

作成したビューセットを使用して点群マップを作成します。pcalign を使用して、ビューの絶対姿勢をビューセット内の点群の位置に合わせます。グリッドサイズを指定して、マップの分解能を制御します。マップは pointCloud オブジェクトとして返されます。

ptClouds = vSet.Views.PointCloud;
absPoses = vSet.Views.AbsolutePose;
mapGridSize = 0.2;
ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize);

hFigAfter = figure('Name', 'GPU SLAM');
hAxAfter = axes(hFigAfter);
pcshow(ptCloudMap, 'Parent', hAxAfter);

% Overlay view set display
hold on
plot(vSet, 'Parent', hAxAfter);

helperMakeFigurePublishFriendly(hFigAfter);

参考文献

Moosmann, Frank and Christoph Stiller. “Velodyne SLAM.” Proceedings of the IEEE Intelligent Vehicles Symposium, 2011, pp. 393–98. http://www.mrt.kit.edu/z/publ/download/Moosmann_IV11.pdf.

サポート関数

関数 "helperReadPointCloudFromFile" は、PNG イメージファイルから点群を読み取り、点群オブジェクトを返します。

function ptCloud = helperReadPointCloudFromFile(fileName)
%helperReadPointCloudFromFile Read pointCloud from PNG image file
%
%   This is an example helper class that is subject to change or removal in
%   future releases.
%
%   ptCloud = helperReadPointCloudFromFile(fileName) reads point cloud
%   data from the .png image file fileName and returns a pointCloud object.
%   This function expects file to be from the Velodyne SLAM Dataset.

% Copyright 2019-2022 The MathWorks, Inc.

% From DATAFORMAT.txt
% -------------------
% Each 360° revolution of the Velodyne scanner was stored as 16bit png
% distance image (scan*.png). The scanner turned clockwise, filling the
% image from the leftmost to the rightmost column, with the leftmost and
% rightmost column being at the back of the vehicle. Note that measurements
% were not corrected for vehicle movement. Thus and due to the physical
% setup of the laser diodes, some strange effects can be seen at the cut of
% the image when the vehicle is turning. As consequence, it is best to
% ignore the 10 leftmost and rightmost columns of the image. To convert the
% pixel values [0..65535] into meters, just divide by 500. This results in
% an effective range of [0..131m]. Invalid measurements are indicated by
% zero distance.

% To convert the distance values into 3D coordinates, use the setup in
% "img.cfg". The yaw angles (counter-clockwise) are a linear mapping from
% the image column [0..869]->[180°..-180°] The pitch angles are specified
% for each image row separately.

validateattributes(fileName, {'char','string'}, {'scalartext'}, mfilename, 'fileName');

% Convert pixel values to range
range = single(imread(fileName)) ./ 500;
range(range==0) = NaN;

% Get yaw angles as a linear mapping of [0..869] -> [180 to -180]. Yaw and
% pitch values are obtained from img.cfg file.
yawAngles = 869 : -1 : 0;
yawAngles =-180 + yawAngles .* (360 / 869);

pitchAngles = [-1.9367; -1.57397; -1.30476; -0.871566; -0.57881; -0.180617;...
    0.088762; 0.451829; 0.80315; 1.20124; 1.49388; 1.83324; 2.20757; ...
    2.54663; 2.87384; 3.23588; 3.53933; 3.93585; 4.21552; 4.5881; 4.91379; ...
    5.25078; 5.6106; 5.9584; 6.32889; 6.67575; 6.99904; 7.28731; 7.67877; ...
    8.05803; 8.31047; 8.71141; 9.02602; 9.57351; 10.0625; 10.4707; 10.9569; ...
    11.599; 12.115; 12.5621; 13.041; 13.4848; 14.0483; 14.5981; 15.1887; ...
    15.6567; 16.1766; 16.554; 17.1868; 17.7304; 18.3234; 18.7971; 19.3202; ...
    19.7364; 20.2226; 20.7877; 21.3181; 21.9355; 22.4376; 22.8566; 23.3224; ...
    23.971; 24.5066; 24.9992];

[yaw,pitch] = meshgrid( deg2rad(yawAngles), deg2rad(pitchAngles));
rangeData = cat(3, range, pitch, yaw);

[xyzData(:,:,2),xyzData(:,:,1),xyzData(:,:,3)] = sph2cart(rangeData(:,:,3), ...
    rangeData(:,:,2),rangeData(:,:,1));
xyzData(:,:,3) = -xyzData(:,:,3); %sign convention

% Transform points so that coordinate system faces towards the front of the
% vehicle.
ptCloud = pointCloud(xyzData.*cat(3,-1,1,1));
end

関数 "helperReadINSConfigFile" は、INS 構成ファイルを読み取り、データを table として返します。

function T = helperReadINSConfigFile(fileName)
%helperReadINSConfigFile Reads INS configuration file
%
%   This is an example helper class that is subject to change or removal in
%   future releases.
%
%   T = helperReadINSConfigFile(fileName) reads the .cfg configuration file
%   containing INS data, and returns it in a table. This function expects
%   data from the Velodyne SLAM Dataset.
%
%   See also timetable, readtable.

validateattributes(fileName, {'char','string'}, {'scalartext'}, mfilename, 'fileName');

% Create options to read delimited text file
opts = delimitedTextImportOptions;
opts.Delimiter = ";";
opts.DataLines = [8 inf];
opts.VariableNames = [...
    "Timestamps", ...
    "Num_Satellites", "Latitude", "Longitude", "Altitude", ...
    "Heading", "Pitch", "Roll", ...
    "Omega_Heading", "Omega_Pitch", "Omega_Roll", ...
    "V_X", "V_Y", "V_ZDown", ...
    "X", "Y", "Z"];
opts.VariableTypes(2:end) = {'double'};

T = readtable(fileName, opts);

% Remove unnecessary column
T.ExtraVar1 = [];

% Convert timestamps to datetime
T.Timestamps = datetime(T.Timestamps, 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSS');
T = table2timetable(T);
end

関数 "helperReadDataset" は、Velodyne SLAM データセットのデータを timetable に読み取ります。

function datasetTable = helperReadDataset(dataFolder, pointCloudFilePattern)
%helperReadDataset Read Velodyne SLAM Dataset data into a timetable
%   datasetTable = helperReadDataset(dataFolder) reads data from the
%   folder specified in dataFolder into a timetable. The function
%   expects data from the Velodyne SLAM Dataset.
%
%   See also fileDatastore, helperReadINSConfigFile.

% Create a file datastore to read in files in the right order
fileDS = fileDatastore(pointCloudFilePattern, 'ReadFcn', ...
    @helperReadPointCloudFromFile);

% Extract the file list from the datastore
pointCloudFiles = fileDS.Files;

imuConfigFile = fullfile(dataFolder, 'scenario1', 'imu.cfg');
insDataTable = helperReadINSConfigFile(imuConfigFile);

% Delete the bad row from the INS config file
insDataTable(1447, :) = [];

% Remove columns that will not be used
datasetTable = removevars(insDataTable, ...
    {'Num_Satellites', 'Latitude', 'Longitude', 'Altitude', 'Omega_Heading', ...
    'Omega_Pitch', 'Omega_Roll', 'V_X', 'V_Y', 'V_ZDown'});

datasetTable = addvars(datasetTable, pointCloudFiles, 'Before', 1, ...
    'NewVariableNames', "PointCloudFileName");
end

関数 "helperMakeFigurePublishFriendly" は、パブリッシュによって取得されたスクリーンショットが適切になるように図を調整します。

function helperMakeFigurePublishFriendly(figure)
if ~isempty(figure) && isvalid(figure)
    figure.HandleVisibility = 'callback';
end
end