Build a Map from Lidar Data Using SLAM on GPU

This example uses:

This example shows how to perform 3-D simultaneous localization and mapping (SLAM) on an NVIDIA® GPU.

This example uses 3-D lidar data from a vehicle-mounted sensor to progressively build a map and estimate the trajectory of the vehicle by using the SLAM approach. This example is based on the Build a Map from Lidar Data Using SLAM example. For more information, see Build a Map from Lidar Data Using SLAM (Computer Vision Toolbox).

Load Recorded Data

This example uses data from the Velodyne SLAM data set [1], and represents close to six minutes of recorded data. Download the data to a temporary directory. The dataset size is 153 MB. This download can take a few minutes.

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

Use the helperReadDataset function to read data from the created folder into a timetable object. The lidar data contains point clouds in the form of PNG image files. Extract the list of point cloud file names to the pointCloudTable variable.

datasetTable = helperReadDataset(dataFolder, pointCloudFilePattern);

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

To pass the point cloud data to entry-point function, copy the data from the point clouds into a matrix. To read the point cloud data from the image file, use the helperReadPointCloudFromFile function. This function takes an image file name and returns a pointCloud object. The size of every point cloud is 64-by-870-by-3 and there are 2513 point clouds. The size of matrix is 64-by-670-by-3-by-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

Build a Map Using Odometry

To build a map, use the approach in the Build a Map from Lidar Data (Computer Vision Toolbox) example. The approach consists of the following steps:

Align lidar scans: Align successive lidar scans using a point cloud registration technique. This example uses pcregisterndt function to register scans. By successively composing these transformations, you transform each point cloud into the reference frame of the first point cloud.
Combine aligned scans: Generate a map by combining all the transformed point clouds.

This approach of incrementally building a map and estimating the trajectory of the vehicle is called odometry.

Examine Entry-Point Function

This example generates GPU code for the ndtSLAM entry-point function. ndtSLAM takes locations of point clouds and INS data as input arguments. Inside the for-loop, the entry-point function registers two consecutive sets of point clouds in a single iteration. It registers the first two pointclouds before the for-loop, and the last two point clouds after the for-loop, if required.

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

The ndtSLAM entry-point function calls processFrame to perform processing and registration of two point clouds.

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 processes a pointCloud object by removing points that belong to the ground plane and the ego vehicle.

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 computes initial transformation estimate from INS data.

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

Generate CUDA® MEX

Generate CUDA MEX file for the entry-point function, ndtSLAM. To improve performance:

Enable the memory manager.
Set the compute capability to the highest supported by the GPU on the system.
Increase the stack limit per thread. This example uses the maximum integer value. Use a lower value if intmax returns an error.

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

Plot Map

The ndtSLAM function returns the absolute transformation for each frame used to build the map.

tforms = ndtSLAM_mex(location, insDataTable);

Create a pcviewset object to store point cloud odometry and the SLAM data as a set of views and pairwise connections between views. In the Views table of vSet, the AbsolutePose variable specifies the absolute pose of each view with respect to the first view. In the Connections table of vSet, the RelativePose variable specifies relative constraints between the connected views, the InformationMatrix variable specifies, for each edge, the uncertainty associated with a connection.

vSet = pcviewset();

To plot the map, convert the transformation matrix into a rigidtform3d object and add the point clouds and the rigidtform3d objects into the pcviewset object.

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

Build a point cloud map using the created view set. Align the view absolute poses with the point clouds in the view set using pcalign. Specify a grid size to control the resolution of the map. The map is returned as a pointCloud object.

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);

References

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.

Supporting Functions

The helperReadPointCloudFromFile function reads point clouds from PNG image files and returns a point cloud object.

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

The helperReadINSConfigFile function reads an INS configuration file and returns the data as a 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

The helperReadDataset function reads the Velodyne SLAM data set data into a 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

The helperMakeFigurePublishFriendly function adjusts figures so that screenshot captured by publishing are correct.

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