使用激光雷达数据检测和跟踪车辆

这个示例使用:

开放脚本

此示例演示如何使用安装在ego车辆顶部的激光雷达传感器的测量值跟踪车辆。激光雷达传感器将测量值报告为点云。此示例说明了MATLAB®中处理点云和跟踪对象的工作流。有关此示例的Simulink®版本，请参阅金宝app在Simulink中使用激光雷达数据跟踪车辆金宝app（传感器融合和跟踪工具箱）．本例中使用的激光雷达数据是从高速公路驾驶场景中记录的。在本例中，使用联合概率数据关联(JPDA)跟踪器和交互多模型(IMM)方法，使用记录的数据跟踪车辆。

三维包围盒探测器模型

由于激光雷达传感器的高分辨率能力，传感器的每次扫描都包含大量的点，通常称为点云。必须对这些原始数据进行预处理，以提取感兴趣的对象，如汽车、自行车和行人。有关将激光雷达数据分割成地面和障碍物等物体的详细信息，请参阅用激光雷达探测地面和障碍物(自动驾驶工具箱)实例在本例中，属于障碍物的点云使用pcsegdist函数，每个簇将转换为具有以下格式的边界框检测：

$[x\ y\ z\ l\ w\ h]$

x美元那 $y$ 和 $z$ 参考边界框的x、y和z位置和 $ l $ 那 w美元和 $h$ 分别指其长度、宽度和高度。

边界框通过使用每个维度中的最小和最大点的最小和最大坐标适合每个群集。检测器由支持类实现金宝appHelperboundingBoxDetector，它围绕点云分割和聚类功能展开。此类的对象接受pointCloud输入并返回的列表ObjectDetection.带有边界盒测量值的对象。

该图显示了边界框检测器模型中涉及的过程和用于实现每个进程的计算机视觉工具箱™功能。它还显示了控制每个过程的支持类的属性。金宝app

激光雷达数据可通过以下链接获得：https://ssd.mathworks.com/金宝appsupportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip

将数据下载到临时目录，其位置由MATLAB的tempdir函数指定。如果要将文件放在不同的文件夹中，请在后续说明中更改目录名称。

%如果不可用，则加载数据。激光雷达数据以单元阵列的形式存储% pointCloud对象。如果~存在('lidardata'那“var”)数据URL=“https://ssd.mathworks.com/金宝appsupportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip”；datasetFolder = fullfile (tempdir,“LidarExampleDataset”）;如果~存在(datasetFolder“dir”)解压（dataURL，datasetFolder）；结束%指定模拟的初始和最终时间。initTime=0；finalTime=35；[lidarData，imageData]=loadLidarAndImageData（datasetFolder，initTime，finalTime）；结束％设置随机种子以产生可重复的结果。S = rng (2018);一个边界盒检测器模型。detectorModel=HelperBoundingBoxDetector(...“XLimits”,[-50 75],...%最小最大值“YLimits”,[-5 5],...%最小最大值“ZLimits”,[-2 5],...%最小最大值“SegmentationMinDistance”, 1.6,...百分之值距离最低“MinDetectionsPerCluster”，1，...每个集群最低积分%“MeasurementNoise”，眼睛（6），...%检测报告中的测量噪声“地面最大距离”, 0.3);接地点到接地面的最大距离

目标状态和传感器测量模型

跟踪目标的第一步是定义其状态，以及定义状态转换和相应测量的模型。这两组方程统称为目标的状态空间模型。为了使用激光雷达对跟踪车辆的状态进行建模，本示例使用了一个长方体模型，该模型具有以下约定：离子：

$x=[x{kin}\{\theta}\l\w\h]$

美元间{亲属}$ 指控制运动中心运动学的状态部分，以及 $\θ美元$ 是偏航角。长方体的长度、宽度和高度被建模为常数，其估计值在滤波器校正阶段随时间而变化。

在本例中，将使用两个状态空间模型：恒定速度（cv）长方体模型和恒定转动率（ct）长方体模型。这些模型定义状态运动学部分的方式不同，如下所述：

$美元间{简历}= [x y \{\点{x}} \ \{\点{y}} \ z \{\点{z}} \{\θ}\ h l \ w \]美元$

