Load Laser Scans

This example uses data collected in an indoor environment using a Jackal™ robot from Clearpath Robotics™. The robot is equipped with a SICK™ TiM-511 laser scanner with a maximum range of 10 meters. Load the wareHouse.mat file containing the laser scans into the workspace.

data = load("wareHouse.mat");
scans = data.wareHouseScans;

Estimate Robot Trajectory

Create a lidarscanmap object. Using this object, you can:

Specify the maximum lidar range and grid resolution values. You can modify these values to fine-tune the map of the environment. Use these values to create the lidar scan map.

maxLidarRange = 8;
gridResolution = 20;
mapObj = lidarscanmap(gridResolution,maxLidarRange);

Incrementally add scans from the input data to the lidar scan map object by using the addScan function. This function rejects scans if they are too close to consecutive scans.

for i = 1:numel(scans)
    isScanAccepted = addScan(mapObj,scans{i});
     if ~isScanAccepted
        continue;
     end
end

Reconstruct the scene by plotting the scans and poses tracked by the lidar scan map.

hFigMap = figure;
axMap = axes(Parent=hFigMap);
show(mapObj,Parent=axMap);
title(axMap,"Map of the Environment and Robot Trajectory")

Notice that the estimated robot trajectory drifts over time. The drift can be due to any of these reasons:

Drift in the estimated trajectory results in an inaccurate map of the environment.

Drift Correction

Correct the drift in trajectory by accurately detecting the loops, which are the places the robot returns to, after previously visiting. Add the loop closure edges to the lidarscanmap object to correct the drift in trajectory during pose graph optimization.

Loop Closure Detection

Loop closure detection determines whether, for a given scan, the robot has previously visited the current location. The search consists of matching the current scan against the previous scans around the current robot location, within the specified radius loopClosureSearchRadius. Accept a scan as a match if the match score is greater than the specified threshold loopClosureThreshold.

You can detect loop closures by using the detectLoopClosure function of the lidarscanmap object and the add them to the map object by using the addLoopClosure function.

You can increase the loopClosureThreshold value to avoid false positives in loop closure detection, but the function can still return bad matches in environments with similar or repeated features. To address this, increase the loopClosureSearchRadius value to search a larger radius around the current scan for loop closures, though this increases computation time.

You can also specify the number of loop closure matches loopClosureNumMatches. All of these parameters help in fine-tuning the loop closure detections.

loopClosureThreshold = 110;
loopClosureSearchRadius = 2;
loopClosureNumMatches = 1;
mapObjLoop = lidarscanmap(gridResolution,maxLidarRange);
for i = 1:numel(scans)
    isScanAccepted = addScan(mapObjLoop,scans{i});
    % Detect loop closure if scan is accepted
     if isScanAccepted
         [relPose,matchScanId] = detectLoopClosure(mapObjLoop, ...
             MatchThreshold=loopClosureThreshold, ...
             SearchRadius=loopClosureSearchRadius, ...
             NumMatches=loopClosureNumMatches);
         % Add loop closure to map object if relPose is estimated
         if ~isempty(relPose)
            addLoopClosure(mapObjLoop,matchScanId,i,relPose);
         end
     end
end

Optimize Trajectory

Create a pose graph object from the drift-corrected lidar scan map by using the poseGraph function. Use the optimizePoseGraph (Navigation Toolbox) function to optimize the pose graph.

pGraph = poseGraph(mapObjLoop);
updatedPGraph = optimizePoseGraph(pGraph);

Extract the optimized absolute poses from the pose graph by using the nodeEstimates (Navigation Toolbox) function, and update the trajectory to build an accurate map of the environment.

optimizedScanPoses = nodeEstimates(updatedPGraph);
updateScanPoses(mapObjLoop,optimizedScanPoses);

Visualize Results

Visualize the change in robot trajectory before and after pose graph optimization. The red lines represent loop closure edges.

hFigTraj = figure(Position=[0 0 900 450]);

% Visualize robot trajectory before optimization
axPGraph = subplot(1,2,1,Parent=hFigTraj);
axPGraph.Position = [0.04 0.1 0.45 0.8];
show(pGraph,IDs="off",Parent=axPGraph);
title(axPGraph,"Before PGO")

% Visualize robot trajectory after optimization
axUpdatedPGraph = subplot(1,2,2,Parent=hFigTraj);
axUpdatedPGraph.Position = [0.54 0.1 0.45 0.8];
show(updatedPGraph,IDs="off",Parent=axUpdatedPGraph);
title(axUpdatedPGraph,"After PGO")
axis([axPGraph axUpdatedPGraph],[-6 10 -7 3])
sgtitle("Robot Trajectory",FontWeight="bold")

Visualize the map of the environment and robot trajectory before and after pose graph optimization.

hFigMapTraj = figure(Position=[0 0 900 450]);

% Visualize map and robot trajectory before optimization
axOldMap = subplot(1,2,1,Parent=hFigMapTraj);
axOldMap.Position = [0.05 0.1 0.44 0.8];
show(mapObj,Parent=axOldMap);
title(axOldMap,"Before PGO")

% Visualize map and robot trajectory after optimization
axUpdatedMap = subplot(1,2,2,Parent=hFigMapTraj);
axUpdatedMap.Position = [0.56 0.1 0.44 0.8];
show(mapObjLoop,Parent=axUpdatedMap);
title(axUpdatedMap,"After PGO")
axis([axOldMap axUpdatedMap],[-9 18 -10 9])
sgtitle("Map of the Environment and Robot Trajectory",FontWeight="bold")