主要内容

本地化调整使用交通标志数据从这里高清地图

打开生活的脚本

这个例子展示了如何使用这里的交通标志数据高清生活(这里HDLM)服务映射到正确的自主车辆收集的GPS测量。

概述

这里HDLM数据包含非常详细和准确的车辆周围环境的信息,如道路和通道拓扑,和适合开发自动驾驶的应用程序。概述的数据可以从这里HDLM层,明白了高清生活映射层。这个例子着重于高清定位模型层,其中包含精确和密集的交通标志的信息。你可以提高定位精度,将这些信息与实时检测交通标志的机载传感器。这个案例展示了一个简化的解决方案的检索路标位置数据,然后使用它来正确的GPS数据。一个更完整的解决方案中使用这些数据作为地标landmark-based同步定位和映射(大满贯)管道。

在这个例子中,您将了解如何:

  1. 从这里读取和处理交通标志信息使用GPS HDLM数据序列。

  2. 交通标志检测到匹配的机载激光雷达和摄像头传感器信号存储在这里HDLM数据。

  3. 计算位置误差通过比较已知信号位置在这里HDLM GPS-returned位置,和正确的定位结果。

负荷传感器数据

在本例中使用的车辆传感器数据被记录rosbag使用ROS。rosbag的例子只包含一个代码片段展示了如何提取、同步和数据后处理。精简定位调整,其余的例子使用了后续处理数据存储在垫子上文件。

%下载传感器数据baseDownloadURL =“https://ssd.mathworks.com/金宝appsupportfiles/vision/data/localizationAutonomousCarDataset.zip”;dataFolder = fullfile (tempdir,“localizationAutonomousCarDataset”,filesep);选择= weboptions (“超时”、正);zipFileName = [dataFolder,“localizationAutonomousCarDataset.zip”];folderExists =存在(dataFolder,“dir”);%在一个临时目录中创建一个文件夹来保存下载文件如果~ folderExists mkdir (dataFolder) disp (“下载localizationAutonomousCarDataset。zip (1.01 GB)。这个下载可以花几分钟”。)websave (zipFileName baseDownloadURL选项);%提取下载的文件的内容disp (“提取localizationAutonomousCarDataset。邮政编码(1.01 GB)……”)解压缩(zipFileName dataFolder)结束从rosbag %提取数据(需要ROS工具箱)readRosbagData = false;hasROSToolbox =许可证(“测试”,“ros_toolbox”);如果readRosbagData & & hasROSToolbox filepath = [dataFolder,“\ rosbag \ demoRosbag.bag”];% helperReadRosbag使用小rosbag示例来展示一个更大的% rosbag可以处理。整个数据集加载从垫文件下面。[ptCloudsRosbag, imgsRosbag gpsRosbag] = helperReadRosbag (filepath);结束%负载传感器数据直接从垫文件同步。这个数据是%提取更大rosbag收集使用自主车。一个=负载([dataFolder,“\传感器\ images.mat”img]);%的图片(dataFolder ptClouds =负载(,“\传感器\ ptClouds.mat”]).pcds;%点云全球定位系统(gps) =负载([dataFolder,“\传感器\ gps.mat”]).gps;% GPS信号

在一个完整的管道中,首先检测相机帧中的交通标志,然后他们投射到激光雷达点云使用lidar-camera校准结果。这个投影给你的3 d位置交通标志,可以用来计算误差度量基于交通标志检测到在这里高清地图数据。为了简化处理出于演示目的,本例中的交通标志数据注解使用地面实况贴标签机应用。负载的边框的交通标志图像和点云。

(dataFolder imgBBoxes =负载(,“\ \ imgBBoxes.mat效用”]).imgBBoxes;(dataFolder pcBBoxes =负载(,“\ \ pcBBoxes.mat效用”]).pcBBoxes;

Undistort使用相机校正图像。使用获得的校准数据相机校准器应用程序。

