使用激光雷达跟踪车辆：从点云到跟踪列表

此示例使用：

开放脚本

本示例演示如何使用安装在ego车辆顶部的激光雷达传感器的测量值跟踪车辆。激光雷达传感器以点云的形式报告测量结果。该示例演示了MATLAB®中用于处理点云和跟踪对象的工作流。有关该示例的Simulink®版本，请参阅万博1manbetx在Simulink中使用激光雷达数据跟踪车辆万博1manbetx（传感器融合和跟踪工具箱）。本示例中使用的激光雷达数据是从公路驾驶场景中记录的。在本例中，您使用记录的数据通过联合概率数据关联（JPDA）跟踪器和交互多模型（IMM）方法跟踪车辆。

三维边界盒检测器模型

由于激光雷达传感器的高分辨率能力，传感器的每次扫描都包含大量点，通常称为点云。必须对这些原始数据进行预处理，以提取感兴趣的对象，例如汽车、骑自行车的人和行人。在本例中，您将使用基于距离的聚类算法的经典分割算法。有关将激光雷达数据分割为地平面和障碍物等对象的更多详细信息，请参阅用激光雷达探测地面和障碍物的例子。关于深度学习分割工作流，请参阅使用激光雷达检测、分类和跟踪车辆（激光雷达工具箱）实例在本例中，属于障碍物的点云使用pcsegdist函数，每个簇将转换为具有以下格式的边界框检测：

$[x\y\z\{\theta}\l\w\h]$

x美元 , $y$ 和 $z$ 请参阅边界框的x、y和z位置， ${\theta}$ 指其偏航角和 1美元$ , w美元和 $h$ 分别指其长度、宽度和高度。这个长方体（激光雷达工具箱）函数使用L形拟合算法确定边界框的偏航角。

检测器由一个支持类实现万博1manbetxHelperBoundingBoxDetector，它围绕点云分割和聚类功能展开。此类的对象接受点云输入并返回一个目标检测具有边界框测量值的对象。

该图显示了边界盒探测器模型和激光雷达工具箱中涉及的过程™ 用于实现每个流程的函数。它还显示了控制每个进程的支持类的属性。万博1manbetx

激光雷达数据可在以下位置获取：https://ssd.mathworks.com/万博1manbetxsupportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip

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

%加载数据（如果不可用）。激光雷达数据存储为%点云对象。如果~存在(“利达达达”,“var”)数据URL=“https://ssd.mathworks.com/万博1manbetxsupportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip”; datasetFolder=fullfile（tempdir，“LidarExampleDataset”）;如果~存在(datasetFolder“dir”)解压（dataURL，datasetFolder）；终止%指定模拟的初始和最终时间。初始时间=0；最终时间=35；[lidarData，imageData]=loadLidarAndImageData（datasetFolder，initTime，finalTime）；终止%设置随机种子以生成可重复的结果。S=rng（2018年）；%边界盒检测器模型。detectorModel=HelperBoundingBoxDetector(...“XLimits”(-50 75),...%最小最大值“YLimits”,[-5 5],...%最小最大值“ZLimits”,[-2 5],...%最小最大值“分段思维距离”, 1.8,...%最小欧几里得距离“MinDetectionsPerCluster”1....%每个群集的最小点数“MeasurementNoise”，blkdiag（0.25*眼（3），25，眼（3）），...%检测报告中的测量噪声“地面最大距离”,0.3);接地点到接地面的最大距离

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

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

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

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

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

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

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

有关其状态转换的信息，请参阅辅助长方体和HelperConstTurn长方体本例中使用的函数。

这个helperCvmeasCuboid和helperCtmeasCuboid测量模型描述传感器如何分别感知恒定速度和恒定转弯率状态，并返回边界框测量值。由于状态包含有关目标大小的信息，测量模型包括传感器感知的中心点偏移和边界框收缩的影响，即d使用自遮挡等效果[1]。此效果由收缩系数建模，收缩系数与履带车辆到传感器的距离成正比。

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

设置跟踪器和可视化

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

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

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

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

设定HasDetectableTrackIDsInput跟踪器的属性为真正的，它使您能够指定与状态相关的检测概率。轨迹的检测概率由辅助可检测性函数，在本例末尾列出。

assignmentGate = [75 1000];%分配阈值；confThreshold=[7 10]；%历史逻辑的确认阈值delThreshold = [8 10];%历史逻辑的删除阈值Kc=1e-9；%单位体积的虚警率% IMM过滤器初始化功能filterInitFcn = @helperInitIMMFilter;%基于IMM滤波器的联合概率数据关联跟踪器跟踪器=跟踪器JPDA(“FilterInitializationFcn”，filterInitFcn，...“跟踪逻辑”,“历史”,...“AssignmentThreshold”，转让门，...“杂波密度”，Kc，...“确认阈值”，阈值，...“删除阈值”，delThreshold，...“HasDetectableTrackIDsInput”符合事实的...“初始化阈值”,0,...“HitMissThreshold”,0.1);

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

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

