% Download sensor data
baseDownloadURL = 'https://ssd.mathworks.com/supportfiles/vision/data/localizationAutonomousCarDataset.zip'; 
dataFolder = fullfile(tempdir,'localizationAutonomousCarDataset',filesep); 
options = weboptions('Timeout',Inf);
zipFileName = [dataFolder,'localizationAutonomousCarDataset.zip'];
folderExists = exist(dataFolder,'dir');

% Create a folder in a temporary directory to save the downloaded file
if ~folderExists  
    mkdir(dataFolder)
    disp('Downloading localizationAutonomousCarDataset.zip (1.01 GB). This download can take a few minutes.') 
    websave(zipFileName, baseDownloadURL, options); 
    
    % Extract contents of the downloaded file
    disp('Extracting localizationAutonomousCarDataset.zip (1.01 GB) ...') 
    unzip(zipFileName, dataFolder)
end

% Extract data from a rosbag (requires ROS Toolbox)
readRosbagData = false;
hasROSToolbox = license('test','ros_toolbox');
if readRosbagData && hasROSToolbox   
    filepath = [dataFolder,'\rosbag\demoRosbag.bag'];
    % helperReadRosbag uses a small rosbag sample to show how a larger 
    % rosbag can be processed.  An entire data set is loaded below from MAT files.
    [ptCloudsRosbag, imgsRosbag, gpsRosbag] = helperReadRosbag(filepath);
end

% Load synchronized sensor data directly from MAT files. This data was 
% extracted from a much larger rosbag collected using an autonomous car.
imgs = load([dataFolder,'\sensor\images.mat']).imgs;    % Images
ptClouds = load([dataFolder,'\sensor\ptClouds.mat']).pcds;  % Point clouds
gps = load([dataFolder,'\sensor\gps.mat']).gps;        % GPS signals

imgBBoxes = load([dataFolder,'\utility\imgBBoxes.mat']).imgBBoxes;
pcBBoxes = load([dataFolder,'\utility\pcBBoxes.mat']).pcBBoxes;

camCalib = load([dataFolder,'\utility\calibration.mat']).calib;
intrinsics = camCalib.intrinsics;

% Pick a frame
frameIndex = 110; 

% Display the camera image and 2-D bounding box of the traffic sign 
helperDisplayCameraFrame(imgs,imgBBoxes,frameIndex,intrinsics);

% Display the point cloud and 3-D bounding box of the traffic sign 
helperDisplayPointCloud(ptClouds, pcBBoxes, frameIndex);

gpsRoute = load([dataFolder,'\utility\gpsRoute.mat']).gps_route;

% Create and configure the reader
config = hereHDLMConfiguration('hrn:here:data::olp-here-had:here-hdlm-protobuf-na-2',2291);
reader = hereHDLMReader(gpsRoute(:,1),gpsRoute(:,2),'Configuration',config);

% Read data
topologyLayer = read(reader,'TopologyGeometry');
signLayer= read(reader,'LocalizationSign');

[linkData,signData] = helperProcessHDLMData(config,topologyLayer,signLayer);

% Initialize variables
hdlmDetectedSigns = zeros(0,2);
lidarDetectedSigns = zeros(0,2);
relevantGPS = zeros(0,2); 
finalGPS = zeros(0,4);
finalSigns = zeros(0,4);
numDetections = 0;

