matchScansLine

Estimate pose between two laser scans using line features

Syntax

relpose = matchScansLine(currScan,refScan,initialRelPose)

[relpose,stats] = matchScansLine(___)

[relpose,stats,debugInfo] = matchScansLine(___)

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

Description

relpose = matchScansLine(currScan,refScan,initialRelPose) estimates the relative pose between two scans based on matched line features identified in each scan. Specify an initial guess on the relative pose, initialRelPose.

[relpose,stats] = matchScansLine(___) returns additional information about the covariance and exit condition in stats as a structure using the previous inputs.

[relpose,stats,debugInfo] = matchScansLine(___) returns additional debugging info, debugInfo, from the line-based scan matching result.

[___] = matchScansLine(___,Name,Value) specifies options using one or more Name,Value pair arguments.

Examples

Estimate Pose of Scans with Line Features

This example shows how to use the matchScansLine function to estimate the relative pose between lidar scans given an initial estimate. The identified line features are visualized to show how the scan-matching algorithm associates features between scans.

Load a pair of lidar scans. The .mat file also contains an initial guess of the relative pose difference, initGuess, which could be based on odometry or other sensor data.

load tb3_scanPair.mat
plot(s1)
hold on
plot(s2)
hold off

Set parameters for line feature extraction and association. The noise of the lidar data determines the smoothness threshold, which defines when a line break occurs for a specific line feature. Increase this value for more noisy lidar data. The compatibility scale determines when features are considered matches. Increase this value for looser restrictions on line feature parameters.

smoothnessThresh = 0.2;
compatibilityScale = 0.002;

Call matchScansLine with the given initial guess and other parameters specified as name-value pairs. The function calculates line features for each scan, attempts to match them, and uses an overall estimate to get the difference in pose.

[relPose, stats, debugInfo] = matchScansLine(s2, s1, initGuess, ...
                                       'SmoothnessThreshold', smoothnessThresh, ...
                                       'CompatibilityScale', compatibilityScale);

After matching the scans, the debugInfo output gives you information about the detected line feature parameters, [rho alpha], and the hypothesis of which features match between scans.

debugInfo.MatchHypothesis states that the first, second, and sixth line feature in s1 match the fifth, second, and fourth features in s2.

debugInfo.MatchHypothesis                                  

ans = 1×6

     5     2     0     0     0     4

The provided helper function plots these two scans and the features extracted with labels. s2 is transformed to be in the same frame based on the initial guess for relative pose.

exampleHelperShowLineFeaturesInScan(s1, s2, debugInfo, initGuess);

Use the estimated relative pose from matchScansLine to transform s2. Then, plot both scans to show that the relative pose difference is accurate and the scans overlay to show the same environment.

s2t = transformScan(s2,relPose);
clf
plot(s1)
hold on
plot(s2t)
hold off

Input Arguments

currScan — Current lidar scan readings

lidarScan object

Current lidar scan readings, specified as a lidarScanobject.

Your lidar scan can contain Inf and NaN values, but the algorithm ignores them.

refScan — Reference lidar scan readings

lidarScan object

Reference lidar scan readings, specified as a lidarScan object.

Your lidar scan can contain Inf and NaN values, but the algorithm ignores them.

initialRelPose — Initial guess of relative pose

[x y theta]

Initial guess of the current pose relative to the reference laser scan frame, specified an [x y theta] vector. [x y] is the translation in meters and theta is the rotation in radians.

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: "LineMergeThreshold",[0.10 0.2]

SmoothnessThreshold — Threshold to detect line break points in scan

0.1 (default) | scalar

Threshold to detect line break points in scan, specified as a scalar. Smoothness is defined by calling diff(diff(scanData)) and assumes equally spaced scan angles. Scan points corresponding to smoothness values higher than this threshold are considered break points. For lidar scan data with a higher noise level, increase this threshold.

MinPointsPerLine — Minimum number of scan points in each line feature

10 (default) | positive integer greater than 3

Minimum number of scan points in each line feature, specified as a positive integer greater than 3.

A line feature cannot be identified from a set of scan points if the number of points in that set is below this threshold. When the lidar scan data is noisy, setting this property too small may result in low-quality line features being identified and skew the matching result. On the other hand, some key line features may be missed if this number is set too large.

