%% Use HERE HD Live Map Data to Verify Lane Configurations
% This example shows how to read and visualize lane configurations for a
% recorded driving route from the HERE HD Live Map (HERE HDLM) service.
% This visualization can be used to verify the lane configurations detected
% by the perception system of an onboard sensor, such as a monocular
% camera.
%
% In this example, you learn how to access the tiled layers from the HDLM
% service and identify relevant road-level and lane-level topology,
% geometry, and attributes.
%
% To read the data, you use a
% |<docid:driving_ref#mw_1b07178b-8143-4b99-81ce-ee194738f9e4
% hereHDLMReader>| object. Use of the HERE HD Live Map service requires
% valid HERE HDLM credentials. You need to enter into a separate agreement
% with HERE in order to gain access to the HDLM services and to get the
% required credentials (access_key_id and access_key_secret) for using the
% HERE Service.
%
% Copyright 2018 The MathWorks, Inc.

%% Overview
% High-definition (HD) maps refer to mapping services developed
% specifically for automated driving applications. The precise geometry of
% these maps (with up to 1 cm resolution near the equator) make them
% suitable for automated driving workflows beyond the route planning
% applications of traditional road maps. Such workflows include lane-level
% verification, localization, and path planning. This example shows you how
% to verify the performance of a lane detection system using lane-level
% information from HD mapping data.
%
% The accuracy of HD mapping data enables its use as a source of ground
% truth data for verification of onboard sensor perception systems. This
% high accuracy enables faster and more accurate verification of existing
% deployed algorithms.
%
% HERE HD Live Map (HERE HDLM) is a cloud-based HD map service developed by HERE
% Technologies to support highly automated driving. The data is composed of
% tiled mapping layers that provide access to accurate geometry and robust
% attributes of a road network. The layers are grouped into the following
% models:
%
% * *Road Centerline Model:* Provides road topology (specified as nodes and
%   links in a graph), shape geometry, and other road-level attributes.
% * *HD Lane Model:* Provides lane topology (as lane groups and lane group
%   connectors), highly accurate geometry, and lane-level attributes.
% * *HD Localization Model:* Provides features to support vehicle
%   localization strategies.
%
% For an overview of HERE HDLM layers, see
% <docid:driving_ug#mw_4b697ed4-ad52-4319-ace2-03999280b820 HERE HD Live
% Map Layers>.

%%
% Cameras are used in automated driving to gather semantic information
% about the road topology around the vehicle. Lane boundary detection, lane
% type classification, and road sign detection algorithms form the core of
% such a camera processing pipeline. You can use the HERE HDLM service,
% along with a high-precision GPS mounted on the vehicle, to evaluate the
% accuracy of such algorithms and verify their performance.
%
% In this example, you learn how to:
%
% # Read road and lane information from the HERE HDLM service for a
%   recorded GPS sequence.
% # Apply a heuristic route matching approach to the recorded GPS data.
%   Because GPS data is often imprecise, it is necessary to solve the
%   problem of matching recorded geographic coordinates to a representation
%   of the road network.
% # Identify environmental attributes relevant to the vehicle. Once a
%   vehicle is successfully located within the context of the map, you can
%   use road and lane attributes relevant to the vehicle to verify data
%   recorded by the vehicle's onboard camera sensor.

%% Load and Display Camera and GPS Data
% Start by loading data from the recorded drive. The recorded data in this
% example is from a driving dataset collected by the Udacity(R)
% Self-Driving Car team. This data includes a video captured by a
% front-facing monocular camera and vehicle positions and velocities logged
% by a GPS.
%
% Load the |centerCamera.avi| camera video data and corresponding video
% timestamps.

recordedData = fullfile(toolboxdir('driving'), 'drivingdata', ...
    'udacity', 'drive_segment_09_29_16');
[videoReader, videoTime] = helperLoadCameraData(fullfile(recordedData));

% Show the first frame of the camera data
imageFrame = readFrame(videoReader);
imshow(imageFrame, 'Border', 'tight');

%%
% Load the GPS data from the |gpsSequence.mat| MAT-file.

