%% 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 intial estiamte. 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 intial 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                                  

% 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 intial 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

% Copyright 2019 The MathWorks, Inc.