加载数据

本例使用了Ouster OS1激光雷达传感器在高速公路上收集的激光雷达数据和安装在ego车辆上的前置摄像头的图像数据。激光雷达和相机的数据是大约时间同步和校准，以估计他们的内在和外在参数。有关激光雷达相机校准的更多信息，请参见激光雷达和相机校准．

注意:下载数据的时间取决于您的互联网连接速度。在执行此代码块期间，MATLAB暂时无响应。

lidarTarFileUrl =“//www.tatmou.com/金宝appsupportfiles/lidar/data/WPI_LidarData.tar.gz”；imageTarFileUrl =“//www.tatmou.com/金宝appsupportfiles/lidar/data/WPI_ImageData.tar.gz”；outputFolder = fullfile(tempdir，批发价格指数的）;lidarDataTarFile = fullfile(输出文件夹，“WPI_LidarData.tar.gz”）;imageDataTarFile = fullfile(输出文件夹，“WPI_ImageData.tar.gz”）;如果~存在(outputFolder“dir”mkdir (outputFolder)结束如果~存在(lidarDataTarFile“文件”) disp (“下载WPI激光雷达驾驶数据(760 MB)……”(lidarDataTarFile,lidarTarFileUrl) untar(lidarDataTarFile,outputFolder)结束检查lidar tar.gz文件是否已下载，但未解压。如果~ (fullfile (outputFolder,存在“WPI_LidarData.mat”)，“文件”)解压(lidarDataTarFile outputFolder)结束如果~存在(imageDataTarFile“文件”) disp (“正在下载WPI图像驾驶数据(225 MB)……”untar(imageDataTarFile,outputFolder)结束检查image tar.gz文件是否已下载，但未解压缩。如果~ (fullfile (outputFolder,存在“imageData”)，“dir”)解压(imageDataTarFile outputFolder)结束imageDataLocation = fullfile(输出文件夹，“imageData”）;images = imageSet(imageDataLocation);imageFileNames = images.ImageLocation;将下载的激光雷达数据加载到工作空间lidarData = fullfile(输出文件夹，“WPI_LidarData.mat”）;负载(lidarData);负载校准数据如果~ (“calib”，“var”)负载(“calib.mat”）结束定义相机到激光雷达的变换矩阵camToLidar = calibre .extrinsics;intrinsic = calibre .intrinsic;

或者，您可以使用web浏览器先将数据集下载到本地磁盘，然后解压缩文件。

本例使用预标记数据作为来自相机图像的二维检测的ground truth。这些二维检测可以使用基于深度学习的对象检测器来生成vehicleDetectorYOLOv2(自动驾驶工具箱)，vehicleDetectorFasterRCNN(自动驾驶工具箱),vehicleDetectorACF(自动驾驶工具箱)．对于本例，已经使用训练图像标签这些二维边界框是这样的向量: $［\mathit{x}\mathit{y}\mathit{w}\mathit{h}］$,在那里 $\mathit{x}\mathrm{而且}\mathit{y}$代表了xy-左上角的坐标，和 $\mathit{w}\mathrm{而且}\mathit{h}$分别表示包围框的宽度和高度。

将图像帧读入工作区，并将其与覆盖的包围框一起显示。

负载imageGTruth.matim = imread(imageFileNames{50});imBbox = imageGTruth{50};图imshow(im)“矩形”imBbox)

三维区域方案

为了从图像数据中的二维矩形包围盒生成激光雷达中的长方体包围盒，提出了一个三维区域来减小包围盒估计的搜索空间。利用相机固有参数和相机-激光雷达外部参数，将图像中每个二维矩形包围框的角转换为三维线。这些3-D线形成了从相关的2-D包围框中向自我车辆相反方向展开的截锥。落在该区域内的激光雷达点根据欧几里得距离被分割成各种簇。聚类采用三维定位的包围框进行拟合，并根据这些聚类的大小来估计最佳聚类。基于相机图像中的二维包围盒，使用的方法估计激光雷达点云中的三维方向包围盒bboxCameraToLidar函数。该图显示了2-D和3-D包围框之间的关系。

三维长方体表示为如下形式的向量: $［\mathit{xcen}\mathit{ycen}\mathit{zcen}\mathit{dimx}\mathit{dimy}\mathit{dimz}\mathit{rotx}\mathit{roty}\mathit{rotz}］$,在那里 $\mathit{xcen}，\mathit{ycen}，\mathrm{而且}\mathit{zcen}$表示长方体的质心坐标。 $\mathit{dimx}，\mathit{dimy}，\mathrm{而且}\mathit{dimz}$表示长方体沿的长度x -，y -,z -轴, $\mathit{rotx}，\mathit{roty}，\mathrm{而且}\mathit{rotz}$表示长方体沿的旋转(以度为单位)x -，y -,z -轴。

利用图像的地真值估计激光雷达点云中的三维包围盒。

pc = lidarData{50};裁剪点云仅处理前面区域ROI = [0 70 -15 15 -3 8];ind = findPointsInROI(pc,roi);PC = select(PC,ind);lidarBbox = bboxCameraToLidar(imBbox,pc,intrinsic，.．.camToLidar,“ClusterThreshold”，2，“MaxDetectionRange”[70]);图pcshow(pc.Location,pc.Location(:，3))“长方体”，lidarBbox) view([-2.90 71.59])