$x_ {ct} = [x \ {\ dot {x}} \ y \ {\ dot {y}} \ {\ dot {\ theta}} \ z \ {\ dot {z}} \ {\ theta}\ l \ w \ h]$

有关其状态转换的信息，请参阅helperConstvelCuboid和helperConstturnCuboid本例中使用的函数。

这helperCvmeasCuboid和helperCtmeasCuboid测量模型分别描述了传感器如何感知恒定速度和恒定转速状态，并返回边界盒测量值。由于状态包含关于目标大小的信息，测量模型包括由传感器感知到的中心点偏移和边界盒收缩的影响，这是由于自遮挡[1]等影响造成的。这种效应是通过收缩系数来模拟的，收缩系数与被跟踪车辆到传感器的距离成正比。

下图展示了在不同状态空间样本下运行的测量模型。请注意，当对象围绕车辆移动时，边界框收缩和中心点偏移的建模效果。

设置跟踪器和可视化

下图显示了从点云输入获取轨迹列表的完整工作流。

现在，设置示例中使用的跟踪器和可视化。

联合概率数据关联跟踪器(追踪器JPDA）与IMM滤清器（跟踪IMM)在本例中，用于跟踪对象。IMM过滤器使用恒定速度和恒定转动率模型，并使用支持函数进行初始化，金宝appHelperinitimmfilter，包括在此示例中。IMM方法有助于轨迹在运动模型之间切换，从而在机动或换道等事件中获得良好的估计精度。下面的动画显示了在IMM过滤器的预测阶段混合恒定速度和恒定转动率模型的效果。

当使用来自对象的检测对每个模型进行校正时，IMM过滤器会更新其概率。下面的动画显示了车道改变事件期间车辆的估计轨迹以及每个模型的相应估计概率。

设定HasDetectableTrackIDsInput跟踪器的属性为真正的，它允许您指定与状态相关的检测概率。跟踪的检测概率由HelpercalcDetecity函数，列在本示例的最后。

assignmentGate=[50 100]；%分配阈值；Confthreshold = [7 10];%历史逻辑的确认阈值delThreshold = [8 10];%历史逻辑的删除阈值kc = 1e-5;%单位体积的虚警率% IMM过滤器初始化功能filterinitfcn = @helperinitimmfilter;带有IMM滤波器的联合概率数据关联跟踪器跟踪器= trackerjpda（'filterinitializationfcn'filterInitFcn,...“TrackLogic”那'历史'那...“AssignmentThreshold”，转让门，...“杂波密度”，Kc，...'确认察觉'confThreshold,...“DeletionThreshold”delThreshold,...“HasDetectableTrackIDsInput”符合事实的...“初始化阈值”, 0);

可视化分为以下主要类别：

激光雷达预处理和跟踪-显示原始点云、分割地面和障碍物。它还显示了从探测器模型得到的检测结果以及由跟踪器生成的车辆轨迹。
自我车辆显示器-这个显示器显示场景的2d鸟瞰图。它显示障碍物点云、包围盒检测和跟踪器生成的轨迹。作为参考，它还显示从安装在ego车上的摄像机记录的图像和它的视野。
跟踪细节-这个显示在自我车辆周围缩放的场景。它还显示了更精细的跟踪细节，如每个轨迹估计位置的误差协方差及其运动模型的概率，用cv和ct表示。

%创建显示displayObject = HelperlidareXampledisplay（Imagedata {1}，...“PositionIndex”(1 3 6),...“速度指数”,[2 4 7],...“DimensionIndex”(9 10 11),...“YawIndex”，8，...“MovieName”那＇＇那...%指定要录制电影的电影名称。“RecordGIF”，假）；%指定true以记录新的GIF

循环浏览数据

通过记录的激光雷达数据循环，使用探测器模型从当前点云生成探测，然后使用跟踪器处理探测。