(dataFolder camCalib =负载(,“\ \ calibration.mat效用”]).calib;intrinsic = camCalib.intrinsics;

显示一个帧检查传感器数据。

%选择一个框架frameIndex = 110;%显示相机图像和二维边界框的交通标志helperDisplayCameraFrame(一、imgBBoxes frameIndex intrinsic);

%显示点云,交通标志的3 d边界框helperDisplayPointCloud (ptClouds pcBBoxes frameIndex);

这里HDLM数据包含在地图的链接。加载GPS坐标的一个完整的路线。这条路线在三HDLM瓷砖覆盖延伸的链接。

(dataFolder gpsRoute =负载(,“\ \ gpsRoute.mat效用”]).gps_route;

读取地图数据

读取数据从GPS路线的地图,使用hereHDLMReader对象。使用高清生活地图服务需要有效这里HDLM凭证。你需要进入一个单独的协议,为了获得HDLM服务和获得所需的凭证(access_key_id和access_key_secret)使用这里的服务。

%读者创建和配置配置= hereHDLMConfiguration (“hrn::数据::olp-here-had: here-hdlm-protobuf-na-2”,2291);读者= hereHDLMReader (gpsRoute (: 1), gpsRoute (:, 2),“配置”、配置);%读取数据topologyLayer =阅读(读者,“TopologyGeometry”);signLayer =阅读(读者,“LocalizationSign”);

预处理的数据来改善搜索的时候查询所需的数据。辅助函数helperProcessHDLMData定义的例子,GPS路线并返回以下数据:

  • linkData——包含所有的链接出现在瓷砖跨越GPS路线。分割成双的链接(纬度、经度)坐标。

  • signData——包含信息中可用的迹象瓷砖跨越GPS路线。

[linkData, signData] = helperProcessHDLMData(配置、topologyLayer signLayer);

定位、交通标志匹配和错误修正

本地化的自主车,使用GPS测量和链接形成中的每个道路TopologyGeometry层:

  1. 对于每一个GPS定位,找到最接近的瓷砖使用链接helperFindClosestLink函数。

  2. 找到最接近的交通标志的车辆位置的基础上LocalizationSign层数据。然后,匹配信号的交通标志检测的激光雷达和摄像头。

  3. 一旦不再检测到交通信号,计算出不同位置之间的交通标志检测的激光雷达和摄像头和交通标志从这里HDLM中选择数据。

  4. 应用修正先前的GPS测量。

%初始化变量hdlmDetectedSigns = 0 (0, 2);lidarDetectedSigns = 0 (0, 2);relevantGPS = 0 (0, 2);finalGPS = 0 (0, 4);finalSigns = 0 (0, 4);numDetections = 0;为i = 1:元素个数(gps) gpsMeasurement = gps{我};pcBBox = pcBBoxes {};%找到最近的链接closestLinkID = helperFindClosestLink (linkData gpsMeasurement);isSignDetected = ~ isempty (pcBBox);如果isSignDetected%如果检测到交通标志由激光雷达和摄像头,发现的ID%的交通标志是最接近当前GPS位置。gpsMeasurement distToSigns = helperDistance (gpsMeasurement (1), (2),…signData (:, 2), signData (:, 3));[~,minIndex] = min (distToSigns);closestSignID = signData (minIndex, 1);%确保最亲密的交通标志是最亲密的联系为m = 1:尺寸(signLayer 1) roadToSignsRefs = signLayer (m, 1) .RoadToSignsReferences;如果ismember (closestLinkID [roadToSignsRefs.LinkRef]) signRefs = vertcat (roadToSignsRefs.SignRefs);如果ismember (closestSignID [signRefs.SignId])%来自激光雷达坐标系的坐标变换到GPS框架(纬度、经度)= helperLidar2latlon (pcBBox, gps,我);%存储相关的结果hdlmDetectedSigns = [hdlmDetectedSigns;signData (minIndex 2:3)];% #好< * AGROW >lidarDetectedSigns = [lidarDetectedSigns;纬度、经度);relevantGPS = [relevantGPS;gpsMeasurement (1:2)];numDetections = numDetections + 1;打破结束结束结束elseifnumDetections > 1%一旦不再检测到交通信号,计算误差%的交通标志检测激光雷达和摄像头和交通标志%从HDLM中选择数据。错误= hdlmDetectedSigns(最终1:2)——意味着(lidarDetectedSigns (: 1:2));%将校正应用于GPS坐标报告的GPS传感器correctedGPS = relevantGPS +错误;%保存结果finalGPS = [finalGPS;relevantGPS correctedGPS);finalSigns = [finalSigns;意味着(lidarDetectedSigns) hdlmDetectedSigns(结束:)];%初始化存储变量numDetections = 0;relevantGPS = 0 (0, 2);hdlmDetectedSigns = 0 (0, 2);lidarDetectedSigns = 0 (0, 2);结束结束

