Evaluate and Visualize Lane Detections Against Ground Truth

% Load MAT file with ground truth data.
loaded = load('roadSequence_laneAndVehicleGroundTruth.mat');

% load Image Sequence.
roadSequenceData = fullfile(toolboxdir("pointcloud"),"pcdata","roadSequence");
imds = imageDatastore(roadSequenceData);

% Load the labeled per-frame ground truth data for analysis. The ground truth data is stored as a timetable..
gtdata = loaded.groundTruthData;

% Display the first few rows of the ground truth data.
head(gtdata)

% Read the first frame of the image sequence.
frameInd = 1;
frame = imread(imds.Files{frameInd});

% Extract all lane points in the first frame.
lanePoints = gtdata.LaneBoundaries{1};

% Extract vehicle bounding boxes in the first frame.
vehicleBBox = gtdata.Vehicles{1};

% Superimpose the right lane points and vehicle bounding boxes.
frame = insertMarker(frame, lanePoints{2}, 'X');
frame = insertObjectAnnotation(frame, 'rectangle', vehicleBBox, 'Vehicle');

% Display ground truth data on the first frame.
figure
imshow(frame)

% Set up monoSensorHelper to process image sequence.
monoCameraSensor = loaded.sensor;
monoSensorHelper = helperMonoSensor(monoCameraSensor);

% Create new timetable with same Time vector for measurements.
measurements = timetable(gtdata.Time);

% Set up timetable columns for holding lane boundary and vehicle data.
numFrames = numel(imds.Files);
measurements.LaneBoundaries    = cell(numFrames, 2);
measurements.VehicleDetections = cell(numFrames, 1);
gtdata.LanesInVehicleCoord     = cell(numFrames, 2);

% Rewind the image sequence to t = 0, and create a frame index to hold current
% frame.
frameIndex  = 0;

% Loop through the image sequenceFile until there are no new frames.
for i = 1 : numel(imds.Files)
    frameIndex = frameIndex+1;
    frame      = imread(imds.Files{frameIndex});

    % Use the processFrame method to compute detections.
    % This method can be replaced with a custom lane detection method.
    detections = processFrame(monoSensorHelper, frame);

    % Store the estimated lane boundaries and vehicle detections.
    measurements.LaneBoundaries{frameIndex} = [detections.leftEgoBoundary ...
        detections.rightEgoBoundary];
    measurements.VehicleDetections{frameIndex} = detections.vehicleBoxes;

    % To facilitate comparison, convert the ground truth lane points to the
    % vehicle coordinate system.
    gtPointsThisFrame = gtdata.LaneBoundaries{frameIndex};
    vehiclePoints = cell(1, numel(gtPointsThisFrame));
    for ii = 1:numel(gtPointsThisFrame)
        vehiclePoints{ii} = imageToVehicle(monoCameraSensor, gtPointsThisFrame{ii});
    end

    % Store the ground truth points in vehicle coordinates in the LanesInVehicleCoord column of the gtdata timetable for Bird's-Eye View visualization later.
    gtdata.LanesInVehicleCoord{frameIndex} = vehiclePoints;
end

% Rewind image sequence to t = 0.
frameIndex  = 0;

% Read the first frame of the image sequence.
frame = imread(imds.Files{1});
birdsEyeImage = transformImage(monoSensorHelper.BirdsEyeConfig, frame);

% Extract right lane points for the first frame in Bird's-Eye View.
firstFrameVehiclePoints = gtdata.LanesInVehicleCoord{1};
pointsInBEV = vehicleToImage(monoSensorHelper.BirdsEyeConfig, firstFrameVehiclePoints{2});

% Superimpose points on the frame.
birdsEyeImage = insertMarker(birdsEyeImage, pointsInBEV, 'X', 'Size', 6);

% Display transformed points in Bird's-Eye View.
figure
imshow(birdsEyeImage)

threshold = 0.25; % in meters

[numMatches, numMisses, numFalsePositives, assignments] = ...
    evaluateLaneBoundaries(measurements.LaneBoundaries, ...
    gtdata.LanesInVehicleCoord, ...
    threshold);

disp(['Number of matches: ', num2str(numMatches)]);

averageDistance = helperComputeLaneStatistics(measurements.LaneBoundaries, ...
    gtdata.LanesInVehicleCoord, ...
    assignments, @mean);

% Plot average distance between estimates and ground truth.
figure
stem(gtdata.Time, averageDistance)
title('Average Distance Between Estimates and Ground Truth')
grid on
ylabel('Distance in Meters')
legend('Left Boundary','Right Boundary')

% Compute the estimated point locations using the monoCamera.
[estVehiclePoints, estImagePoints] = helperGetCorrespondingPoints(monoCameraSensor, ...
    measurements.LaneBoundaries, ...
    gtdata.LanesInVehicleCoord, ...
    assignments);