data = load(fullfile(recordedData, 'gpsSequence.mat'));
gpsData = data.gpsTT;

% Plot the full route and the first position recorded from the GPS
gpsPlayer = geoplayer(gpsData.Latitude(1), gpsData.Longitude(1), 18);
plotRoute(gpsPlayer, gpsData.Latitude, gpsData.Longitude);
plotPosition(gpsPlayer, gpsData.Latitude(1), gpsData.Longitude(1));

%% Match Recorded Vehicle Position to a Road
% Create a reader for reading the HERE HD Live Map tiles that cover all
% recorded GPS locations in the drive. If you have not previously set up
% HERE HDLM credentials, a dialog box prompts you to enter them. Enter the
% *Access Key ID* and *Access Key Secret* that you obtained from HERE
% Technologies, and click *OK*.
reader = hereHDLMReader(gpsData.Latitude, gpsData.Longitude);

%%
% Read and plot road topology data from the |TopologyGeometry| layer. This
% layer represents the configuration of the road network. Nodes of the
% network correspond to intersections and dead-ends. Links between nodes
% represent the shape of the connecting streets as polylines. The
% connectivity and geometry for these features are contained in the
% |LinksStartingInTile| and |NodesInTile| fields. 
topologyLayer = read(reader, 'TopologyGeometry')

figure('Name', 'TopologyGeometry');
topologyAxes = plot(topologyLayer);
hold(topologyAxes, 'on');
geoplot(topologyAxes, gpsData.Latitude, gpsData.Longitude, ...
    'bo-', 'DisplayName', 'Route');

%%
% The plotting functionality is captured within the |helperPlotLayer|
% function, which visualizes available data from a HD Live Map layer with
% the recorded drive on the same geographic axes. This function, defined at
% the end of the example, will be used to plot subsequent layers.
%
% Given a GPS location recorded along a drive, you can use a route matching
% algorithm to determine which road on the network the recorded position
% corresponds to. This example uses a heuristic route matching algorithm
% that considers the nearest links spatially to the recorded geographic
% point. The algorithm applies the vehicle's direction of travel to
% determine the most probable link. This route matching approach does not
% consider road connectivity, vehicle access, or GPS data with high
% positional error. Therefore, this approach might not apply to all
% scenarios.
%
% The |helperGetGeometry| function extracts geometry information from the
% given topology layer and returns this information in a table with the
% corresponding links.
topologyTable = helperGetGeometry(topologyLayer.LinksStartingInTile, ...
    {'LinkId', 'Geometry.Here2dCoordinateDiffs'});
topologyTable.Properties.VariableNames = {'LinkId', 'Geometry'};

%%
% The
% |<matlab:openExample('driving/UseHEREHDLiveMapDataToVerifyLaneConfigurationsExample','supportingFile','HelperLinkMatcher.m')
% HelperLinkMatcher>| class creates a link matcher, which contains the
% shape geometry for each link in the desired map tile. This class uses a
% basic spatial analysis to match the recorded position to the shape
% coordinates of the road links.
linkMatcher = HelperLinkMatcher(topologyTable);

% Match first point of the recorded route to the most probable link
[linkId, linkLat, linkLon] = match(linkMatcher, gpsData.Latitude(1), ...
    gpsData.Longitude(1), gpsData.Velocity(1,1:2));

% Plot the shape geometry of the link
geoplot(gpsPlayer.Axes, linkLat, linkLon, 'r.-');

%% Retrieve Speed Limit Along a Matched Road
% Characteristics common to all lanes along a given road are attributed to
% the link element that describes that road. One such attribute, speed
% limit, describes the maximum legal speed for vehicles traveling on the
% link. Once a given geographic coordinate is matched to a link, you can
% identify the speed limit along that link. Because features like speed
% limits often change along the length of a link, these attributes are
% identified for specific ranges of the link.
%
% The |SpeedAttributes| layer contains information about the expected
% vehicle speed on a link, including the posted speed limit.
speedLayer = read(reader, 'SpeedAttributes');

