Main Content

lidarSLAM

Perform localization and mapping using lidar scans

expand all in page

Description

The lidarSLAM class performs simultaneous localization and mapping (SLAM) for lidar scan sensor inputs. The SLAM algorithm takes in lidar scans and attaches them to a node in an underlying pose graph. The algorithm then correlates the scans using scan matching. It also searches for loop closures, where scans overlap previously mapped regions, and optimizes the node poses in the pose graph.

Creation

Syntax

slamObj = lidarSLAM
slamObj = lidarSLAM(mapResolution,maxLidarRange)
slamObj = lidarSLAM(mapResolution,maxLidarRange,maxNumScans)

Description

slamObj = lidarSLAM creates a lidar SLAM object. The default occupancy map size is 20 cells per meter. The maximum range for each lidar scan is 8 meters.

slamObj = lidarSLAM(mapResolution,maxLidarRange) creates a lidar SLAM object and sets the MapResolution and MaxLidarRange properties based on the inputs.

example

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.

Properties

expand all

Underlying pose graph that connects scans, specified as a poseGraph object. Adding scans to lidarSLAM updates this pose graph. When loop closures are found, the pose graph is optimized using OptimizationFcn.

Resolution of the occupancy grid map, specified as a positive integer in cells per meter. Specify the map resolution on construction.

Maximum range of the lidar sensor, specified as a positive scalar in meters. Specify the maximum range on construction.

Pose graph optimization function, specified as a function handle. By default, the algorithm calls the optimizePoseGraph function. To specify your own optimization method, the class requires the function signature to be:

[updatedPose,stat] = myOptimizationFcn(poseGraph)
poseGraph is a poseGraph object. updatedPose is an n-by-3 vector of [x y theta] poses listed in sequential node ID order. stat is a structure containing a ResidualError field as a positive scalar. Use the stat structure to include other information relevant to your optimization.

Threshold on the score from the scan matching algorithm for accepting loop closures, specified as a positive scalar. Higher thresholds correspond to a better match, but scores vary based on sensor data.

Search radius for loop closure detection, specified as a positive scalar. Increasing this radius affects performance by increasing search time. Tune this distance based on your environment and the expected vehicle trajectory.

Number of attempts at finding looping closures, specified as a positive integer. Increasing the number of attempts affects performance by increasing search time.

Allow automatic rollback of added loop closures, specified as true or false. The SLAM object tracks the residual error returned by the OptimizationFcn. If it detects a sudden change in the residual error and this property is true, it rejects (rolls back) the loop closure.

Number of loop closures accepted to trigger optimization, specified as a positive integer. By default, the PoseGraph is optimized every time lidarSLAM adds a loop closure.

Minimum change in pose required to process scans, specified as a [translation rotation] vector. A relative pose change for a newly added scan is calculated as [x y theta]. If the translation in xy-position or rotation of theta exceeds these thresholds, the lidarSLAM object accepts the scan and adds a pose is added to the PoseGraph.

Scan registration method, specified as a character vector.

Note

Image Processing Toolbox™ is required for using Phase Correlation method.

Incremental match translational search range, specified as a two-element vector of the form [x y] in meters. This property is only applicable when the ScanRegistrationMethod property is set to 'BranchAndBound'.

These values define the search window around the initial translation estimate specified in the relPoseEst argument of the addScan function. Set the value of this property to the maximum expected translation between consecutive accepted scans.

This property is similar to the 'TranslationSearchRange' name-value pair argument in matchScansGrid function.

Incremental match rotational search range, specified as positive scalar in radians. This property is only applicable when the ScanRegistrationMethod property is set to 'BranchAndBound'.

This values define the search window around the initial rotation estimate specified in the relPoseEst argument of the addScan function. Set the value of this property to the maximum expected rotation between consecutive accepted scans.

This property is similar to the 'RotationSearchRange' name-value pair argument in matchScansGrid function.

Object Functions

addScanAdd scan to lidar SLAM map
copyCopy lidar SLAM object
removeLoopClosures Remove loop closures from pose graph
scansAndPoses Extract scans and corresponding poses
showPlot scans and robot poses

Examples

collapse all

Open Live Script

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]")

Figure contains an axes object. The axes object with title Lidar Scans and Poses, xlabel X [meters], ylabel Y [meters] contains 121 objects of type line.

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

Figure contains an axes object. The axes object with title Occupancy Map of Garage, xlabel X [meters], ylabel Y [meters] contains an object of type image.

More About

expand all

References

[1] Hess, Wolfgang, Damon Kohler, Holger Rapp, and Daniel Andor. "Real-Time Loop Closure in 2D LIDAR SLAM." 2016 IEEE International Conference on Robotics and Automation (ICRA). 2016.

Extended Capabilities

Version History

Introduced in R2019b

See Also

|

Topics