scansAndPoses

Extract scans and corresponding poses

Syntax

[scans,poses] = scansAndPoses(slamObj)

[scans,poses] = scansAndPoses(slamObj,nodeIDs)

Description

[scans,poses] = scansAndPoses(slamObj) returns the scans used by the lidarSLAM object as lidarScan objects, along with their associated [x y theta] poses from the underlying pose graph of slamObj.

[scans,poses] = scansAndPoses(slamObj,nodeIDs) returns the scans and poses for the specific node IDs. To get the node IDs, see the underlying poseGraph object in slamObj for the node IDs.

Examples

Perform SLAM Using Lidar Scans

Use a lidarSLAM object to iteratively add and compare lidar scans and build an optimized pose graph of the robot trajectory. To get an occupancy map from the associated poses and scans, use the buildMap function.

Load Data and Set Up SLAM Algorithm

Load a cell array of lidarScan objects. The lidar scans were collected in a parking garage on a Husky® robot from ClearPath Robotics®. Typically, lidar scans are taken at a high frequency and each scan is not needed for SLAM. Therefore, down sample the scans by selecting only every 40th scan.

load garage_fl1_southend.mat scans
scans = scans(1:40:end);

To set up the SLAM algorithm, specify the lidar range, map resolution, loop closure threshold, and search radius. Tune these parameters for your specific robot and environment. Create the lidarSLAM object with these parameters.

maxRange = 19.2; % meters
resolution = 10; % cells per meter

slamObj = lidarSLAM(resolution,maxRange);
slamObj.LoopClosureThreshold = 360;
slamObj.LoopClosureSearchRadius = 8;

Add Scans Iteratively

Using a for loop, add scans to the SLAM object. The object uses scan matching to compare each added scan to previously added ones. To improve the map, the object optimizes the pose graph whenever it detects a loop closure. Every 10 scans, display the stored poses and scans.

for i = 1:numel(scans)

    addScan(slamObj,scans{i});
    
    if rem(i,10) == 0
        show(slamObj);
    end
end
title("Lidar Scans and Poses")
xlabel("X [meters]")
ylabel("Y [meters]")

View Occupancy Map

After adding all the scans to the SLAM object, build an occupancyMap map by calling buildMap with the scans and poses. Use the same map resolution and max range you used with the SLAM object.

[scansSLAM,poses] = scansAndPoses(slamObj);
occMap = buildMap(scansSLAM,poses,resolution,maxRange);
figure
show(occMap)
title('Occupancy Map of Garage')

Input Arguments

slamObj — Lidar SLAM object

lidarSLAM object

Lidar SLAM object, specified as a lidarSLAM object. The object contains the SLAM algorithm parameters, sensor data, and underlying pose graph used to build the map.

nodeIDs — Node IDs from pose graph

positive integer

Node IDs from pose graph, specified as a positive integer. Nodes are added to the pose graph with sequential ID numbers. To get the node IDs, see the underlying poseGraph object in slamObj for the node IDs.

Output Arguments

scans — Lidar scan readings

lidarScan object

Lidar scan readings, returned as a lidarScan object.

poses — Pose for each scan

n-by-3 matrix | [x y theta] vectors

Pose for each scan, returned as an n-by-3 matrix of [x y theta] vectors. Each row is a pose that corresponds to a scan in scans.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Use this syntax when constructing lidarSLAM objects for code generation:

slamObj= lidarSLAM(mapResolution,maxLidarRange,maxNumScans) specifies the upper bound on the number of accepted scans allowed when generating code. maxNumScans is a positive integer. This scan limit is only required when generating code.

Version History

Introduced in R2019b