LineMergeThreshold — Threshold on line parameters to merge line features

[0.05 0.1] (default) | two-element vector [rho alpha]

Threshold on line parameters to merge line features, specified as a two-element vector [rho alpha]. A line is defined by two parameters:

• rho –– Distance from the origin to the line along a vector perpendicular to the line, specified in meters.

• alpha –– Angle between the x-axis and the rho vector, specified in radians.

If the difference between these parameters for two line features is below the given threshold, the line features are merged.

MinCornerPromenance — Lower bound on prominence value to detect a corner

0.05 (default) | positive scalar

Lower bound on prominence value to detect a corner, specified as a positive scalar.

Prominence measures how much a local extrema stands out in the lidar data. Only values higher than this lower bound are considered a corner. Corners help identify line features, but are not part of the feature itself. For noisy lidar scan data, increase this lower bound.

CompatibilityScale — Scale used to adjust the compatibility thresholds for feature association

0.0005 (default) | positive scalar

Scale used to adjust the compatibility thresholds for feature association, specified as a positive scalar. A lower scale means tighter compatibility threshold for associating features. If no features are found in lidar data with obvious line features, increase this value. For invalid feature matches, reduce this value.

Output Arguments

relpose — Pose of current scan

[x y theta]

Pose of current scan relative to the reference scan, returned as [x y theta], where [x y] is the translation in meters and theta is the rotation in radians.

stats — Scan matching information

structure

Scan matching information, returned as a structure with the following fields:

• Covariance –– 3-by-3 matrix representing the covariance of the relative pose estimation. The matScansLine function does not provide covariance between the (x,y) and the theta components of the relative pose. Therefore, the matrix follows the pattern: [Cxx, Cxy 0; Cyx Cyy 0; 0 0 Ctheta].

• ExitFlag –– Scalar value indicating the exit condition of the solver:

  • 0 –– No error.

  • 1 –– Insufficient number of line features (< 2) are found in one or both of the scans. Consider using different scans with more line features.

  • 2 –– Insufficient number of line feature matches are identified. This may indicate the initialRelPose is invalid or scans are too far apart.

debugInfo — Debugging information for line-based scan matching result

structure

Debugging information for line-based scan matching result, returned as a structure with the following fields:

• ReferenceFeatures –– Line features extracted from the reference scan as an n-by-2 matrix. Each line feature is represented as [rho alpha] for the parametric equation, rho = x∙cos(alpha) + y∙sin(alpha).

• ReferenceScanMask –– Mask indicating which points in the reference scan are used for each line feature as an n-by-p matrix. Each row corresponds to a row in ReferenceFeatures and contains zeros and ones for each point in refScan.

• CurrentFeatures –– Line features extracted from the current scan as an n-by-2 matrix. Each line feature is represented as [rho alpha] for the parametric equation, rho = x∙cos(alpha) + y∙sin(alpha).

• CurrentScanMask –– Mask indicating which points in the current scan are used for each line feature as an n-by-p matrix. Each row corresponds to a row in ReferenceFeatures and contains zeros and ones for each point in refScan.

• MatchHypothesis –– Best line feature matching hypothesis as an n element vector, where n is the number of line features in CurrentFeatures. Each element represents the corresponding feature in ReferenceFeaturesand gives the index of the matched feature in ReferenceFeatures is an index match the

• MatchValue –– Scalar value indicating a score for each MatchHypothesis. A lower value is considered a better match. If two elements of MatchHypothesis have the same index, the feature with a lower score is used.

References

[1] Neira, J., and J.d. Tardos. “Data Association in Stochastic Mapping Using the Joint Compatibility Test.” IEEE Transactions on Robotics and Automation 17, no. 6 (2001): 890–97. https://doi.org/10.1109/70.976019.

[2] Shen, Xiaotong, Emilio Frazzoli, Daniela Rus, and Marcelo H. Ang. “Fast Joint Compatibility Branch and Bound for Feature Cloud Matching.” 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2016. https://doi.org/10.1109/iros.2016.7759281.

Version History

Introduced in R2020a