for i = 1:numel(gps)
    
    gpsMeasurement = gps{i};
    pcBBox = pcBBoxes{i};
    
    % Find the closest link
    closestLinkID = helperFindClosestLink(linkData, gpsMeasurement);
    
    isSignDetected = ~isempty(pcBBox);
    if isSignDetected
        % If a traffic sign is detected by lidar and camera, find the ID of
        % the traffic sign that is closest to the current GPS position.
        distToSigns = helperDistance(gpsMeasurement(1),gpsMeasurement(2), ...
            signData(:,2),signData(:,3));
        [~,minIndex] = min(distToSigns);
        closestSignID = signData(minIndex,1);
        
        % Make sure that the closest traffic sign is on the closest link
        for m = 1:size(signLayer,1)
            roadToSignsRefs = signLayer(m,1).RoadToSignsReferences;
            
            if ismember(closestLinkID,[roadToSignsRefs.LinkRef])
                signRefs = vertcat(roadToSignsRefs.SignRefs);
                if ismember(closestSignID,[signRefs.SignId])
                    % Transform the coordinates from the lidar frame to the GPS frame
                    [lat,lon] = helperLidar2latlon(pcBBox,gps,i);
                    
                    % Store relevant results
                    hdlmDetectedSigns = [hdlmDetectedSigns; signData(minIndex,2:3)]; %#ok<*AGROW>
                    lidarDetectedSigns = [lidarDetectedSigns; lat, lon];
                    relevantGPS = [relevantGPS; gpsMeasurement(1:2)];
                    numDetections = numDetections+1;
                    break
                end
            end
        end
        
    elseif numDetections > 1
        % Once a traffic sign is not detected anymore, calculate the error between 
        % the traffic signs detected by the lidar and camera and the traffic signs 
        % selected from the HDLM data.
        error = hdlmDetectedSigns(end,1:2) - mean(lidarDetectedSigns(:,1:2));
        
        % Apply the correction to the GPS coordinates reported by the GPS sensor
        correctedGPS = relevantGPS + error;
        
        % Store results
        finalGPS = [finalGPS; relevantGPS, correctedGPS];
        finalSigns = [finalSigns; mean(lidarDetectedSigns), hdlmDetectedSigns(end,:)];
        
        % Re-initialize storage variables
        numDetections = 0;
        relevantGPS = zeros(0,2);
        hdlmDetectedSigns = zeros(0,2);
        lidarDetectedSigns = zeros(0,2);
    end
end

helperDisplayResults(finalSigns,finalGPS,topologyLayer);

function [ptClouds, images, gps] = helperReadRosbag(filepath)

bag = rosbag(filepath);

% Names of ROS topics
lidarTopic = '/os1_cloud_node/points';
imageTopic = '/camera/image_color';
gpsTopic = '/raw_fix';

bagLidar = select(bag,'Topic',lidarTopic);
bagImage = select(bag,'Topic',imageTopic);
bagGPS = select(bag,'Topic',gpsTopic);

% Read the messages
msgStructsImage = readMessages(bagImage);
msgStructsPC = readMessages(bagLidar);
msgStructsGPS = readMessages(bagGPS);

numLidarFrames = size(bagLidar.MessageList,1);

ptClouds = cell(numLidarFrames, 1);
images = cell(numLidarFrames, 1);
gps = cell(numLidarFrames, 1);

% Since each sensor runs at a different frequency, use the
% lidar's acquisition rate as the basis for synchronizing lidar, 
% video and GPS data. Collect frames from other sensors by
% selecting the closest ones according to their time stamps.
for i = 1:numLidarFrames
    timeStamp = bagLidar.MessageList.Time(i);
    
    % Lidar point cloud
    msgPC = msgStructsPC{i};
    ptClouds{i} = pointCloud(readXYZ(msgPC));
    
    % Camera data
    [~, minIdx] = min(abs(bagImage.MessageList.Time-timeStamp));
    images{i} = readImage(msgStructsImage{minIdx});
    
    % GPS data
    [~, minIdx] = min(abs(bagGPS.MessageList.Time-timeStamp));
    gps{i} = [msgStructsGPS{minIdx}.Latitude, ...
        msgStructsGPS{minIdx}.Longitude, ...
        msgStructsGPS{minIdx}.Altitude];
end
end

function [linkData,signData] = helperProcessHDLMData(config,topologyLayer,signLayer)

% Collect all the links in the tiles from the TopologyGeometry layer
[linkIDs,linkGeometries] = helperCollectLinksData(config,topologyLayer);

