Code Generation for Map Building from Lidar Data Using SLAM

Since R2024a

This example shows how to generate C++ code for building a map from lidar data using simultaneous localization and mapping (SLAM).

This example uses 3-D lidar data and measurements from an inertial navigation sensor (INS) to progressively build a map using these steps:

• Align successive lidar scans using a point cloud registration technique. This example uses the pcregisterloam function to register scans.

• Generate a map by combining the aligned lidar scans.

The focus of this example is on code generation for building a map. For more information on how to build a map, see the Build a Map from Lidar Data Using SLAMexample.

Download Data

The data used in this example is part of the Velodyne SLAM Dataset, and represents close to 6 minutes of recorded data. Download the data to a temporary directory. 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 is incomplete 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 filenames to the pointCloudTable timetable.

datasetTable = helperReadDataset(dataFolder,pointCloudFilePattern);

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

To pass the point cloud data to the entry-point function, you must first copy the data from the point clouds to an array. To read the point cloud data from the image file, use the helperReadPointCloudFromFile function. This function takes an image filename and returns a pointCloud object. The size of every point cloud is 64-by-870-by-3, and the data contains 2513 point clouds, so the size of the array is 64-by-870-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

Entry-Point Function

This example generates C++ code using the loamSLAM entry-point function. The function takes point cloud locations and INS data as input arguments and returns the absolute transformation for each point cloud.

The function performs these steps:

• To store and manage point cloud data across multiple views, initialize a pcviewset object.

• Initialize a scan context descriptor-based loop closure detector.

• Align successive lidar scans using the LOAM registration technique.

• Detect loop closures using the detectLoop object function.

• Optimize the poses using the optimizePoses function to correct drift in odometry.

type loamSLAM.m

function absTformOut = loamSLAM(location,insDataTable)
%loamSLAM takes point cloud locations and INS data as input arguments and
% aligns successive lidar scans using the LOAM registration method. The
% function performs loop closure detection and pose graph optimization to
% correct any drift in the odometry, and returns the absolute
% transformation for each point cloud.

% Copyright 2023 The MathWorks, Inc.
%#codegen

% Set random seed to ensure reproducibility.
rng("default")

% Create an empty view set.
vSet = pcviewset;

% Initialize point cloud processing parameters.
gridStep = 3;

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

maxTolerableRMSE = 3; % Maximum allowed RMSE for a loop closure candidate to be accepted.
viewId = 1;
skipFrames = 5;
numFrames = size(location,4);

% Preprocess all point cloud
%   - Segment and remove ground plane.
%   - Segment and remove ego vehicle.
parfor i = 1:numFrames
    ptCloudIn = pointCloud(location(:,:,:,i));
    gtc = helperProcessPointCloud(ptCloudIn);
    location(:,:,:,i) = gtc.Location;
end

% Create the point cloud from the first location.
ptCloudOrig = pointCloud(location(:,:,:,1));

% Add first absolute pose as a view to the view set.
vSet = addView(vSet,viewId,absTform);

% Create a loop closure detector.
loopDetector = scanContextLoopDetector;

% Extract the scan context descriptor from the first point cloud.
descriptor = scanContextDescriptor(ptCloudOrig);

% Add the first descriptor to the loop closure detector.
addDescriptor(loopDetector,viewId,descriptor)

viewId = viewId + 1;

for n = 1+skipFrames:skipFrames:numFrames

    % Create the current point cloud.
    ptCloud = pointCloud(location(:,:,:,n));

    % Create the previous point cloud.
    ptCloudPrev = pointCloud(location(:,:,:,n-skipFrames));

    % Detect LOAM points.
    pointsMoving = detectLOAMFeatures(ptCloud);
    pointsFixed = detectLOAMFeatures(ptCloudPrev);

    % Downsample the less planar surface points.
    pointsMoving = downsampleLessPlanar(pointsMoving,gridStep);
    pointsFixed = downsampleLessPlanar(pointsFixed,gridStep);

    % Use INS to estimate an initial transformation for registration.
    initTform = helperComputeInitialEstimateFromINS(relTform, ...
        insDataTable(n-skipFrames:n,:));

    % Compute rigid transformation that registers current LOAM points with
    % previous LOAM points.
    relTform = pcregisterloam(pointsMoving,pointsFixed, ...
        InitialTransform=initTform);

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

    % Add current absolute pose as a view to the view set.
    vSet = addView(vSet,viewId,absTform);

    % Add a connection from the previous view to the current view, representing
    % the relative transformation between them.
    vSet = addConnection(vSet,viewId-1,viewId,relTform);

    % Extract the scan context descriptor from the point cloud.
    descriptor = scanContextDescriptor(ptCloud);

    % Add the descriptor to the loop closure detector.
    addDescriptor(loopDetector,viewId,descriptor)

    % Detect loop closure candidates.
    loopViewId = detectLoop(loopDetector,MaxDetections=1);

    % A loop candidate was found.
    if ~isempty(loopViewId)
        loopViewId = loopViewId(1);

        % Retrieve point cloud from view set.
        ptCloudOld = pointCloud(location(:,:,:,((loopViewId-1)*skipFrames)+1));

        % Use registration to estimate the relative pose.
        [relTform,rmse] = pcregisterloam(ptCloud,ptCloudOld, ...
            gridStep);

        acceptLoopClosure = rmse <= maxTolerableRMSE;
        if acceptLoopClosure
            % For simplicity, use a constant, small information matrix for
            % loop closure edges.
            infoMat = 0.01 * eye(6);

            % Add a connection corresponding to a loop closure.
            vSet = addConnection(vSet,loopViewId,viewId,relTform,infoMat);
        end
    end

    viewId = viewId + 1;
    initTform = relTform;