% Add estimated lanes to the measurements timetable.
measurements.EstimatedLanes      = estImagePoints;
measurements.LanesInVehicleCoord = estVehiclePoints;

% Create a new timetable with all the variables needed for visualization.
names = {'LanePoints'; 'DetectedLanePoints'};
types = labelType({'Line'; 'Line'});
labelDefs = table(names, types, 'VariableNames', {'Name','Type'});

visualizeInFrame = timetable(gtdata.Time, ...
    gtdata.LaneBoundaries, ...
    measurements.EstimatedLanes, ...
    'VariableNames', names);

% Create groundTruth object.
dataSource = groundTruthDataSource(roadSequenceData);
dataToVisualize = groundTruth(dataSource, labelDefs, visualizeInFrame);

% Save all the results of the previous section in distanceData.mat in a
% temporary folder.
dataToLoad  = fullfile(tempdir,'distanceData.mat');
save(dataToLoad, 'monoSensorHelper', 'roadSequenceData', 'measurements', 'gtdata', 'averageDistance');

function stat = helperComputeLaneStatistics(estModels, gtPoints, assignments, fcnHandle)

numFrames = length(estModels);
% Make left and right estimates NaN by default to represent lack of
% data.
stat = NaN(numFrames,2);

for frameInd = 1:numFrames
    % Make left and right estimates NaN by default.
    stat(frameInd, :) = NaN(2, 1);

    for idx = 1:length(estModels{frameInd})
        % Ignore false positive assignments.
        if assignments{frameInd}(idx) == 0
            continue;
        end

        % The kth boundary in estModelInFrame is matched to kth
        % element indexed by assignments in gtPointsInFrame.
        thisModel = estModels{frameInd}(idx);
        thisGT = gtPoints{frameInd}{assignments{frameInd}(idx)};
        thisGTModel = driving.internal.piecewiseLinearBoundary(thisGT);
        if mean(thisGTModel.Points(:,2)) > 0
            % left lane
            xPoints = thisGTModel.Points(:,1);
            yDist = zeros(size(xPoints));
            for index = 1:numel(xPoints)
                gtYPoints   = thisGTModel.computeBoundaryModel(xPoints(index));
                testYPoints = thisModel.computeBoundaryModel(xPoints(index));
                yDist(index) = abs(testYPoints-gtYPoints);
            end
            stat(frameInd, 1) = fcnHandle(yDist);
        else % right lane
            xPoints = thisGTModel.Points(:,1);
            yDist = zeros(size(xPoints));
            for index = 1:numel(xPoints)
                gtYPoints   = thisGTModel.computeBoundaryModel(xPoints(index));
                testYPoints = thisModel.computeBoundaryModel(xPoints(index));
                yDist(index) = abs(testYPoints-gtYPoints);
            end
            stat(frameInd, 2) = fcnHandle(yDist);
        end
    end
end
end

function [vehiclePoints, imagePoints] = helperGetCorrespondingPoints(monoCameraSensor, estModels, gtPoints, assignments)

numFrames = length(estModels);
imagePoints = cell(numFrames, 1);
vehiclePoints = cell(numFrames, 1);

for frameInd = 1:numFrames
    if isempty(assignments{frameInd})
        imagePointsInFrame = [];
        vehiclePointsInFrame = [];
    else
        estModelInFrame = estModels{frameInd};
        gtPointsInFrame = gtPoints{frameInd};
        imagePointsInFrame = cell(length(estModelInFrame), 1);
        vehiclePointsInFrame = cell(length(estModelInFrame), 1);
        for idx = 1:length(estModelInFrame)

            % Ignore false positive assignments.
            if assignments{frameInd}(idx) == 0
                imagePointsInFrame{idx} = [NaN NaN];
                continue;
            end

            % The kth boundary in estModelInFrame is matched to kth
            % element indexed by assignments in gtPointsInFrame.
            thisModel = estModelInFrame(idx);
            thisGT = gtPointsInFrame{assignments{frameInd}(idx)};
            xPoints = thisGT(:, 1);
            yPoints = thisModel.computeBoundaryModel(xPoints);

            vehiclePointsInFrame{idx} = [xPoints, yPoints];
            imagePointsInFrame{idx} = vehicleToImage(monoCameraSensor, [xPoints yPoints]);
        end
    end
    vehiclePoints{frameInd} = vehiclePointsInFrame;
    imagePoints{frameInd}   = imagePointsInFrame;
    % Make imagePoints [] instead of {} to comply with groundTruth object.
    if isempty(imagePoints{frameInd})
        imagePoints{frameInd} = [];
    end
    if isempty(vehiclePoints{frameInd})
        vehiclePoints{frameInd} = [];
    end
end
end