%%
% The |helperGetSpeedLimits| function extracts speed limit data for the
% relevant length and direction of a link. As with extracting the geometry
% information for a link, specifically capturing the speed limit data
% requires specialized code.
speedTable = helperGetSpeedLimits(speedLayer);

% Find the speed limit entry for the matched link
speed = speedTable(speedTable.LinkId == linkId, :);

%% Match Recorded Position to a Lane Group
% The HD Lane Model contains the lane-level geometry and attributes of the
% road, providing the detail necessary to support automated driving
% applications. Much like the Road Centerline Model, the HD Lane Model also
% follows a pattern of using topology to describe the road network at the
% lane level. Then features of the lane groups are attributed to the
% elements of this topology. In the HD Lane Model, the primary topological
% element is the lane group.
%
% Read and plot lane topology data from the |LaneTopology| layer. This
% layer represents lane topology as lane groups and lane group connectors.
% Lane groups represent a group of lanes within a link (road segment). Lane
% group connectors connect individual lane groups to each other. The
% connectivity and geometry for these features are contained in the
% |LaneGroupsStartingInTile| and |LaneGroupConnectorsInTile| fields, for
% lane groups and lane group connectors, respectively.
laneTopologyLayer = read(reader, 'LaneTopology')
laneAxes = helperPlotLayer(laneTopologyLayer, ...
    gpsData.Latitude, gpsData.Longitude);
geolimits(laneAxes, [37.3823, 37.3838], [-122.1151, -122.1128]);

%%
% The lane group represents multiple lanes. Therefore, the geometry of this
% element is given by the polygonal shape the group takes, as expressed by
% the left and right boundaries of the lane group. Obtain this lane
% geometry from the lane boundaries by using the |helperGetGeometry|
% function.
laneGroupFields = {'LaneGroupId', ...
    'BoundaryGeometry.LeftBoundary.Here2dCoordinateDiffs', ...
    'BoundaryGeometry.RightBoundary.Here2dCoordinateDiffs'};
laneTopologyTable = helperGetGeometry(laneTopologyLayer.LaneGroupsStartingInTile, ...
    laneGroupFields);
laneTopologyTable.Properties.VariableNames = {'LaneGroupId', ...
    'LeftGeometry', 'RightGeometry'};

%%
% As with developing a matching algorithm to identify the most probable
% travel link, matching the given GPS data to the most probable lane group
% can follow multiple approaches. The approach described here uses two
% layers: |LaneRoadReferences| and |LaneTopology|.
%
% * The |LaneRoadReferences| layer enables you to translate a position on
%   the Road Centerline Model (given by a link) to a corresponding position
%   on the HD Lane Model (given by a lane group). Since the link was
%   previously identified, you can filter the candidates for a lane group
%   match to a smaller subset of all the lane groups available in the tile.
% * The |LaneTopology| layer gives geometry data, which can be used to
%   consider data that exists spatially within the boundaries of each
%   candidate lane group. As with spatially matching GPS data to links,
%   such an approach is prone to error and subject to the accuracy of the
%   recorded GPS data. In addition to matching the lane group, you also
%   need to match the direction vector of the vehicle relative to the
%   orientation of the lane group. This step is necessary because the
%   attributes of the lanes are defined with respect to the topology
%   orientation.
%
% Use the |helperGetReferences| function to generate a table of all lane
% groups that exist for at least some length of a link.
referenceLayer = read(reader, 'LaneRoadReferences');
referenceTable = helperGetReferences(referenceLayer);

%%
% Create a lane group matcher that contains the boundary geometry for each
% lane group in the desired map tile. The
% |<matlab:openExample('driving/UseHEREHDLiveMapDataToVerifyLaneConfigurationsExample','supportingFile','HelperLaneGroupMatcher.m')
% HelperLaneGroupMatcher>| class creates a lane group matcher that contains
% the boundary shape geometry for each lane group in the desired map tile.
% It also contains a reference table of links to lane groups. As with the
% |HelperLinkMatcher| class, this class uses a simple spatial analysis
% approach to determine if a given recorded coordinate exists within the
% boundaries of a lane group.
laneGroupMatcher = HelperLaneGroupMatcher(referenceTable, laneTopologyTable);

