matchScansLine
Syntax
Description
estimates the relative pose between two scans based on matched line features identified in
each scan. Specify an initial guess on the relative pose,
relpose
= matchScansLine(currScan
,refScan
,initialRelPose
)initialRelPose
.
[___] = matchScansLine(___,
specifies options using one or more name-value pair arguments.Name,Value
)
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 lidarScan
object.
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 thex
-axis and therho
vector, specified in radians.
If the difference between these parameters for two line features is below the given threshold, the line features are merged.
MinCornerProminence
— 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. ThematScansLine
function does not provide covariance between the(x,y)
and thetheta
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 theinitialRelPose
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 inReferenceFeatures
and contains zeros and ones for each point inrefScan
.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 inReferenceFeatures
and contains zeros and ones for each point inrefScan
.MatchHypothesis
–– Best line feature matching hypothesis as an n element vector, where n is the number of line features inCurrentFeatures
. Each element represents the corresponding feature inReferenceFeatures
and gives the index of the matched feature inReferenceFeatures
is an index match theMatchValue
–– Scalar value indicating a score for eachMatchHypothesis
. A lower value is considered a better match. If two elements ofMatchHypothesis
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 R2020b
See Also
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)