地图上显示结果

显示的结果在地图上定位管道使用纬度和经度计算测量。

helperDisplayResults (finalSigns finalGPS topologyLayer);

修正定位跟踪匹配巷水平定位更准确地比一个实际的驱动器。

辅助函数

helperReadRosbag从rosbag读取传感器数据和输出同步点云,图像,和GPS数据。它是为了说明rosbag处理。在这个例子中,it过程只有一个片段的数据存储在后续处理垫文件。这个函数需要ROS工具箱™。

函数[ptClouds、图像、gps] = helperReadRosbag (filepath)袋= rosbag (filepath);%的名字ROS的话题lidarTopic =“os1_cloud_node /点”;imageTopic =“/相机/ image_color”;gpsTopic =' / raw_fix ';bagLidar =选择(包,“主题”,lidarTopic);bagImage =选择(包,“主题”,imageTopic);bagGPS =选择(包,“主题”,gpsTopic);%阅读消息msgStructsImage = readMessages (bagImage);msgStructsPC = readMessages (bagLidar);msgStructsGPS = readMessages (bagGPS);numLidarFrames =大小(bagLidar.MessageList, 1);ptClouds =细胞(numLidarFrames, 1);图像=细胞(numLidarFrames, 1);全球定位系统(gps) =细胞(numLidarFrames, 1);%因为每个传感器以不同的频率运行,使用%激光雷达的收购率作为同步激光雷达的基础,%视频和GPS数据。从其他传感器收集帧%选择最接近的人根据他们的时间戳。为i = 1: numLidarFrames timeStamp = bagLidar.MessageList.Time(我);%激光雷达点云msgPC = msgStructsPC {};ptClouds{我}= pointCloud (readXYZ (msgPC));%相机数据[~,minIdx] = min (abs (bagImage.MessageList.Time-timeStamp));图片我{}= readImage (msgStructsImage {minIdx});% GPS数据[~,minIdx] = min (abs (bagGPS.MessageList.Time-timeStamp));全球定位系统(gps){我}= [msgStructsGPS {minIdx} .Latitude,…msgStructsGPS {minIdx} .Longitude,…msgStructsGPS {minIdx} .Altitude];结束结束

helperProcessHDLMData过程中可用的数据TopologyGeometry和LocalizationSign层用于本地化。

函数[linkData, signData] = helperProcessHDLMData(配置、topologyLayer signLayer)%收集所有瓷砖从TopologyGeometry层中的链接[linkIDs, linkGeometries] = helperCollectLinksData(配置,topologyLayer);%把链接分门别类的两点。这样更准确%结果当找到最近的链接对于一个给定的GPS位置linkData = helperSegmentLinks (linkIDs linkGeometries);%收集所有从LocalizationSign层瓷砖的迹象signData = helperCollectSignData (signLayer);结束

helperCollectLinksData收集相关信息的链接出现在瓷砖。

