addScan

Add scan to lidar SLAM map

collapse all in page

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

example

[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

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, downsample 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.

Input Arguments

collapse all

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.

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

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

Output Arguments

collapse all

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.

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.

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

expand all

Version History

Introduced in R2019b

See Also

|

Topics