addLoopClosure

Add loop closure to map

Since R2022b

Syntax

addLoopClosure(scanMapObj,fromScanID,toScanID,relPose)

addLoopClosure(scanMapObj,fromScanID,toScanID,relPose,informationMatrix)

Description

addLoopClosure(scanMapObj,fromScanID,toScanID,relPose) adds a loop closure between the scans with scan IDs fromScanID and toScanID to the lidarscanmap object, where relPose specifies the relative pose between the scans.

addLoopClosure(scanMapObj,fromScanID,toScanID,relPose,informationMatrix) specifies the information matrix that represents the uncertainty in the relative pose measurement.

Examples

Detect and Add Loop Closures to Map

Load a MAT file containing 2-D lidar scans into the workspace.

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

Create a lidarscanmap object.

scanMapObj = lidarscanmap;

Add the first 70 scans from the input data to the scanMapObj object by using the addScan function.

for currentID = 1:70
    addScan(scanMapObj,scans{currentID});
end

Detect loop closures in the map by using the detectLoopClosure function. The function searches the map for a previous scan matching the most recent scan. For best results, adjust the excluded views and the search radius according to the sensor trajectory.

[~,scanID,~] = detectLoopClosure(scanMapObj,NumExcludeViews=20,SearchRadius=5)

scanID = 
11

Add the loop closure detected between the scans 70 and 11 to the scanMapObj by using the addLoopClosure function. Display the loop closure connection.

addLoopClosure(scanMapObj,70,11,[2 2 0])
disp(scanMapObj.LoopClosureIDs)

    FromScanID    ToScanID
    __________    ________

        70           11   

Visualize the map and the sensor trajectory.

figure
show(scanMapObj);

Input Arguments

scanMapObj — 2-D lidar scan map

lidarscanmap object

2-D lidar scan map, specified as a lidarscanmap object.

fromScanID — Scan ID of scan at beginning of loop closure

positive integer | vector

Scan ID of the scan at the beginning of the loop closure, specified as a positive integer or vector. To add multiple loop closure connections in the map, you must specify the value as a vector.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

toScanID — Scan ID of scan at end of loop closure

positive integer | vector

Scan ID of the scan at the end of the loop closure, specified as a positive integer or vector. To add multiple loop closure connections in the map, you must specify the value as a vector.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

relPose — Relative pose between scans

three-element vector | M-by-3 matrix

Relative pose between the scans in the loop closure, specified as a three-element vector or an M-by-3 matrix. To add more than one loop closure connection, specify the value as a matrix, where M is the number of loop closure connections. Each row of the matrix is of the form [x y Θ], where x and y define the translational offset in meters, and Θ defines the rotational offset between the scans in radians.

informationMatrix — Uncertainty in relative pose measurement

M-element cell array

Uncertainty in the relative pose measurement, specified as an M-element cell array, where M is the number of loop closure connections. Each cell contains a 3-by-3 positive definite matrix.

Version History

Introduced in R2022b