函数[linkIDs, linkGeometries] = helperCollectLinksData(配置、topologyLayer) linkIDs = [];linkGeometries = [];intersectLinkRefs = vertcat (topologyLayer.IntersectingLinkRefs);tileIDs =独特(vertcat (intersectLinkRefs.LinkHereTileId));读者= hereHDLMReader (tileIDs,“配置”、配置);拓扑=阅读(读者,“TopologyGeometry”);为j = 1:尺寸(topologyLayer, 1)%交叉链接currIntersectLinkRefs = topologyLayer (j, 1) .IntersectingLinkRefs;为i = 1:尺寸(currIntersectLinkRefs 1) tileID = currIntersectLinkRefs .LinkHereTileId;linkID = currIntersectLinkRefs .LinkId;拓扑=拓扑(tileIDs = = tileID);linksStartingInTile = topology.LinksStartingInTile;linksTileID = vertcat (linksStartingInTile.LinkId);linkGeometry = linksStartingInTile (linksTileID = = linkID) .Geometry.Here2dCoordinateDiffs;linkIDs = [linkIDs;linkID];linkGeometries = [linkGeometries;{linkGeometry}];结束%从瓷砖的链接linksStartingInTile = topologyLayer (j, 1) .LinksStartingInTile;为i = 1:尺寸(linksStartingInTile, 1) linkIDs = [linkIDs;linksStartingInTile .LinkId);linkGeometry = linksStartingInTile .Geometry.Here2dCoordinateDiffs;linkGeometries = [linkGeometries;{linkGeometry}];结束结束结束

helperCollectSignData收集id和交通标志的位置。

函数signData = helperCollectSignData (signLayer) signData = [];为i = 1:尺寸(signLayer, 1) signData = [signData;双(vertcat (signLayer(我).Signs.SignId)),…vertcat (signLayer(我).Signs.Here2dCoordinate)];结束结束

helperSegmentLinks将长链接分解为短段。