end

% Optimize the pose graph.
vSetOptim = optimizePoses(vSet);

% Store the optimized poses.
numPoses = numel(vSetOptim.Views.AbsolutePose);
absTformOut = coder.nullcopy(zeros(4,4,numPoses,'single'));

for i = 1:numPoses
    absTformOut(:,:,i) = vSetOptim.Views.AbsolutePose(i).A;
end

end

%%-------------------------------------------------------------------------
function ptCloud = helperProcessPointCloud(ptCloudIn)
%helperProcessPointCloud Processes the pointCloud to remove ground and ego vehicle
%   ptCloud = helperProcessPointCloud(ptCloudIn) processes
%   ptCloudIn by removing the ground plane and the ego vehicle.

elevationAngleDelta = coder.const(11);

% Segment ground using floodfill on range image.
groundFixedIdx = segmentGroundFromLidarData(ptCloudIn, ...
    ElevationAngleDelta=elevationAngleDelta);
groundFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2));
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 = findPointsInCylinder(ptCloudIn,radius,Center=sensorLocation);
egoFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2));
egoFixed(egoFixedIdx) = true;

% Retain subset of point cloud without ground and ego vehicle.
indices = ~groundFixed & ~egoFixed;

ptCloud = select(ptCloudIn,indices,OutputSize="full");
end

%%-------------------------------------------------------------------------
function initTform = helperComputeInitialEstimateFromINS(initTform,insData)
%helperComputeInitialEstimateFromINS Computes an estimate 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 = coder.const([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 C++ Code

To generate C++ code from the loamSLAM entry-point function, first create a code configuration object for a MEX target, and set the target language to C++. Then, generate C++ code using the codegen function, which produces a MEX file, loamSLAM_mex, in the current working folder. For more information on generating C++ code and specifying options for code generation, see the codegen (MATLAB Coder) function.

cfg = coder.config("mex");
cfg.TargetLang = "C++";
codegen -config cfg loamSLAM -args {location,insDataTable}

Code generation successful.

Run Generated MEX File

Run the generated MEX file, and display the results using the helperSLAMVisualization function.

absTformOut = loamSLAM_mex(location,insDataTable);
helperSLAMVisualization(location,absTformOut)

Supporting Functions and Classes

helperReadDataset — Reads data from the specified folder 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 by dataFolder into a timetable. The function
%   expects data from the Velodyne SLAM Dataset.
%
%   See also fileDatastore, helperReadINSConfigFile.
%
%   This is an example helper class that is subject to change or removal in
%   future releases.

% Copyright 2019-2023 The MathWorks, Inc.

% 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 unused columns.
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

helperReadPointCloudFromFile — Reads a point cloud from a 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.
%   The image file must be from the Velodyne SLAM Dataset.

% Copyright 2019-2023 The MathWorks, Inc.

% From DATAFORMAT.txt
% -------------------
% Each 360° revolution of the Velodyne scanner is stored as a 16-bit png
% distance image (scan*.png). The scanner turns 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
% have not been corrected for vehicle movement. Due to this, and 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 a result, best practice is to
% ignore the 10 leftmost and rightmost columns of the image. To convert the
% pixel values [0,65535] into meters, divide by 500. This results in
% an effective range of [0,131] meters. Invalid measurements have a distance 
% of 0.

% To convert the distance values into 3-D coordinates, use the setup in
% "img.cfg". The yaw angles (counter-clockwise) map linearly 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,-180]. These
% 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

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

helperReadINSConfigFile — 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.

% Copyright 2019-2023 The MathWorks, Inc.

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

helperSLAMVisualization — Visualizes the point cloud map.

function helperSLAMVisualization(location,absTformOut)
%helperSLAMVisualization function takes location data of point clouds 
% and the absolute transformation for each point cloud as input 
% arguments, and visualizes the point cloud map with view set 
% connections overlaid on the map.
%
% This is a helper function for example purposes and may be removed or
% modified in the future.

% Copyright 2023 The MathWorks, Inc.

% Initialize the view set.
vSet = pcviewset;
skipFrames = 5;
viewId = 1;

for idx = 1:skipFrames:size(location,4)
    ptCloud = pointCloud(location(:,:,:,idx));
    absTform = rigidtform3d(absTformOut(:,:,viewId));
    vSet = addView(vSet,viewId,absTform,PointCloud=ptCloud);
    if viewId > 1
        vSet = addConnection(vSet,viewId-1,viewId);
    end
    viewId = viewId + 1;
end

% Create point cloud map based on point clouds and absolute transformations
% present in the view set.
ptClouds = vSet.Views.PointCloud;
absPoses = vSet.Views.AbsolutePose;
mapGridSize = 0.2;
ptCloudMap = pcalign(ptClouds,absPoses,mapGridSize);

hFigAfter = figure(Name="Lidar SLAM");
hAxAfter = axes(hFigAfter);
pcshow(ptCloudMap,Parent=hAxAfter)

% Overlay view set display.
hold on
plot(vSet,Parent=hAxAfter)
hold off
end