Коррекция локализации с использованием Traffic Sign Data from HERE HD Maps

В этом примере показано, как использовать данные знака трафика от сервиса HERE HD Live Map (HERE HDLM), чтобы исправить измерения GPS, собранные автономным транспортным средством.

Обзор

Данные HERE HDLM содержат очень подробную и точную информацию об окружении транспортного средства, такую как топология дорог и маршрутов, и подходят для разработки беспилотных аппаратов. Обзор данных, доступных из слоев HERE HDLM, см. в HERE HD Live Map Layers. Этот пример фокусируется на слое HD Localization Model, который содержит точную и плотную информацию о дорожных знаках. Можно улучшить точность локализации путем объединения этой информации с живыми обнаружениями дорожных знаков с бортовых датчиков. Этот пример демонстрирует упрощенное решение, в котором вы извлекаете данные о местоположении дорожного знака и затем используете его для исправления показаний GPS. Более полное решение использует такие данные как ориентиры в конвейере одновременной локализации и картографии (SLAM) на основе ориентиров.

В этом примере вы узнаете, как:

  1. Считывайте и обрабатывайте информацию о дорожных знаках из данных HERE HDLM с помощью последовательности GPS.

  2. Сопоставьте дорожные знаки, обнаруженные бортовым лидаром и датчиками камеры, со знаками, сохраненными в данных HERE HDLM.

  3. Вычислите ошибку положения путем сравнения известных расположений знаков в HERE HDLM с местоположениями, возвращенными GPS, и исправьте результаты локализации.

Загрузка данных датчика

Данные о датчике транспортного средства, используемые в этом примере, были записаны в rosbag с использованием ROS. Пример включает только фрагмент rosbag, чтобы показать, как извлечь, синхронизировать и постобработать данные. Чтобы упростить коррекцию локализации, в остальном примере используются постобработанные данные, хранящиеся в файлах MAT.

% 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

В полном конвейере вы сначала обнаруживаете дорожные знаки в системы координат камеры, а затем проецируете их на лидар облаков точек используя результат калибровки лидар-камеры. Эта проекция дает вам 3-D местоположения дорожных знаков, которые могут использоваться, чтобы вычислить метрику ошибки на основе дорожных знаков, обнаруженных в данных карты HERE HD. Чтобы упростить обработку в целях рисунка, данные о дорожных знаках в этом примере были аннотированы вручную с помощью приложения Ground Truth Labeler. Загрузите ограничительные рамки дорожных знаков как для изображений, так и для облаков точек.

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

Без искажения изображения при помощи калибровки камеры. Данные калибровки были получены с помощью приложения Camera Calibrator.

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);

Данные HERE HDLM содержат ссылки в плитках карты. Загрузка GPS-координат полного маршрута. Этот маршрут охватывает ссылку, которая растягивается на три плитки HDLM.

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

Чтение данных карты

Чтобы считать данные из мозаик карты, на которых работает маршрут GPS, используйте hereHDLMReader объект. Для использования службы HERE HD Live Map требуются действительные учетные данные HERE HDLM. Вам необходимо заключить отдельное соглашение с HERE в порядок получения доступа к сервисам HDLM и получения необходимых учетных данных (access_key_id и access_key_secret) для использования сервиса HERE.

% 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');

Предварительно обработайте данные, чтобы улучшить время поиска при запросе необходимых данных. Функция помощника helperProcessHDLMData, заданный в конце примера, берёт GPS-маршрут и возвращает следующие данные:

  • linkData - Содержит все ссылки, существующие в плитках, охватывающих маршрут GPS. Ссылки сегментируются на пары [lat, lon] координаты.

  • signData - Содержит информацию о знаках, доступных в плитках, охватывающих маршрут GPS.

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

Локализация, совпадение знаков трафика и коррекция ошибок

Чтобы локализовать автономное транспортное средство, используйте измерения GPS и ссылок, образующие каждую дорогу в TopologyGeometry слой:

  1. Для каждого местоположения GPS найдите ближайшая ссылка в плитке с помощью helperFindClosestLink функция.

  2. Найдите ближайший дорожный знак к положению транспортного средства на основе LocalizationSign данные слоя. Затем сопоставьте этот знак с одним из дорожных знаков, обнаруженных лидаром и камерой.

  3. После того, как дорожный знак больше не обнаруживается, вычислите различие в положении между дорожными знаками, обнаруженными лидаром и камерой, и дорожными знаками, выбранными из данных HERE HDLM.

  4. Примените коррекции к предыдущим измерениям GPS.

% 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);

Исправленная трассировка локализации более точно соответствует локализации уровня маршрута по сравнению с фактическим приводом.

Вспомогательные функции

helperReadRosbag считывает данные датчика из rosbag и выводит синхронизированные облака точек, изображения и данные GPS. Это предназначено для иллюстрации обработки rosbag. В этом примере он обрабатывает только фрагмент данных, который хранится в постпроцессированных файлах MAT. Эта функция требует ROS Toolbox™.

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

helperProcessHDLMData обрабатывает данные, доступные в TopologyGeometry и LocalizationSign слои, используемые для локализации.

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

helperCollectLinksData собирает соответствующую информацию обо всех ссылках, отображаемых в плитках.

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

helperCollectSignData собирает идентификаторы и положения дорожных знаков.

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

helperSegmentLinks разбивает длинные ссылки на более короткие сегменты.

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

helperFindClosestLink находит ближайшая ссылка к местоположению GPS.

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

helperLidar2latlon преобразует координаты из лидара системы координат в систему координат GPS.

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

helperDistance вычисляет расстояние между двумя группами точек GPS.

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

helperDisplayResults отображает результаты GPS конвейера на карте. Эта основная карта относится к HERE Technologies и требует действительной лицензии отдельно от лицензии HERE HDLM. Чтобы использовать эту карту, вы должны ввести идентификатор ключа доступа, соответствующий вашей лицензии HERE.

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

helperDisplayCameraFrame отображает одно изображение камеры с 2-D ограничивающими рамками дорожных знаков.

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

helperDisplayPointCloud Отображения один лидар облака точек с 3-D ограничивающими рамками дорожных знаков.

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

См. также

Похожие темы

Для просмотра документации необходимо авторизоваться на сайте