% Divide the links into segments of two points. This gives more accurate 
% results when finding the closest link for a given GPS position
linkData = helperSegmentLinks(linkIDs,linkGeometries);

% Collect all the signs in the tiles from the LocalizationSign layer
signData = helperCollectSignData(signLayer);
end

function [linkIDs, linkGeometries] = helperCollectLinksData(config,topologyLayer)

linkIDs=[];
linkGeometries=[];

intersectLinkRefs = vertcat(topologyLayer.IntersectingLinkRefs);
tileIDs = unique(vertcat(intersectLinkRefs.LinkHereTileId));
reader = hereHDLMReader(tileIDs,'Configuration',config);
topologies = read(reader,'TopologyGeometry'); 

for j = 1:size(topologyLayer,1)
    % Intersecting links
    currIntersectLinkRefs = topologyLayer(j, 1).IntersectingLinkRefs;
    for i = 1:size(currIntersectLinkRefs,1)
        tileID = currIntersectLinkRefs(i).LinkHereTileId;
        linkID = currIntersectLinkRefs(i).LinkId;
        topology = topologies(tileIDs==tileID);
        
        linksStartingInTile = topology.LinksStartingInTile;
        linksTileID = vertcat(linksStartingInTile.LinkId);
        linkGeometry = linksStartingInTile(linksTileID==linkID).Geometry.Here2dCoordinateDiffs;
        linkIDs = [linkIDs; linkID];
        linkGeometries = [linkGeometries; {linkGeometry}];
    end
    
    % Links starting in tile
    linksStartingInTile = topologyLayer(j,1).LinksStartingInTile;
    for i = 1:size(linksStartingInTile,1)
        linkIDs = [linkIDs; linksStartingInTile(i).LinkId];
        linkGeometry = linksStartingInTile(i).Geometry.Here2dCoordinateDiffs;
        linkGeometries = [linkGeometries; {linkGeometry}];
    end
end
end

function signData = helperCollectSignData(signLayer)

signData = [];
for i = 1:size(signLayer,1)
    signData = [signData; double(vertcat(signLayer(i).Signs.SignId)), ...
        vertcat(signLayer(i).Signs.Here2dCoordinate)];
end
end

function linkData = helperSegmentLinks(linkIDs,linkGeometries)

linkData = zeros(0,5); % [LinkID, start_lat, start_lon, end_lat, end_lon]
for i = 1:numel(linkIDs)
    points = linkGeometries{i};
    numSegments = size(points, 1) - 1;
    linkData = [linkData; [repmat(double(linkIDs(i)),numSegments,1), ...
        points(1:numSegments, 1:2), points(2:numSegments+1, 1:2)]];
end
end

function closestLinkID = helperFindClosestLink(linkData,gps)

% Compute distance to link
distToLinkStart = helperDistance(gps(1),gps(2),linkData(:, 2),linkData(:, 3));
distToLinkEnd = helperDistance(gps(1),gps(2),linkData(:, 4),linkData(:, 5));
distToLinks = min(distToLinkStart,distToLinkEnd);

% Find closest link
[~,index] = min(distToLinks);
closestLinkID = linkData(index, 1);
end

function [lat,lon] = helperLidar2latlon(bbox, gps, i)
% Converts the lidar coordinates of the detected traffic sign to latitude and 
% longitude measurements. This is done by first converting the lidar coordinates 
% to the vehicle frame, then the East, North, Up (ENU) frame, and finally the GPS frame.

% Center of the bounding box in the lidar frame.
center = bbox(1:3);

% Calculate the bearing angle.
lon1 = gps{1,i-4}(1);
lon2 = gps{1,i}(1);
lat1 = gps{1,i-4}(2);
lat2 = gps{1,i}(2);

dLon = lon2 - lon1;
y = sin(dLon) * cos(lat2);
x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
theta = atan2(y, x);