为了改善检测到的边界盒，对点云进行预处理，去除地平面。

设置显示

使用helperLidarCameraObjectsDisplay类来可视化激光雷达和图像数据。这种可视化提供了同时查看点云、图像、点云上的3-D包围框和图像上的2-D包围框的能力。可视化布局由以下窗口组成:

初始化显示display = helperLidarCameraObjectsDisplay;initializeDisplay(显示)%更新显示点云和图像updateDisplay(显示，im, pc)

循环数据

在前200帧的2-D标签上运行bboxCameraToLidar生成3-D长方体

为I = 1:200加载点云和图像im = imread(imageFileNames{i});pc = lidarData{i};加载图像地面真实值imBbox = imageGTruth{i};移除地平面groundPtsIndex = segmentGroundFromLidarData(pc，“ElevationAngleDelta”15岁的.．.“InitialElevationAngle”10);nonGroundPts = select(pc，~groundPtsIndex);如果imBbox [lidarBbox，~，boxUsed] = bboxCameraToLidar(imBbox,nonGroundPts,intrinsics，.．.camToLidar,“ClusterThreshold”，2，“MaxDetectionRange”[70]);显示带有边框的图像im = updateImage(display,im,imBbox);结束显示带包围框的点云updateDisplay(显示、即时通讯、电脑);updateLidarBbox drawnow(显示、lidarBbox boxUsed)结束

利用边界盒跟踪检测边界盒，如联合概率数据关联(JPDA)。有关更多信息，请参见使用激光雷达跟踪车辆:从点云到跟踪列表．

估计车辆与Ego车辆之间的距离

对于前方碰撞预警等车辆安全功能，准确测量车辆与其他物体之间的距离至关重要。激光雷达传感器可提供物体与自我车辆的精确三维距离，也可用于从二维图像包围框自动创建地面真相。若要为2-D边界框生成真实值，请使用projectLidarPointsOnImage函数将3d包围框内的点投影到图像上。投影点通过寻找与投影3-D点的欧几里得距离最小的边界框与2-D边界框相关联。由于投影点从激光雷达到相机，使用相机到激光雷达的逆外部参数。这张图说明了从激光雷达到相机的转变。

初始化显示display = helperLidarCameraObjectsDisplay;initializeDisplay(显示)让激光雷达到相机矩阵lidarToCam = invert(camToLidar);循环前200帧。要循环所有帧，将200替换为nummel (imageGTruth)为i = 1:20 00 im = imread(imageFileNames{i});pc = lidarData{i};imBbox = imageGTruth{i};移除地平面groundPtsIndex = segmentGroundFromLidarData(pc，“ElevationAngleDelta”15岁的.．.“InitialElevationAngle”10);nonGroundPts = select(pc，~groundPtsIndex);如果imBbox [lidarBbox，~，boxUsed] = bboxCameraToLidar(imBbox,nonGroundPts,intrinsics，.．.camToLidar,“ClusterThreshold”，2，“MaxDetectionRange”[70]);[distance,nearestRect,idx] = helperComputeDistance(imBbox,nonGroundPts,lidarBbox，.．.intrinsic lidarToCam);用包围框更新图像im = updateImage(display,im,nearestRect,distance);lidarBbox updateLidarBbox(显示)结束%更新显示updateDisplay drawnow(显示、即时通讯、电脑)结束

金宝app支持文件

helperComputeDistance

函数[distance, nearestRect, index] = helperComputeDistance(imBbox, pc, lidarBbox, intrinsic, lidarToCam)% helperComputeDistance估计给定的2-D包围框的距离%图像使用三维包围框从激光雷达。它还计算% 2-D和3-D包围框之间的关联版权归MathWorks, Inc.所有numLidarDetections = size(lidarBbox,1);nearestRect = 0 (0,4);距离=零(1,numLidarDetections);索引= 0 (0,1);为i = 1:numLidarDetections bboxCuboid = lidarBbox(i，:);创建cuboidModelmodel = cuboidModel(bboxCuboid);找到长方体内的点ind = findPointsInsideCuboid(模型，pc);PTS = select(pc,ind);%项目长方体指向图像imPts = projectLidarPointsOnImage(pts,intrinsic,lidarToCam);找到三维包围框对应的二维矩形[nearestRect(i，:)，idx] = findNearestRectangle(imPts,imBbox);索引(end+1) = idx;求二维矩形的距离距离(i) = min(pts.Location(:，1));结束结束函数[nearestRect,idx] = findNearestRectangle(imPts,imBbox) numbox = size(imBbox,1);比率=零(numbox,1);遍历所有矩形为i = 1: imBbox(i，:);拐角= get拐角frombbox (bbox);求投影点与矩形的重叠率idx = (imPts(: 1) >角落(1,1))& (imPts(: 1) <角落(2,1))&.．.(imPts(:, 2) >角落(1、2)& (imPts(:, 2) <角落(3,1));Ratio (i) = sum(idx);结束获取最近的矩形[~，idx] = max(ratio);nearestRect = imBbox(idx，:);结束函数角摄像机= getCornersFromBbox(bbox)角摄像机=零(4,2);角相机(1,1:2)= bbox(1:2);bbox(2,1:2) + [bbox(3)，0];角相机(3,1:2)= bbox(1:2) + bbox(3:4);bbox(1:2) + [0,bbox(4)];结束