В этом примере показано, как использовать данные знака трафика от сервиса HERE HD Live Map (HERE HDLM), чтобы исправить измерения GPS, собранные автономным транспортным средством.
Данные HERE HDLM содержат очень подробную и точную информацию об окружении транспортного средства, такую как топология дорог и маршрутов, и подходят для разработки беспилотных аппаратов. Обзор данных, доступных из слоев HERE HDLM, см. в HERE HD Live Map Layers. Этот пример фокусируется на слое HD Localization Model, который содержит точную и плотную информацию о дорожных знаках. Можно улучшить точность локализации путем объединения этой информации с живыми обнаружениями дорожных знаков с бортовых датчиков. Этот пример демонстрирует упрощенное решение, в котором вы извлекаете данные о местоположении дорожного знака и затем используете его для исправления показаний GPS. Более полное решение использует такие данные как ориентиры в конвейере одновременной локализации и картографии (SLAM) на основе ориентиров.
В этом примере вы узнаете, как:
Считывайте и обрабатывайте информацию о дорожных знаках из данных HERE HDLM с помощью последовательности GPS.
Сопоставьте дорожные знаки, обнаруженные бортовым лидаром и датчиками камеры, со знаками, сохраненными в данных HERE HDLM.
Вычислите ошибку положения путем сравнения известных расположений знаков в 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
слой:
Для каждого местоположения GPS найдите ближайшая ссылка в плитке с помощью helperFindClosestLink
функция.
Найдите ближайший дорожный знак к положению транспортного средства на основе LocalizationSign
данные слоя. Затем сопоставьте этот знак с одним из дорожных знаков, обнаруженных лидаром и камерой.
После того, как дорожный знак больше не обнаруживается, вычислите различие в положении между дорожными знаками, обнаруженными лидаром и камерой, и дорожными знаками, выбранными из данных HERE HDLM.
Примените коррекции к предыдущим измерениям 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