detectLoopClosure

Detect loop closure in 2-D lidar scan map

Since R2022b

Syntax

relPose = detectLoopClosure(scanMapObj)

[relPose,scanID,score] = detectLoopClosure(scanMapObj)

[___] = detectLoopClosure(___,Name=Value)

Description

relPose = detectLoopClosure(scanMapObj) searches for a scan matching the most recent scan of the scanMapObj object to detect loop closures in the map. The function returns the relative pose relPose between the matched scan and the most recent scan.

[relPose,scanID,score] = detectLoopClosure(scanMapObj) returns the scan ID of the matched scan and its corresponding correlation score.

[___] = detectLoopClosure(___,Name=Value) specifies options using one or more name-value arguments in addition to any combination of arguments from previous syntaxes. For example, detectLoopClosure(scanMapObj,SearchRadius=10) searches for a matching scan in the map within a 10 meter radius of the most recent scan.

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.

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.

Example: detectLoopClosure(scanMapObj,SearchRadius=10) searches for a matching scan within a 10 meter radius of the most recent scan.

SearchRadius — Search radius to identify matching scan

8 (default) | positive scalar

Search radius to identify a matching scan in the map, specified as a positive scalar. The function searches in the specified radius around the most recent scan. Increasing this value can increase the computation time of the function.

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

MatchThreshold — Minimum correlation score to identify matching scan

100 (default) | positive scalar

Minimum correlation score to identify a matching scan, specified as a positive scalar. A higher value can result a better match and decrease false positives.

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

NumExcludeViews — Number of recently added views to exclude

10 (default) | positive integer

Number of recently added views to exclude for loop closure detection, specified as a positive integer. Increase this value when multiple recently added views belong to the same region of the map.

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

NumMatches — Number of matches to output

1 (default) | positive integer

Number of matches to output, specified as a positive integer.

Note

When this value is greater than 1, the function returns relPose as a matrix, where each row specifies a matching scan. The number of rows in the matrix is equal to the NumMatches value.

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

Output Arguments

relPose — Relative pose between matched scan and most recent scan

three-element vector (default) | matrix

Relative pose between the matched scan and the most recent scan of the map, returned as a three-element vector of the form [x y Θ], where x and y define the translational offset in meters, and Θ defines the rotational offset in radians.

Note

When the value of NumMatches is greater than 1, the function returns relPose as a matrix, where each row specifies a matching scan. The number of rows in the matrix is equal to the NumMatches value.

scanID — Scan ID of matching scan

positive integer | vector

Scan ID of the matching scan in the map, returned as a positive integer.

Note

When the value of NumMatches is greater than 1, the function returns scanID as an M-element vector, where each value corresponds to a matching scan. M is the number of matching scans.

Data Types: double

score — Correlation score of matching scan

positive scalar | vector

Correlation score of the matching scan to the input scan, returned as a positive scalar. A higher score indicates a better match.

Note

When the value of NumMatches is greater than 1, the function returns score as an M-element vector, where each value corresponds to a matching scan. M is the number of matching scans.

Data Types: double

Version History

Introduced in R2022b