%创建显示displayObject=HelperLidarExampleDisplay（图像数据{1}），...“位置索引”,[1 3 6],...“速度指数”,[2 4 7],...“DimensionIndex”(9 10 11),...“YawIndex”8....“MovieName”,'',...%指定要录制电影的电影名称。“RecordGIF”，假）；%指定true以记录新的GIF

循环浏览数据

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

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

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

生成C代码

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

MATLAB编码器要求指定所有输入参数的属性。的示例在命令行中定义输入属性是一种简单的方法-args选项。有关详细信息，请参阅在命令行中通过示例定义输入特性（MATLAB编码器）。请注意，顶级输入参数不能是的对象手柄班因此，函数接受x,Y和Z作为输入的点云的位置。从存储的点云中，可以使用地方财产点云对象该信息也可作为激光雷达传感器的原始数据直接获取。

%输入列表inputExample={lidarData{1}.Location，0}；为MEX生成创建配置cfg=coder.config(“墨西哥”）;%将cfg替换为以下内容以生成静态库并执行%软件在环仿真。这需要嵌入式编码器许可证。%%cfg=coder.config（'lib'）；%静态库%cfg.VerificationMode='SIL'；%循环中的软件%如果文件不存在，则生成代码。如果~存在(“mexLidarTracker_mex”,“文件”)h=msgbox({'生成代码。这可能需要几分钟……”;'完成后，此消息框将关闭。'},“Codegen消息”）;%-config允许指定codegen配置%-o允许指定输出文件的名称编码基因-配置cfg- omexLidarTracker\U mexmexLidarTracker-argsinputExample关闭（h）；其他的清楚的mexLidarTracker\U mex;终止

代码生成成功。

使用MEX代码重新运行模拟

使用生成的MEX代码重新运行模拟，mexLidarTracker\U mex.重置时间

时间=0；对于i=1:numel（lidarData）time=time+dT；currentLidar=lidarData{i}；[detectionsMex，obstacleIndicesMex，groundIndicatesMex，CroppedDicesMex，...confirmedTracksMex，modelProbsMex]=mexLidarTracker_-mex（当前激光雷达位置、时间）；%记录数据，以便与MATLAB执行进行比较。numTracks（i，2）=numel（confirmedTracksMex）；终止

比较MATLAB与MEX执行的结果

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

1.

请注意，对于MATLAB和MEX代码执行，确认的轨迹数是相同的。这确保了激光雷达预处理和跟踪算法使用生成的C代码返回与MATLAB代码相同的结果。

后果

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

轨道维护

上面的动画显示了时间=3秒和时间=16秒之间的模拟。请注意，轨迹（如T10和T6）在时间跨度内保持其ID和轨迹。但是，轨迹T9丢失，因为被跟踪车辆丢失（未检测到）在很长一段时间内，通过传感器。此外，请注意，通过将检测定位到车辆的可见部分，被跟踪对象能够保持其形状和运动中心。例如，当轨道T7向前移动时，边界框检测开始落在其可见后部，轨道保持车辆的实际大小这说明了测量函数中模拟的偏移和收缩效应。

捕捉动作

动画显示，使用IMM过滤器有助于跟踪器在机动车辆上保持轨迹。请注意，T4跟踪的车辆在ego车辆后面改变车道。跟踪器能够在此机动事件期间在车辆上保持轨迹。还请注意，在显示屏中，跟踪器遵循恒定转弯模式的概率l、由ct表示，在换道机动过程中增加。

联合概率数据关联

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

总结

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

万博1manbetx支持文件

helperLidarModel

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

函数meas=辅助LIDARMODEL（位置、尺寸、偏航）%此函数返回给定值的预期边界框测量值%对象的位置、尺寸和偏航角。%版权归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米。深圳= 2/50;%50米处有2米高。%得到长度，宽度和高度。L=dim（1，：）；W=dim（2，：）；H=dim（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；%x和y方向也有类似的偏移。shiftX=Lshrink.*cosd（偏航）+Wshrink.*sind（偏航）；shiftY=Lshrink.*sind（偏航）+Wshrink.*cosd（偏航）；shiftZ=Hshrink；%长方体原点偏移影响的建模x=x-符号（x）。*shiftX/2；y=y-符号（y）。*移位/2；z=z+shiftZ/2+2；%测量格式meas=[x；y；z；偏航；Ls；Ws；Hs]；终止

helperInverseLidarModel

此函数定义反向激光雷达模型，以使用激光雷达边界盒测量启动跟踪滤波器。此函数用于HelperInItemFilter函数从边界框度量中获取状态估计。

函数[pos、posCov、dim、dimCov、偏航、yawCov]=辅助线模型（meas、measCov）%此函数使用边界返回位置、尺寸和偏航%盒子测量。%版权归MathWorks公司所有。%收缩率。s=3/50；sz=2/50；%测量的x、y和zx=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=sz*r；shiftX=Lshrink；shiftY=Wshrink；shiftZ=Hshrink；x=x+sign（x）。*shiftX/2；y=z-shiftZ/2；pos=[x；y；z]；posCov=measCov（1:3,1:3，：；偏航=meas（4，：）；雅科夫=measCov（4,4，：；%尺寸是为低噪音标准乘用车初始化的%不确定性。dim=[4.7；1.8；1.4]；dimCov=0.01*眼（3）；终止

HelperBoundingBoxDetector

这是辅助班万博1manbetxHelperBoundingBoxDetector接受点云输入并返回点云列表的步骤目标检测

classdefHelperBoundingBoxDetector < matlab。系统%HelperBoundingBoxDetector用于分割点云的辅助类%进入边界框检测。%对对象的step调用执行以下操作：%% 1. 删除超出限制的点云。%2.从幸存的点云中，分割出地面% 3。从障碍点云，形成簇并放置边界%每个集群上的框。%种植特性性质%XLimits场景的XLimitsXLimits=[-70]；%YLimits现场的YLimitsYLimits=[-6]；%现场的限制ZLimits=[-210]；终止%地面分割特性性质%地面最大距离点到地平面的最大距离地面最大距离=0.3；%地参考矢量地平面的参考矢量GroundReferenceVector=[01]；%地面最大角度距离点到参考向量的最大角度距离GroundMaxAngularDistance = 5;终止%边界框分割属性性质%分割MinDistance用于分割的距离阈值分段MinDistance=1.6；% MinDetectionsPerCluster每个集群的最小检测数MinDetectionsPerCluster=2；%MaxZDistanceCluster簇的最大Z坐标MaxZDistanceCluster=3；%最小距离簇的最小Z坐标MinZDistanceCluster = 3;终止自我车辆半径移除自我车辆点云。性质%EgoVehicle EgoVehicle的半径EgoVehicleRadius = 3;终止性质%测量用于边界框检测的噪声测量噪声测量噪声=blkdiag（眼睛（3），10，眼睛（3））；终止性质(Nontunable) MeasurementParameters = struct.empty(0,1);终止方法函数obj=HelperBoundingBoxDetector（varargin）setProperties（obj，nargin，varargin{：}）终止终止方法(访问=保护)函数[bboxDets，obstacleIndices，groundindex，croppedinces]=stepImpl（对象，当前点云，时间）切点云[pcSurvived，SurvivedDices，CroppedDices]=CroppedCloud（当前点云，对象XLimits，对象YLimits，对象ZLimits，对象eladius）；%删除地平面[P障碍物、障碍物、地面索引]=移除圆平面（P障碍物、obj地面最大距离、obj地面参考向量、obj地面最大角度距离、生存碎片）；%形成簇并获取边界框DetbBox=GetBoundingBox（Pc障碍物、对象分段MindDistance、对象MindDetectionsPerCluster、对象MaxZDistanceCluster、对象MinZDistanceCluster）；%集合检测如果isempty（对象测量参数）measParams={}；其他的测量参数=对象测量参数；终止bboxDets=组合检测（Detboxes，obj.MeasurementNoise，measParams，time）；终止终止终止函数检测=组合检测（B盒、测量值、测量值、时间）%此方法以objectDetection格式组合检测。numBoxes=大小（bboxes，2）；检测=单元（numBoxes，1）；对于i=1:numBoxes detections{i}=objectDetection（时间，强制转换）（b框（：，i），“双人”),...“MeasurementNoise”，双人（米），“对象属性”，结构，...“测量参数”，米斯巴拉姆）；终止终止函数bboxes=GetBoundingBox（ptCloud、MindDistance、minDetsPerCluster、maxZDistance、minZDistance）%该方法在每个簇上使用一些基本的边界框%规则。% Cluster必须至少有minDetsPerCluster点。%其平均z必须介于maxZDistance和minZDistance之间。%长度、宽度和高度均使用每个值的最小值和最大值进行计算%维度。[labels，numcluster]=pcsegdist（ptCloud，minDistance）；pointData=ptCloud.Location；bboxes=nan（7，numcluster，“喜欢”，点数据）；isValidCluster=false（1，numClusters）；对于i=1:numClusters thisPointData=pointData（labels==i，：）；meanPoint=mean（thisPointData，1）；如果大小（thisPointData，1）>minDetsPerCluster&&...平均点（3）minZDistance长方体=pcfit长方体（点云（thisPointData））；偏航=长方体。方向（3）；L=长方体。尺寸（1）；W=长方体。尺寸（2）；H=长方体。尺寸（3）；如果abs（偏航）>45个可能性=偏航+[-90；90]；[~，toChoose]=min（abs（可能））；偏航=可能（选择）；温度=L；L=W；W=温度；终止b长方体（：，i）=[长方体中心偏航L W H]'；isValidCluster（i）=L<20&W<20；终止终止bboxes=bboxes（：，isValidCluster）；终止函数[ptCloudOut，obstacleIndices，groundindex]=removeGroundPlane（ptCloudIn，maxGroundDist，referenceVector，maxAngularDist，currentindex）%此方法使用从点云中删除地平面%飞机。[~，地面索引，异常值]=pFitPlane（ptCloudIn，maxGroundDist，referenceVector，maxAngularDist）；ptCloudOut=select（ptCloudIn，异常值）；obstacleIndices=currentIndex（异常值）；地面索引=currentIndex（地面索引）；终止函数[ptCloudOut，index，crappedices]=crappointcloud（ptCloudIn，xLim，yLim，zLim，eladius）%此方法选择限制内的点云并删除%使用findNeighborsInRadius的ego车辆点云地点=ptCloudIn.Location；地点=重塑（地点，[]，3）；insideX=地点（：，1）xLim（1）；insideY=地点（：，2）yLim（1）；insideZ=地点（：，3）zLim（1）；insideX=insideX&insideY&insideZ；%拆下ego车辆NearIndexes=findNeighborsInRadius（ptCloudIn，[0 0]，egoVehicleRadius）；nonEgoIndices=true（ptCloudIn.Count，1）；nonEgoIndices（NearIndexes）=false；validIndices=inside&nonEgoIndices；Indexes=find（validIndices）；CroppedDices=find（~validIndices）；ptCloudOut=select（ptCloudIn，Indexes）；终止

mexLidarTracker

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

函数[检测、障碍物、地面指数、切屑、，...confirmedTracks，modelProbs]=mexLidarTracker（ptCloudLocations，time）持久的detectorModel tracker Detectable Tracks输入电流numTracks如果isempty（detectorModel）| | isempty（tracker）| | isempty（detectableTracksInput）| | isempty（currentNumTracks）%使用与MATLAB相同的起始种子在SIL中复制结果%模拟。rng（2018）；%边界盒检测器模型。detectorModel=HelperBoundingBoxDetector(...“XLimits”(-50 75),...%最小最大值“YLimits”,[-5 5],...%最小最大值“ZLimits”,[-2 5],...%最小最大值“分段思维距离”, 1.8,...%最小欧几里得距离“MinDetectionsPerCluster”1....%每个群集的最小点数“MeasurementNoise”，blkdiag（0.25*眼（3），25，眼（3）），...%检测报告中的测量噪声。“地面最大距离”,0.3);接地点到接地面的最大距离assignmentGate = [75 1000];%分配阈值；confThreshold=[7 10]；%历史逻辑的确认阈值delThreshold = [8 10];%历史逻辑的删除阈值Kc=1e-9；%单位体积的虚警率filterInitFcn=@helperInitIMMFilter；tracker=trackerJPDA(“FilterInitializationFcn”，filterInitFcn，...“跟踪逻辑”,“历史”,...“AssignmentThreshold”，转让门，...“杂波密度”，Kc，...“确认阈值”，阈值，...“删除阈值”，delThreshold，...“HasDetectableTrackIDsInput”符合事实的...“初始化阈值”,0,...“MaxNumTracks”30岁的...“HitMissThreshold”，0.1）；可检测的TracksInput=0（tracker.MaxNumTracks，2）；currentNumTracks=0；终止ptCloud=点云（ptCloudLocations）；%探测器模型[detections，obstacleIndices，groundindex，crappedDices]=detectorModel（ptCloud，time）；%呼叫跟踪器[confirmedTracks，~，allTracks]=跟踪器（检测，时间，可检测的tracks输入（1:currentNumTracks，：）；%更新可检测性输入currentNumTracks =元素个数(allTracks);detectableTracksInput(1:currentNumTracks，:) = helperCalcDetectability(allTracks，[1 3 6]);%获取模型概率modelProbs=零（2，numel（确认跟踪））；如果已锁定（跟踪器）对于k=1:numel（confirmedTracks）c1=getTrackFilterProperties（跟踪器，confirmedTracks（k）.TrackID，“模型概率”); probs=c1{1}；modelProbs（1，k）=probs（1）；modelProbs（2，k）=probs（2）；终止终止终止

辅助可检测性

该函数计算每个轨迹的检测概率。此函数用于为系统生成“DetectableTracksID”输入追踪器JPDA.

函数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, (“利达达达”，num2str（i）]，“currentLidarData”)；lidarData（开始帧：结束帧）=当前lidarData（1：（结束帧+1-开始帧））；计数器=计数器+1；终止终止