函数linkData = helperSegmentLinks (linkIDs linkGeometries) linkData = 0 (0 5);% (LinkID、start_lat start_lon、end_lat end_lon]为i = 1:元素个数(linkIDs)点= linkGeometries {};numSegments =大小(点,1)- 1;linkData = [linkData;[repmat(双(linkIDs(我)),numSegments, 1),…点(1:numSegments, 1:2),点(2:numSegments + 1, 1:2)];结束结束

helperFindClosestLink找到最接近的GPS位置的链接。

函数closestLinkID = helperFindClosestLink (linkData, gps)%计算距离的链接distToLinkStart = helperDistance (gps (1), gps (2), linkData (:, 2), linkData (:, 3));distToLinkEnd = helperDistance (gps (1), gps (2), linkData (:, 4), linkData (:, 5));distToLinks = min (distToLinkStart distToLinkEnd);%找到最近的链接[~,指数]= min (distToLinks);closestLinkID = linkData(指数(1);结束

helperLidar2latlon将激光雷达坐标系的坐标转换为GPS框架。

函数(纬度、经度)= helperLidar2latlon (bbox, gps,我)%的激光雷达坐标转换检测交通标志和纬度%经度测量。这是通过首先将激光雷达坐标%的车辆帧,然后东,北,(ENU表示)框架,最后GPS框架。%在激光雷达框架边界框的中心。中心= bbox (1:3);%计算方位。gps lon1 ={1,我}(1);gps lon2 = {1,} (1);gps lat1 ={1,我}(2);gps lat2 = {1,} (2);dLon = lon2 - lon1;y =罪(dLon) * cos (lat2);x = cos (lat1) * sin (lat2)——sin (lat1) * cos (lat2) * cos (dLon);θ=量化(y、x);initialBearing =国防部(θ+ 2 *π2 *π);finalBearing =国防部(initialBearing +π/ 2,2 * pi);%从激光雷达坐标系变换到车辆帧。x =中心(1);y =中心(2);%从车辆帧变换ENU表示框架。e =罪(finalBearing) * x - cos (finalBearing) * y;罪n = cos (finalBearing) * x + y (finalBearing) *;% ENU表示帧的变换GPS框架。%的起源ENU表示本地框架设置为当前GPS定位。起源= [gps全球定位系统(gps) {1,} (1) {1,} (2) gps {1,} (3)];(纬度、经度)= local2latlon (e, n, 0,起源);结束

helperDistance计算两组GPS点之间的距离。

函数d = helperDistance (lat1 lon1、lat2 lon2) numSignals =元素个数(lat2);(x, y) = latlon2local (lat2 lon2, 0 (numSignals, 1), [lat1 lon1 0]);%假设零高度d =√x。^ 2 + y ^ 2);结束

helperDisplayResults显示GPS地图上的管道的结果。这个技术是技术和需要一个有效的许可证分开你这里HDLM执照。使用这张地图,你必须输入访问密钥ID对应于你的许可。

函数helperDisplayResults (finalSigns finalGPS topologyLayer)%为进入访问密钥ID创建对话框。url = [“https://1.base.maps.ls.hereapi.com/maptile/2.1/maptile/”,…的最新/ normal.day / $ {z} / {x} / {y}美元/ 256 / png ? apikey = % s '];提示= {“这里访问密钥ID:”};title =“这令牌”;dim = 40 [1];%文本编辑字段高度和宽度hereTokens = inputdlg(提示、标题、dim);如果~ isempty (hereTokens)%添加技术有自定义属性。url = sprintf (url, hereTokens {1});copyrightSymbol = char (169);归因= [copyrightSymbol,' 'datestr(现在,“yyyy”),“在这里”];addCustomBasemap (“herestreets”、url、“归因”、归因);图(f =“名字”,修正GPS测量的);gx = geoaxes (“父”f);情节(topologyLayer“轴”、gx);持有(gx“上”);传奇(gx“界限”,“节点”,“链接”,“位置”,“西北”);nlat = [finalSigns (1,1) finalSigns (, 1)];nlon = [finalSigns (1、2) finalSigns (, 2)];nzoom = 16;geolimits (gx nlat nlon);gx。ZoomLevel = nzoom;geobasemap (gx“herestreets”);geoplot (gx finalSigns (: 1), finalSigns (:, 2),“o”,“DisplayName的”,“激光雷达交通标志”);geoplot (gx finalSigns (:, 3), finalSigns (:, 4),‘*’,“DisplayName的”,“HDLM交通标志”);geoplot (gx finalGPS (: 1), finalGPS (:, 2),的r - +,“DisplayName的”,“GPS校正之前”);geoplot (gx finalGPS (:, 3), finalGPS (:, 4),“g - +”,“DisplayName的”,GPS校正后的);其他的错误(“你必须从这里输入有效的凭证访问地图技术”);结束结束

helperDisplayCameraFrame显示一个摄像头与二维交通标志图像边界框。

函数helperDisplayCameraFrame(一、imgBBoxes frameIndex intrinsic) img = undistortImage(一个{frameIndex}, intrinsic);图(f =“名字”,“相机帧”);甘氨胆酸ax = (f);%显示相机的帧imshow (img,“父”、ax);(ax,“上”)showShape (“矩形”,imgBBoxes {frameIndex},“颜色”,“蓝”,“线宽”2,“父”、ax);结束

helperDisplayPointCloud显示一个激光雷达点云三维交通标志边界框。

函数helperDisplayPointCloud (ptClouds pcBBoxes frameIndex)图(f =“名字”,“点云”);甘氨胆酸ax = (f);pcshow (ptClouds {frameIndex},“父”、ax);(ax,“上”)包含(ax,“X”)ylabel (ax,“Y”)zlabel (ax,“Z”40)xlim ([-40]) ylim (40 [-20]) zlim(10[5])视图(45 [90])showShape (“长方体”,pcBBoxes {frameIndex},“颜色”,“红色”,“线宽”2,“父”、ax);结束

另请参阅

相关的话题