buildMap

Build occupancy map from lidar scans

Syntax

map = buildMap(scans,poses,mapResolution,maxRange)

Description

map = buildMap(scans,poses,mapResolution,maxRange) creates an occupancyMap map by inserting lidar scans at the given poses. Specify the resolution of the resulting map, mapResolution, and the maximum range of the lidar sensor, maxRange.

Examples

Build Occupancy Map from Lidar Scans and Poses

The buildMap function takes in lidar scan readings and associated poses to build an occupancy grid as lidarScan objects and associated [x y theta] poses to build an occupancyMap.

Load scan and pose estimates collected from sensors on a robot in a parking garage. The data collected is correlated using a lidarSLAM algorithm, which performs scan matching to associate scans and adjust poses over the full robot trajectory. Check to make sure scans and poses are the same length.

load scansAndPoses.mat
length(scans) == length(poses)

ans = logical
   1

Build the map. Specify the scans and poses in the buildMap function and include the desired map resolution (10 cells per meter) and the max range of the lidar (19.2 meters). Each scan is added at the associated poses and probability values in the occupancy grid are updated.

occMap = buildMap(scans,poses,10,19.2);
figure
show(occMap)
title('Occupancy Map of Garage')

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

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

scans — Lidar scans

cell array of lidarScan objects

Lidar scans used to build the map, specified as a cell array of lidarScan objects.

poses — Poses of lidar scans

n-by-3 matrix

Poses of lidar scans, specified as an n-by-3 matrix. Each row is an [x y theta] vector representing the xy-position and orientation angle of a scan.

mapResolution — Resolution of occupancy grid

positive integer

Resolution of the output occupancyMap map, specified as a positive integer in cells per meter.

maxRange — Maximum range of lidar sensor

positive scalar

Maximum range of lidar sensor, specified as a positive scalar in meters. Points in the scans outside this range are ignored.

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Before R2021a, use commas to separate each name and value, and enclose Name in quotes.

Example: ['MapWidth',10]

MapWidth — Width of occupancy grid

positive scalar

Width of the occupancy grid, specified as the comma-separated pair consisting of 'MapWidth' and a positive scalar. If this value is not specified, the map is automatically scaled to fit all laser scans.

MapHeight — Height of occupancy grid

positive scalar

Height of occupancy grid, specified as the comma-separated pair consisting of 'MapHeight' and a positive scalar. If this value is not specified, the map is automatically scaled to fit all laser scans.

Output Arguments

map — Occupancy Map

occupancyMap object

Occupancy map, returned as a occupancyMap object.

Version History

Introduced in R2019b