时间= 0;％ 开始时间dT=0.1；%时间步启动所有轨道。allTracks=struct（[]）；%启动用于比较MATLAB和MEX模拟的变量。numtracks =零（numel（lidardata），2）;%循环浏览数据为i=1:numel（lidarData）%更新时间时间=时间+ dt;获得当前激光雷达扫描currentLidar = lidarData {};激光雷达探测到的生成器。[检测，障碍，地面，裁剪] =检察机（CurrentLidar，时间）;%计算每个道的可检测性。detectableTracksInput=helperCalcDetectability（所有轨道，[1 3 6]）；%通过检测跟踪。[confirmedTracks，tentivetracks，allTracks]=跟踪器（检测，时间，可检测的tracksinput）；numTracks（i，1）=numel（confirmedTracks）；%使用IMM滤波器从每个轨迹的IMM滤波器中获取模型概率跟踪器的% getTrackFilterProperties函数。modelProbs = 0(2,元素个数(confirmedTracks));为k=1:numel（confirmedTracks）c1=getTrackFilterProperties（跟踪器，confirmedTracks（k）.TrackID，“模型概率”); modelProbs（：，k）=c1{1}；结束％更新显示如果IsValid（displayObject.pointCloudProcessingDisplay.obStraclePlotter）％获取当前图像扫描参考图像currentImage = imageData {};%更新显示对象显示对象（检测、确认跟踪、当前激光雷达、障碍物、，...groundIndices、croppedIndices currentImage modelProbs);结束%在时间=18时捕捉图形如果abs(time - 18) < dT/2 snapnow(displayObject);结束结束如果有要求，写电影如果~isempty（displayObject.MovieName）writeMovie（displayObject）；结束%如果需要，写下新的GIF。如果displayObject。RecordGIF%第二次输入是开始帧，第三次输入是结束帧和最后一次输入%是指定要记录的面板的字符向量。writeAnimatedGIF (displayObject, 10170,“trackMaintenance”那'自我'）;writeAnimatedGIF (displayObject, 310330,jpda的那“处理”）;WriteAnimatedGIF（DisplayObject，150,180，'IMM'那“细节”）;结束

上图显示了时间=18秒时的三个显示。轨迹由绿色边界框表示。边界框检测由橙色边界框表示。探测中也有橙色点，表示分割为障碍物的点云。分段地面显示为紫色。裁剪或丢弃的点云以蓝色显示。

生成C代码

您可以使用MATLAB Coder™从MATLAB®代码生成跟踪和预处理算法的C代码。C代码生成使您能够加速MATLAB代码的仿真。为了生成C代码，算法必须重新构造为MATLAB函数，该函数可以编译为MEX文件或共享库。为此，将点云处理算法和跟踪算法重构为MATLAB函数，mexLidarTracker. 有些变量定义为执着的在多次调用函数之间保留其状态（请参见执着的)．在本例末尾的“支持文件”一节中提供的函数描述中，可以看到函数的输入和输出。金宝app

MATLAB编码器要求指定所有输入参数的属性。的示例在命令行中定义输入属性是一种简单的方法- args.选择。有关更多信息，请参见在命令行通过示例定义输入属性(MATLAB编码器)．类的对象不能作为顶级输入参数处理班因此，函数接受X那y和Z.作为输入的点云位置。从存储的点云中，可以使用地方财产pointCloud对象该信息也可作为激光雷达传感器的原始数据直接获取。

%输入列表InputExample = {Lidardata {1} .Location，0};为MEX生成创建配置cfg = coder.config (墨西哥人的）;%将cfg替换为以下文件生成静态库并执行% software-in-the-loop模拟。这需要嵌入式编码器许可证。％% CFG = code .config('lib');%静态库%cfg.VerificationMode='SIL'；%循环中的软件%如果文件不存在，则生成代码。如果~存在('mexlidartracker_mex'那“文件”）h = msgbox（{'生成代码。这可能需要几分钟……”；完成后此消息框将关闭。}，'codegen消息'）;% -config允许指定代码生成配置允许指定输出文件的名称编码基因-配置CFG.-omexLidarTracker_mexmexLidarTracker-  args.inputExample关闭(h);其他的清楚的mexLidarTracker_mex；结束

使用MEX代码重新运行模拟

使用生成的MEX代码重新运行模拟，mexLidarTracker_mex．

%复位时间时间= 0;为i = 1:numel(lidarData) time = time + dT;currentLidar = lidarData {};[detectionsMex, obstacleIndicesMex groundIndicesMex croppedIndicesMex,...confirmedTracksMex，modelProbsMex]=mexLidarTracker_-mex（当前激光雷达位置、时间）；%记录数据，以便与MATLAB执行进行比较。numTracks（i，2）=numel（confirmedTracksMex）；结束

比较MATLAB与MEX执行的结果

disp (isequal (numTracks (: 1), numTracks (:, 2)));

请注意，Matlab和MEX代码执行的确认轨道的数量是相同的。这确保了LIDAR预处理和跟踪算法与与MATLAB代码一样的生成的C代码返回相同的结果。

结果

现在，通过对场景中不同事件的分析，了解激光雷达测量模型、联合概率数据关联、交互多模型滤波器的组合如何帮助实现对车辆轨迹的良好估计。

跟踪维护

上面的动画显示了时间= 3秒到时间= 16秒之间的模拟。注意，像T10和T6这样的轨迹在时间跨度内保持它们的id和轨迹。但是，由于被跟踪车辆被传感器长时间错过(未检测到)，导致轨迹T9丢失。另外，注意被跟踪的物体能够通过将检测定位到车辆的可见部分来保持其形状和运动中心。例如，当轨道T7向前移动时，包围盒检测开始落在其可见的后部，轨道保持车辆的实际尺寸。这说明了在测量函数中模拟的偏移和收缩效应。

捕捉动作

动画表明，使用IMM滤波器有助于跟踪器在机动车辆上保持轨道。请注意，T4跟踪的车辆改变了自助式车辆后面的车道。在此机动事件期间，跟踪器能够在车辆上维护轨道。还注意到在显示中，它在恒定转向模型之后的概率，由CT表示，在车道变换机动期间增加。

联合概率数据关联

此动画显示，使用联合概率数据关联跟踪器有助于在不明确的情况下保持轨迹。此处，由T24和T62跟踪的车辆由于距离传感器较远，检测概率较低。请注意，跟踪器能够在其中一辆车发生故障时保持轨迹未检测到。在事件期间，轨道首先合并，这是JPDA中的已知现象，然后在再次检测到车辆时分离。

总结

此示例显示了如何使用带有IMM滤波器的JPDA跟踪器来使用LIDAR传感器跟踪对象。您了解到如何预处理原始点云以生成传统跟踪器的检测，该检测器假设每个传感器扫描每个对象的一个检测。您还学习了如何定义长方体模型来描述由JPDA跟踪器跟踪的扩展对象的运动学，尺寸和测量。此外，您还生成了算法中的C代码，并使用MATLAB仿真验证其执行结果。

金宝app支持文件

helperLidarModel

这个函数定义了激光雷达模型来模拟边界盒的收缩测量和中心点偏移。函数中使用helperCvmeasCuboid和helperCtmeasCuboid用于从状态获取边界框测量值的函数。

函数MEAS = Helperlidarmodel（POS，DIM，YAW）给定给定的值，该函数返回预期的边界框测量值%对象的位置、尺寸和偏航角。%版权归MathWorks公司所有。%得到x，y和z。x = pos (1:);: y = pos (2);Z = pos(3，:) - 2;%激光雷达安装在高度= 2米。%获取球形测量值。[AZ，〜，R] = Cart2Sph（x，y，z）;％缩小率s = 3/50;%50米处的径向长度为3米。sz = 2/50;% 2米高度在50米。%得到长度，宽度和高度。L =暗(1:);W =暗(2:);H =暗(3:);Az = Az - deg2rad(偏航);%沿径向收缩长度。Lshrink = min (L, abs (s * r。* (cos (az))));Ls = L - Lshrink;沿径向收缩宽度。Wshrink = min (W, abs (s * r。* (sin (az))));Ws = W - Wshrink;%收缩高度。Hshrink=min（H，sz*r）；Hs=H-Hshrink；％测量由MIN-MAX检测器给出，因此长度和宽度必须是%沿x和y方向投影。lmeas = ls。* cosd（偏航）+ ws。* sind（偏航）;wmeas = ls。* sind（偏航）+ ws。* cosd（偏航）;类似的移动是在x和y方向。shiftX=Lshrink.*cosd（偏航）+Wshrink.*sind（偏航）；shiftY=Lshrink.*sind（偏航）+Wshrink.*cosd（偏航）；shiftZ=Hshrink；%长方体原点偏移影响的建模x = x -符号(x).*shiftX/2;y = y -符号(y).*shiftY/2;z = z + shift /2 + 2;%测量格式多边环境协定=[x；y；z；多边环境协定；多边环境协定；Hs]；结束

helperInverseLidarModel

这个函数定义了逆激光雷达模型，使用激光雷达边界盒测量来初始化跟踪滤波器。函数中使用Helperinitimmfilter函数从边界框测量获得状态估计。

函数[pos、posCov昏暗,dimCov,偏航,yawCov] = helperInverseLidarModel(量、measCov)这个函数使用边界返回位置，尺寸，偏航%箱测量。%版权归MathWorks公司所有。%缩小率。s = 3/50;sz = 2/50;% x,y和z的测量x=meas（1，：）；y=meas（2，：）；z=meas（3，：）；[az，~，r]=cart2sph（x，y，z）；%移动x和y位置。Lshrink = abs (s * r。* (cos (az)));Wshrink = abs (s * r。* (sin (az)));Hshrink =深圳* r;shiftX = Lshrink;机智的= Wshrink;shiftZ = Hshrink;x = x +符号(x).*shiftX/2;y = y + (y).*shiftY/2;z = z +符号(z).*shiftZ/2;pos = [x, y, z]; posCov = measCov(1:3,1:3,:); yaw = zeros(1,numel(x),“喜欢”, x);yawCov = 1(1, 1,元素个数(x)“喜欢”, x);%尺寸初始化为标准乘用车的低% uncertainity。昏暗的= (4.7;1.8;1.4);眼睛dimCov = 0.01 * (3);结束

HelperboundingBoxDetector

这是支持类金宝appHelperboundingBoxDetector接受点云输入并返回点云列表的步骤ObjectDetection.

classdefHelperBoundingBoxDetector < matlab。系统％helperboundingboxdetector一个辅助课程分段点云%进入边界框检测。％对对象的步骤调用执行以下内容：％% 1。移除限制之外的点云。%2.从幸存的点云中，分割出地面% 3。从障碍点云，形成簇并放置边界每个群集上的％框。%裁剪属性性质场景的XLimitsXLimits=[-70]；%YLimits现场的YLimitsYLimits=[-6]；场景的ZLimitsZLimits = [-2 10];结束%地面分割属性性质% GroundMaxDistance点到地平面的最大距离地面达到0.3;%地参考矢量地平面的参考矢量grounddreferencevector = [0 0 1];%地面最大角度距离点到参考向量的最大角度距离GroundMaxAngularDistance = 5;结束％边界框分割属性性质%分割MinDistance用于分割的距离阈值分段MinDistance=1.6；% MinDetectionsPerCluster每个集群的最小检测数MinDetectionsPerCluster=2；% MaxZDistanceCluster集群的最大z坐标MaxZDistanceCluster=3；% MinZDistanceCluster集群的最小z坐标MinZDistanceCluster = 3;结束自我车辆半径移除自我车辆点云。性质%EgoVehicle EgoVehicle的半径EgoVehicleRadius = 3;结束性质%测量用于边界框检测的噪声测量噪声MeasurementNoise = blkdiag(眼睛(3)、眼睛(3));结束性质(Nontunable) MeasurementParameters = struct.empty(0,1);结束方法函数obj=HelperBoundingBoxDetector（varargin）setProperties（obj，nargin，varargin{：}）结束结束方法(访问=保护)函数[BboxDets，EpplationInces，GroundInces，CroppedIndices] = Stepimpl（OBJ，ChrentPointCloud，时间）%裁剪点云[pcSurvived, survivedIndices croppedIndices] = cropPointCloud (currentPointCloud、obj.XLimits obj.YLimits, obj.ZLimits, obj.EgoVehicleRadius);%删除地平面[pcObstacles, obstacleIndices groundIndices] = removeGroundPlane (pcSurvived、obj.GroundMaxDistance obj.GroundReferenceVector, obj.GroundMaxAngularDistance, survivedIndices);％形成群集并获取边界框detBBoxes = getBoundingBoxes (pcObstacles obj.SegmentationMinDistance、obj.MinDetectionsPerCluster obj.MaxZDistanceCluster, obj.MinZDistanceCluster);％组装检测如果isempty（对象测量参数）measParams={}；其他的测量参数=对象测量参数；结束bboxDets=组合检测（Detboxes，obj.MeasurementNoise，measParams，time）；结束结束结束函数检测= assembleDetections (bboxes measNoise measParams,时间)%此方法以objectDetection格式组装检测。numboxes = size（bboxes，2）;检测=单元格（Numboxes，1）;为i = 1:numBoxes detection {i} = objectDetection(time,cast(bboxes(:，i))，“双”)，...“MeasurementNoise”，双（moreNoise），'ObjectAttributes'、结构、...“MeasurementParameters”，米斯巴拉姆）；结束结束函数bboxes=GetBoundingBox（ptCloud、MindDistance、minDetsPerCluster、maxZDistance、minZDistance）%该方法在每个簇上使用一些基本的边界框%规则。%群集必须至少具有minDetsPerCluster点。％其平均值Z必须在MaxZdistance和Minzdistance之间。%长度，宽度和高度使用最小值和最大值计算%的维度。[labels，numClusters]=pcsegdist（ptCloud，minDistance）；pointData=ptCloud.Location；bboxes=nan（6，numClusters，“喜欢”, pointData);isValidCluster = false(1、numClusters);为i=1:numClusters thisPointData=pointData（labels==i，：）；平均点=平均值（此点数据，1）；如果尺寸（托盘Data，1）> MindetSpercluster &&...meanPoint(3) < maxZDistance && meanPoint(3) > minZDistance xMin = min(thisPointData(:，1));xMax = max (thisPointData (: 1));yMin = min (thisPointData (:, 2));yMax = max (thisPointData (:, 2));zMin = min (thisPointData (:, 3));zMax = max (thisPointData (:, 3));l = (xMax - xMin);w = (yMax - yMin);h = (zMax - zMin);x = (xMin + xMax)/2; y = (yMin + yMax)/2; z = (zMin + zMax)/2; bboxes(:,i) = [x y z l w h]'; isValidCluster(i) = l < 20;最大长度20米结束结束bboxes = bboxes (:, isValidCluster);结束函数[ptCloudOut，obstacleIndices，groundindex]=removeGroundPlane（ptCloudIn，maxGroundDist，referenceVector，maxAngularDist，currentindex）%此方法使用从点云中删除地平面% pcfitplane。[~, groundIndices,离群值]= pcfitplane (ptCloudIn、maxGroundDist referenceVector, maxAngularDist);ptCloudOut =选择(ptCloudIn、异常值);obstacleIndices = currentIndices(异常值);groundIndices = currentIndices (groundIndices);结束函数[ptCloudOut,指数,croppedIndices] = cropPointCloud (ptCloudIn、xLim yLim, zLim, egoVehicleRadius)%此方法选择限制内的点云并删除使用FindNeighborsInradius的％EGO车辆点云Locations = ptcloudin.location;Locations = Rehape（位置，[]，3）;Insidex =位置（：，1） XLIM（1）;Insidey =位置（:,2） ylim（1）;Ineinez =位置（:,3） zlim（1）;内部= Insidex＆Inney＆Ineinez;%移除自我车辆附近intindes = findneighborsinradius（ptcloudin，[0 0 0]，Egovehicleradius）;nodgoindices = true（ptcloudin.count，1）;nodgoindices（附近）= false;validindices =内部和非核果草坪;索引=查找（验证）;croppedindices = find（〜validindices）;ptcloudout = select（ptcloudin，索引）;结束

mexLidarTracker

此函数使用用于代码生成的功能接口实现点云预处理显示和跟踪算法。

函数[检测、障碍物、地面指数、切屑、，...confirmedTracks, modelProbs] = mexLidarTracker(ptCloudLocations,time)执着的detectorModel tracker Detectable Tracks输入电流numTracks如果Isempty（Detectormodel）||isempty（跟踪器）||isempty（detectabretacksinput）||isempty（currentnumtracks）%使用与MATLAB相同的起始种子在SIL中复制结果%的模拟。rng (2018);一个边界盒检测器模型。detectorModel=HelperBoundingBoxDetector(...“XLimits”,[-50 75],...%最小最大值“YLimits”,[-5 5],...%最小最大值“ZLimits”,[-2 5],...%最小最大值“SegmentationMinDistance”, 1.6,...百分之值距离最低“MinDetectionsPerCluster”，1，...每个集群最低积分%“MeasurementNoise”，眼睛（6），...检测报告中测量噪声百分比。“地面最大距离”, 0.3);接地点到接地面的最大距离assignmentGate=[50 100]；%分配阈值；Confthreshold = [7 10];%历史逻辑的确认阈值delThreshold = [8 10];%历史逻辑的删除阈值kc = 1e-5;%单位体积的虚警率filterinitfcn = @helperinitimmfilter;跟踪器= trackerjpda（'filterinitializationfcn'filterInitFcn,...“TrackLogic”那'历史'那...“AssignmentThreshold”，转让门，...“杂波密度”，Kc，...'确认察觉'confThreshold,...“DeletionThreshold”delThreshold,...“HasDetectableTrackIDsInput”符合事实的...“初始化阈值”,0,...“MaxNumTracks”, 30);detectableTracksInput = 0 (tracker.MaxNumTracks, 2);currentNumTracks = 0;结束ptCloud = pointCloud (ptCloudLocations);%探测器模型(检测、obstacleIndices groundIndices croppedIndices] = detectorModel (ptCloud、时间);％呼叫跟踪器[confirmedTracks，~，allTracks]=跟踪器（检测，时间，可检测的tracks输入（1:currentNumTracks，：）；％更新可检索性输入currentNumTracks =元素个数(allTracks);detectableTracksInput(1:currentNumTracks，:) = helperCalcDetectability(allTracks，[1 3 6]);%获取模型概率modelProbs = 0(2,元素个数(confirmedTracks));如果已锁定（跟踪器）为k=1:numel（confirmedTracks）c1=getTrackFilterProperties（跟踪器，confirmedTracks（k）.TrackID，“模型概率”）;聚合氯化铝c1 = {1};modelProbs (k) =聚合氯化铝(1);modelProbs (2 k) =聚合氯化铝(2);结束结束结束

HelpercalcDetecity

该函数计算每个轨道的检测概率。该函数用于为轨道生成“可检测的轨道SID”输入追踪器JPDA．

函数posIndices detectableTracksInput = helperCalcDetectability(跟踪)这是一个辅助函数，用于计算的检测概率%跟踪的激光雷达跟踪示例。将来可能会被移除%释放。%版权归MathWorks公司所有。%边界盒检测器分割点云的概率很低大于40米的距离。这个函数％使用状态依赖检测概率模型此效果%每个跟踪器。在达到最大范围后，Pd设置为高值，以%以更快的速度删除曲目。如果isempty（轨迹）可检测轨迹输入=零（0,2）；返回；结束rMax=75；兰比格=40；stateSize=numel（轨道（1）.State）；posSelector=0（3，stateSize）；posSelector（1，posindex（1））=1；posSelector（2，posindex（2））=1；占有者（3，posindex（3））=1；pos=getTrackPositions（轨迹、Possector）；如果coder.target (MATLAB的) trackid = [trackid . trackid];其他的轨迹ID=零（1，numel（轨迹），‘uint32’）;为i = 1:numel(tracks) trackid (i) = tracks(i).TrackID;结束结束(~, ~, r) = cart2sph (pos (: 1), pos (:, 2), pos (:, 3));probDetection = 0.9 *(元素个数(跟踪),1);probDetection(r > rAmbig) = 0.4;probDetection(r > rMax) = 0.99;detectableTracksInput = [double(trackIDs(:)) probDetection(:)];结束

loadLidarAndImageData

缝合激光雷达和相机数据，以便使用指定的初始和最终时间进行处理。

函数[lidarData,imageData] = loadlidaranddimagedata (datasetFolder,initTime,finalTime) initFrame = max(1,floor(initTime*10));lastFrame = min(350年,装天花板(finalTime * 10));负载(fullfile (datasetFolder'imagedata_35seconds.mat')，“allImageData”）;imagedata = Allimagedata（InitFrame：LastFrame）;numframes = lastframe  -  initframe + 1;lidardata = cell（numframes，1）;%每个文件包含70帧。initFileIndex=floor（initFrame/70）+1；lastFileIndex=ceil（lastFrame/70）；帧索引=[1:70:numFrames numFrames+1]；计数器=1；为i = initFileIndex:lastFileIndex startFrame = frameIndices(计数器);endFrame = frameIndices(counter + 1) - 1;负载(fullfile (datasetFolder, (“lidarData_”num2str(我)]),“currentLidarData”）;lidarData(startFrame:endFrame) = currentLidarData(1:(endFrame + 1 - startFrame));Counter = Counter + 1;结束结束