Main Content

addLoopClosure

Add loop closure to the map

Description

example

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

collapse all

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

Figure contains an axes object. The axes object contains 71 objects of type line.

Input Arguments

collapse all

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

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

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

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.

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