initialBearing = mod(theta + 2*pi, 2*pi);
finalBearing   = mod(initialBearing + pi/2, 2*pi);

% Transform from the lidar frame to the vehicle frame.
x = -center(1);
y = -center(2);

% Transform from the vehicle frame to the ENU frame.
e = sin(finalBearing)*x - cos(finalBearing)*y;
n = cos(finalBearing)*x + sin(finalBearing)*y;

% Transform from the ENU frame to the GPS frame.
% The origin of the ENU local frame is set to the current GPS location.
origin     = [gps{1,i}(1) gps{1,i}(2) gps{1,i}(3)];
[lat,lon] = local2latlon(e,n,0,origin);
end

function d = helperDistance(lat1,lon1,lat2,lon2)
numSignals = numel(lat2);
[x,y] = latlon2local(lat2,lon2,zeros(numSignals,1),[lat1 lon1 0]); % Assuming zero altitude
d = sqrt(x.^2 + y.^2);
end

function helperDisplayResults(finalSigns,finalGPS,topologyLayer)

% Create dialog box for entering the Access Key ID.
url = ['https://1.base.maps.ls.hereapi.com/maptile/2.1/maptile/', ...
    'newest/normal.day/${z}/${x}/${y}/256/png?apikey=%s'];

prompt = {'HERE Access Key ID:'};
title = 'HERE Tokens';

dims = [1 40]; % Text edit field height and width
hereTokens = inputdlg(prompt,title,dims);

if ~isempty(hereTokens)
    
    % Add HERE basemap with custom attribution.
    url = sprintf(url,hereTokens{1});
    copyrightSymbol = char(169); 
    attribution = [copyrightSymbol,' ',datestr(now,'yyyy'),' HERE'];
    addCustomBasemap('herestreets',url,'Attribution',attribution);
    
    f = figure('Name','Corrected GPS measurements');
    gx = geoaxes("Parent",f);
    plot(topologyLayer,'Axes',gx);
    hold(gx,'on');
    legend(gx,'Boundaries','Nodes','Links','Location','northwest');
    
    nlat = [finalSigns(1,1) finalSigns(end,1)];
    nlon = [finalSigns(1,2) finalSigns(end,2)];
    nzoom = 16;
    
    geolimits(gx,nlat,nlon);
    gx.ZoomLevel = nzoom;
    
    geobasemap(gx,'herestreets');
    geoplot(gx,finalSigns(:,1),finalSigns(:,2),'o','DisplayName','Lidar traffic signs');
    geoplot(gx,finalSigns(:,3),finalSigns(:,4),'*','DisplayName','HDLM traffic signs');
    geoplot(gx,finalGPS(:,1),finalGPS(:,2),'r-+','DisplayName','GPS before correction');
    geoplot(gx,finalGPS(:,3),finalGPS(:,4),'g-+','DisplayName','GPS after correction');
else
    error('You must enter valid credentials to access maps from HERE Technologies');
end
end

function helperDisplayCameraFrame(imgs,imgBBoxes,frameIndex,intrinsics)

img = undistortImage(imgs{frameIndex},intrinsics);

f  = figure('Name','Camera frame');
ax = gca(f);

% Display camera frame
imshow(img,'Parent',ax);
hold(ax,'on')

showShape('rectangle',imgBBoxes{frameIndex},'Color','blue','LineWidth',2,'Parent',ax);
end

function helperDisplayPointCloud(ptClouds,pcBBoxes,frameIndex)

f = figure('Name','Point cloud');
ax = gca(f);

pcshow(ptClouds{frameIndex},'Parent',ax);
hold(ax,'on')
xlabel(ax,'X')
ylabel(ax,'Y')
zlabel(ax,'Z')

xlim([-40 40])
ylim([-20 40])
zlim([-5 10])
view([90 45])

showShape('cuboid', pcBBoxes{frameIndex},'Color','red','LineWidth',2,'Parent',ax);
end