addScan

Add scan to lidar SLAM map

Syntax

addScan(slamObj,currScan)

addScan(slamObj,currScan,relPoseEst)

[isAccepted,loopClosureInfo,optimInfo] = addScan(___)

Description

addScan(slamObj,currScan) adds a lidar scan, currScan, to the lidar SLAM object, slamObj. The function uses scan matching to correlate this scan to the most recent one, then adds it to the pose graph defined in slamObj. If the scan is accepted, addScan detects loop closures and optimizes based on settings in slamObj.

addScan(slamObj,currScan,relPoseEst) also specifies a relative pose to the latest lidar scan pose in slamObj. This relative pose improves the scan matching.

Note

The relPoseEst input is ignored when the ScanRegistrationMethod property of lidarSLAM object is set to 'PhaseCorrelation'.

[isAccepted,loopClosureInfo,optimInfo] = addScan(___) outputs detailed information about adding the scan to the SLAM object. isAccepted indicates if the scan is added or rejected. loopClosureInfo and optimInfo indicate if a loop closure is detected or the pose graph is optimized.

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.

currScan — Lidar scan reading

lidarScan object

Lidar scan reading, specified as a lidarScan object. This scan is correlated to the most recent scan in slamObj using scan matching.

relPoseEst — Relative pose estimate of scan

[x y theta] vector

Relative pose estimate of scan, specified as an [x y theta] vector. This relative pose improves scan matching.

Output Arguments

isAccepted — Indicates if scan is accepted

true | false

Indicates if scan is accepted, returned as true or false. If the relative pose between scans is below the MovementThreshold property of slamObj, the scan is rejected. By default, all scans are accepted.

loopClosureInfo — Loop closure details

structure

Loop closure details, returned as a structure with these fields:

• EdgeIDs –– IDs of newly connected edges in the pose graph, returned as a vector.

• Edges –– Newly added loop closure edges, returned as an n-by-2 matrix of node IDs that each edge connects.

• Scores –– Scores of newly connected edges in the pose graph returned from scan matching, returned as a vector.

Note

If the LoopClosureAutoRollback property is set to true in slamObj, loop closure edges can be removed from the pose graph. This property rejects loops closures if the residual error changes drastically after optimization. Therefore, some of the edge IDs listed in this structure may not exist in the actual pose graph.

optimInfo — Pose graph optimization details

structure

Pose graph optimization details, returned as a structure with these fields:

• IsPerformed –– Boolean indicating if optimization is performed when adding this scan. Optimization performance depends on the OptimizationInterval property in slamObj.

• IsAccepted –– Boolean indicating if optimization was accepted based on ResidualError.

• ResidualError –– Error associated with optimization, returned as a scalar.

• LoopClosureRemoved –– List of IDs of loop closure edges removed during optimization, returned as a vector.

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