% Match the lane group and relative direction
[laneGroupId, isForward, boundGeometry] = match(laneGroupMatcher, linkId, ...
    gpsData.Latitude(1), gpsData.Longitude(1), gpsData.Velocity(1,1:2));

% Plot the boundary geometry of the lane group
geoplot(gpsPlayer.Axes, boundGeometry(:,1), boundGeometry(:,2), 'm.-');

%% Retrieve Lane Configurations for a Matched Lane Group
% As with the speed attributes that are mapped to links by an identifier,
% lane attributes also assign features to lane groups by using a lane group
% ID. The |LaneAttributes| layer contains information about the lane
% groups, including the types of each lane in the group and the
% characteristics of the lane boundaries.
%
% Use the |helperGetLaneAttributes| function to extract the different lane
% types and lane boundary markings for each lane group in the tile.
laneAttributesLayer = read(reader, 'LaneAttributes');
laneAttributesTable = helperGetLaneAttributes(laneAttributesLayer);

% Find the lane attribute entry for the matched lane group
laneAttribute = laneAttributesTable.LaneGroupId == laneGroupId;

%% Visualize and Verify Recorded Drive with HERE HDLM Data
% The matching algorithms and tables generated to identify the road and
% lane properties can be extended to a sequence of recorded GPS
% coordinates. For each time step, the vehicle's position is matched to a
% link and lane group on the road. In addition, the speed limit and lane
% configurations are displayed along with the corresponding camera images.
%
% The
% |<matlab:openExample('driving/UseHEREHDLiveMapDataToVerifyLaneConfigurationsExample','supportingFile','HelperHDLMUI.m')
% HelperHDLMUI>| class creates a tool for streaming video and GPS data from
% a recorded drive and displaying relevant information from selected HERE
% HD Live Map layers at each recorded vehicle position.
hdlmUI = HelperHDLMUI(gpsData.Latitude(1), gpsData.Longitude(1));

% Synchronize the camera and GPS data into a common timetable
synchronizedData = synchronize(videoTime, gpsData);
videoReader.CurrentTime = 0;
maxDisplayRate = videoReader.FrameRate * 5;

% Initialize some variables to maintain history
prevLinkId      = 0;
prevLaneGroupId = 0;

for idx = 1 : height(synchronizedData)
    
    timeStamp = synchronizedData.Time(idx);
    
    % Check if the current timestamp has GPS data
    hasGPSFrame = ~(ismissing(synchronizedData.Latitude(idx)) || ...
        ismissing(synchronizedData.Longitude(idx)));
    
    if hasGPSFrame
        latitude  = synchronizedData.Latitude(idx);
        longitude = synchronizedData.Longitude(idx);
        velocity  = synchronizedData.Velocity(idx, 1:2); 
        
        % Match GPS position to link
        [linkId, linkLat, linkLon] = match(linkMatcher, ...
            latitude, longitude, velocity);
        
        if linkId ~= prevLinkId
            % Update link
            updateLink(hdlmUI, linkLat, linkLon);
            prevLinkId = linkId;
            
            % Update speed limit
            speed = speedTable(speedTable.LinkId == linkId, :);
            updateSpeed(hdlmUI, speed.Value);
        end
        
        % Match GPS position to lane group
        [laneGroupId, isForward, boundGeometry] = match(laneGroupMatcher, linkId, ...
            latitude, longitude, velocity);
        
        if laneGroupId ~= prevLaneGroupId
            % Update lane group
            updateLaneGroup(hdlmUI, boundGeometry);
            prevLaneGroupId = laneGroupId;
            
            % Update lane types and boundary markings
            laneAttribute = laneAttributesTable.LaneGroupId == laneGroupId;
            plotLanes(hdlmUI, laneAttributesTable.Lanes{laneAttribute}, ...
                laneAttributesTable.LaneBoundaries{laneAttribute}, isForward);
        end
        
        updatePosition(hdlmUI, latitude, longitude);
    else
        % Read frame of the video
        imageFrame = readFrame(videoReader);
    end
    
    updateImage(hdlmUI, imageFrame);
    updateTime(hdlmUI, timeStamp);
    pause(1/maxDisplayRate);
