Main Content

Build a Map from Lidar Data using SLAM on GPU

This example uses:

Open Live Script

This example shows how to perform 3-D Lidar simultaneous localization and mapping (SLAM) on 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

The data used in this example is part of the Velodyne SLAM Dataset [1], and represents close to 6 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
Downloading scenario1.zip (153 MB) ...

Use the helperReadDataset function to read data from the created folder in the form of a timetable. The point clouds captured by the lidar are stored in the form of PNG image files. Extract the list of point cloud file names in 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 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);
numColumns = 64;
numRows = 870;
location = zeros(numColumns, numRows, 3, 'single');
for idx = 1 : pointCloudCount
    filename = pointCloudTable.PointCloudFileName{idx};
    ptCloud = helperReadPointCloudFromFile(filename);
    location(:,:,:,idx) = ptCloud.Location;
end

Build a Map Using Odometry

Use the approach explained in the Build a Map from Lidar Data Using SLAM (Computer Vision Toolbox) example to build a map. The approach consists of the following steps:

  • Align lidar scans: Align successive lidar scans using a point cloud registration technique. This example uses pcregisterndt for registering scans. By successively composing these transformations, each point cloud is transformed back to 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

ndtSLAM is the entry-point function for GPU code generation. ndtSLAM takes locations of point clouds and INS data as input. Inside the for-loop, it registers two consecutive sets of point clouds in a single iteration. The first two pointclouds are registered before the for-loop, and the last two point clouds are registered 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 N x R x C x 3, 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 N x 4 x 4.

% 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

processFrame performs the processing and registration of two point clouds. processFrame is called by ndtSLAM.

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 belonging 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 for the entry-point function(ndtSLAM). To improve performance,

  1. Enable Memory Manager

  2. Set the compute capability to the highest supported by the GPU on the system.

  3. Increase stack limit per thread. This example uses max integer value. Use lower value if this gives 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

ndtSLAM function returns the absolute transformation for each of the frame that is used to build the map. To plot the map convert the transformation matrix into rigidtform3d object and add the point clouds and the rigidtform3d objects into pcviewset object.

The view set object viewset, now holds views and connections. In the Views table of viewset, the AbsolutePose variable specifies the absolute pose of each view with respect to the first view. Now, build a point cloud map using the created viewset. Align the view absolute poses with the point clouds in the viewset using pcalign. Specify a grid size to control the resolution of the map. The map is returned as a pointCloud object.

tforms = ndtSLAM_mex(location, insDataTable);
% Add results into pcviewset
viewset = pcviewset();
skipFrames = 5;
viewId = 1;
for idx = 1: skipFrames: 2513
    ptCloud = pointCloud(location(:,:,:,idx));
    absTforms = rigidtform3d(tforms(:,:,viewId));
    viewset = addView(viewset, viewId, absTforms, "PointCloud", ptCloud);
    if viewId > 1
        viewset = addConnection(viewset, viewId-1, viewId);
    end
    viewId = viewId + 1;
end

% plot result
ptClouds = viewset.Views.PointCloud;
absPoses = viewset.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(viewset, 'Parent', hAxAfter);

helperMakeFigurePublishFriendly(hFigAfter);

References

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

convertFromSphericalToCartesianCoordinates converts coordinates from spherical to cartesian system.

function xyzData = convertFromSphericalToCartesianCoordinates(rangeData)

xyzData = zeros(size(rangeData),'like',rangeData);

range = rangeData(:, :, 1);
pitch = rangeData(:, :, 2);
yaw   = rangeData(:, :, 3);

xyzData(:, :, 1) = range .* cos(pitch) .* sin(yaw);
xyzData(:, :, 2) = range .* cos(pitch) .* cos(yaw);
xyzData(:, :, 3) = -range .* sin(pitch);
end

helperReadPointCloudFromFile reads pointcloud from PNG image file 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 = convertFromSphericalToCartesianCoordinates(rangeData);

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

helperReadINSConfigFile reads 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

helperReadDataset reads velodyne SLAM dataset 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

helperMakeFigurePublishFriendly adjusts figures so that screenshot captured by publish is correct.

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