end

%% Conclusion
% In this example, you explored how to: 
%
% # Access HD mapping data for a given GPS sequence from the HERE HD Live
%   Map service and import that data into MATLAB.
% # Match recorded GPS data against the imported road network data to find
%   the relevant link and lane group for each geographic coordinate.
% # Query attributes of the matched link and lane group, such as speed
%   limit and lane types, to develop a tool for visually verifying
%   features of the road against the recorded camera data.
% 
% The techniques discussed in this example can be further extended to
% support automated verification of perception algorithms.

%% Supporting Functions
% |helperPlotLayer| plots layer data and route on a geographic plot.
function gx = helperPlotLayer(layer, latitude, longitude)
%helperPlotLayer Create geographic plot with layer data and route
%   gx = helperPlotLayer(layer, latitude, longitude) creates a geographic
%   axes plot with the plottable HDLM layer and the route given by latitude
%   and longitude on a new figure.

figure;

% Plot layer
gx = plot(layer);

% Enable adding data to the plot
hold(gx, 'on');

% Plot latitude, longitude data
geoplot(gx, latitude, longitude, 'bo-', 'DisplayName', 'Route');
hold(gx, 'off');
end

%%
% |helperGetGeometry| extracts geometry for topology elements from a layer
% in the form of a table.
function geometryTable = helperGetGeometry(layer, fields)
%helperGetGeometry Create a table with geometry for topology elements
%   geometryTable = helperGetGeometry(layer, fields) returns a table
%   formatted with the specified geometry fields of the TopologyGeometry
%   and LaneTopology layers.

% Pre-allocate struct
S = repmat(struct, size(layer));

for field = fields
    C = strsplit(field{:}, '.');
    for idx = 1:numel(layer)
        fieldname = strjoin(C, '');
        S(idx).(fieldname) = getfield(layer, {idx}, C{:});
    end
end

geometryTable = struct2table(S);
end

%%
% |helperGetSpeedLimits| extracts speed limit data from a layer in the form
% of a table.
function speedTable = helperGetSpeedLimits(layer)
%helperGetSpeedLimits Create a data table with speed limits
%   speedTable = helperGetSpeedLimits(layer) returns a table formatted with
%   the speed limit start, end, direction, and value along a specified link
%   as given by the SpeedAttributes layer object specified by layer.

speed = struct(...
    'LinkId',       {}, ...
    'Start',        {}, ...
    'End',          {}, ...
    'Direction',    {}, ...
    'Value',        {});

for idx = 1 : numel(layer.LinkAttribution)
    
    % Assign the link ID
    link            = layer.LinkAttribution(idx);
    attributions    = link.ParametricAttribution;
    
    % Examine each attribute assigned to the link
    for attrIndex = 1 : numel(attributions)
        
        linkAttr = vertcat(attributions.LinkParametricAttribution);
        
        % For each attribute, check if the speed limit information is
        % listed. If speed limit is provided, make an entry.
        for linkAttrIndex = 1 : numel(linkAttr)
            
            if ~isempty(linkAttr(linkAttrIndex).SpeedLimit)
                
                % Assign speed limit to specified link
                speedLimit = struct;
                speedLimit.LinkId       = link.LinkLocalRef;
                speedLimit.Start        = attributions(attrIndex).AppliesToRange.RangeOffsetFromStart;
                speedLimit.End          = attributions(attrIndex).AppliesToRange.RangeOffsetFromEnd;
                speedLimit.Direction    = attributions(attrIndex).AppliesToDirection;
                speedLimit.Value        = linkAttr(linkAttrIndex).SpeedLimit.Value;
                
                % Convert KPH to MPH
                if strcmpi(linkAttr(linkAttrIndex).SpeedLimit.Unit, 'KILOMETERS_PER_HOUR')
                    speedLimit.Value = speedLimit.Value / 1.609;
                end
                
                if strcmpi(speedLimit.Direction, 'BOTH')
                    speed = [speed; speedLimit]; %#ok<AGROW>
                end
            end
        end
        
    end
end

speedTable = struct2table(speed);
end

%%
% |helperGetReferences| extracts lane road references from a layer object
% in the form of a table.
function laneRoadReferenceTable = helperGetReferences(layer)
%helperGetReferences Create a data table with lane road references
%   laneRoadReferenceTable = helperGetReferences(layer) returns a table
%   formatted with a list of all lane groups existing on a specified link
%   as given by the LaneRoadReferences layer object specified by layer.

numLinks = numel(layer.LinkLaneGroupReferences);
reference = repmat(struct('LinkId', {}, 'LaneGroupId', {}), numLinks, 1);

% Get references from links to lane groups
for idx = 1 : numLinks
    link = layer.LinkLaneGroupReferences(idx);
    laneGroups = vertcat(link.LaneGroupReferences.LaneGroupRef);
    
    reference(idx).LinkId = link.LinkLocalRef;
    reference(idx).LaneGroupId = [laneGroups(:).LaneGroupId]';
end

laneRoadReferenceTable = struct2table(reference);
end

%%
% |helperGetLaneAttributes| extracts lane attributes from a layer object in
% the form of a table.
function laneAttributesTable = helperGetLaneAttributes(layer)
%helperGetLaneAttributes Create a table with lane and boundary types
%   laneAttributesTable = helperGetLaneAttributes(layer) returns a table
%   formatted with the lane types and the lane boundary markings for each
%   lane group in the LaneAttributes layer object specified by layer.

for laneGroupAttrIndex = 1 : numel(layer.LaneGroupAttribution)
    laneGroup = layer.LaneGroupAttribution(laneGroupAttrIndex);
    attributes(laneGroupAttrIndex).LaneGroupId = laneGroup.LaneGroupRef; %#ok
    
    % Get lane types for each lane group
    for laneAttrIndex = 1 : numel(laneGroup.LaneAttribution)
        
        lane = laneGroup.LaneAttribution(laneAttrIndex);
        
        laneAttr = vertcat(lane.ParametricAttribution);
        laneAttr = vertcat(laneAttr.LaneParametricAttribution);
        
        for idx = 1 : numel(laneAttr)
            if ~isempty(laneAttr(idx).LaneType)
                attributes(laneGroupAttrIndex).Lanes{lane.LaneNumber} = ...
                    laneAttr(idx).LaneType;
            end
        end
    end
    
    % Get lane boundaries for each lane group
    for laneBoundaryIndex = 1 : numel(laneGroup.LaneBoundaryAttribution)
        laneBoundary = laneGroup.LaneBoundaryAttribution(laneBoundaryIndex);
        boundaries   = vertcat(laneBoundary.ParametricAttribution.LaneBoundaryParametricAttribution);
        
        attributes(laneGroupAttrIndex).LaneBoundaries{laneBoundary.LaneBoundaryNumber} = ...
            boundaries.LaneBoundaryMarking;
    end
    
end

laneAttributesTable = struct2table(attributes);
end

%%
% |helperLoadCameraData| loads a video reader and timestamps from folder.
function [videoReader, videoTime] = helperLoadCameraData(dirName)
%helperLoadCameraData Load camera images from folder in a timetable
%   [videoReader, videoTime] = helperLoadCameraData(dirName) loads a video
%   from the folder dirName. Timestamps for the video are read from a
%   MAT-file in the folder named timeStamps.mat.

if ~isfolder(dirName)
    error('Expected dirName to be a path to a folder.')
end

matFileName = fullfile(dirName, 'centerCameraTime.mat');
if exist(matFileName, 'file') ~= 2
    error('Expected dirName to have a MAT-file named centerCameraTime.mat containing timestamps.')
end

% Load the MAT-file with timestamps
ts = load(matFileName);
fieldNames = fields(ts);
Time = ts.(fieldNames{1});

videoFileName = fullfile(dirName, 'centerCamera.avi');
if exist(matFileName, 'file') ~= 2
    error('Expected dirName to have a video file named centerCamera.avi.')
end

% Load the video file
videoTime = timetable(Time);
videoReader = VideoReader(videoFileName);
end