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

这个示例使用:

开放脚本

这个示例向您展示了如何使用安装在ego车辆顶部的激光雷达传感器进行测量来跟踪车辆。激光雷达传感器以点云的形式报告测量结果。该示例说明了在MATLAB®中处理点云和跟踪目标的工作流。有关Simu金宝applink®版本的示例，请参阅在Simulink中使用激光雷达数据跟踪车辆金宝app.本例中使用的激光雷达数据来自高速公路驾驶场景。在本例中，使用联合概率数据关联(JPDA)跟踪器和交互多模型(IMM)方法，使用记录的数据跟踪车辆。

3-D边界盒探测器模型

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

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

$x$ ， y美元和 z美元参考边界框的x、y和z位置， ${\theta}$ 指的是它的偏航角和 l美元， $w$ 和 $h$ 分别参考其长度、宽度和高度。这个pcfitcuboid(激光雷达工具箱)函数使用l形拟合算法确定包围框的偏航角。

检测器由一个支持类实现金宝appHelperBoundingBoxDetector，它围绕点云分割和聚类功能展开。此类的对象接受pointCloud输入并返回列表objectDetection具有边界框测量的对象。

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

激光雷达数据可在下列地点获得:https://ssd.mathworks.com/金宝appsupportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip

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

如果不可用，％加载数据。LIDAR数据存储为单元数组% pointCloud对象。如果~ (“lidarData”，'var') dataURL ='https://ssd.mathworks.com/金宝appsupportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip'；datasetfolder = fullfile（tempdir，“LidarExampleDataset”）;如果~exist（datasetFolder，“dir”)解压缩(dataURL datasetFolder);结尾%指定模拟的初始和最终时间。initTime = 0;finalTime = 35;[lidarData, imageData] = loadlidaranddimagedata (datasetFolder,initTime,finalTime); / /读取数据结尾%设置随机种子以产生可重复的结果。S = rng (2018);％一个边界盒检测器模型。detectorModel = HelperBoundingBoxDetector (.．.'xlimits',[-50 75],.．.% min-max'ylimits', 5 [5],.．.% min-max“ZLimits”,[-2 5],.．.% min-max'semonationmindistance',1.8,.．.最小欧几里得距离“MinDetectionsPerCluster”,1，.．.每簇的最小点％“测量噪音”眼睛,blkdiag(0.25 *(3), 25岁,眼(3)),.．.检测报告中测量噪声百分比“GroundMaxDistance”，0.3）;%接地点与地平面的最大距离

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

跟踪对象的第一步是定义它的状态，以及定义状态转换和相应度量的模型。这两组方程统称为目标的状态空间模型。为了使用激光雷达对车辆状态进行建模，本例使用长方体模型，约定如下:

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

$x_ {kin}$ 指控制运动中心运动学的那部分状态，和 $\theta$ 为偏航角。长方体的长度、宽度和高度被建模为常数，其估计在滤波器的校正阶段随时间变化。

在这个例子中，您使用了两个状态空间模型:一个恒定速度(cv)长方体模型和一个恒定转速(ct)长方体模型。这些模型的不同之处在于它们定义状态的运动部分的方式，如下所述:

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

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

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

的helperCvmeasCuboid和Helperctmeascuboid.测量模型描述了传感器如何分别感知恒定速度和恒定的转折状态，并且它们返回边界框测量。因为该状态包含有关目标大小的信息，所以测量模型包括中心点偏移和边界箱收缩的效果，因为传感器所感知，由于自动闭塞等效果[1]。这种效果由收缩因子建模，该收缩因子与距离跟踪车辆到传感器的距离成比例。

下图展示了在不同状态空间样本下运行的测量模型。注意当物体围绕ego车辆移动时，包围盒收缩和中心点偏移的模型效果。

设置跟踪器和可视化

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

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

联合概率数据关联跟踪器（trackerJPDA)，再加上IMM滤波器(trackingIMM)用于跟踪本例中的对象。IMM滤波器采用恒定速度和恒定转速模型，并使用支持函数进行初始化，金宝apphelperInitIMMFilter，包含在本示例中。IMM方法帮助赛道在运动模型之间切换，从而在机动或变道等事件中获得良好的估计精度。下面的动画显示了在IMM滤波器的预测阶段混合恒定速度和恒定转率模型的效果。

IMM滤波器更新每个模型的概率，当它被从目标的检测修正。下面的动画显示了在变道事件中车辆的估计轨迹以及每个模型的估计概率。

设置HasDetectableTrackIDsInput跟踪器的属性为符合事实的，这使您可以指定依赖的检测概率。轨道的检测概率由helperCalcDetectability函数，列在本示例的最后。

assignmentGate=[75 1000]；%赋值阈值;confThreshold = [7 10];历史逻辑的确认阈值delThreshold=[8 10]；%历史逻辑删除阈值Kc = 1 e-9;单位体积误报率%%IMM过滤器初始化函数filterInitFcn = @helperInitIMMFilter;％具有IMM滤波器的联合概率数据关联跟踪器追踪= trackerJPDA (“FilterInitializationFcn”filterInitFcn,.．.'tracklogic'，“历史”，.．.'AssignmentThreshold'assignmentGate,.．.“ClutterDensity”Kc,.．.“ConfirmationThreshold”confThreshold,.．.“DeletionThreshold”，德拉什德，.．.“HasDetectableTrackIDsInput”,真的,.．.“InitializationThreshold”0,.．.“HitMissThreshold”, 0.1);

可视化主要分为以下几类:

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

%创建显示displayObject = HelperLidarExampleDisplay (imageData {1},.．.'positionIndex'，[1 3 6]，.．.“VelocityIndex”(2 4 7),.．.“维度索引”,[9 10 11],.．.“YawIndex”8.．.“MovieName”，''，.．.%指定电影名称以录制电影。'唱片'、假);%指定true来记录新的gif

循环浏览数据

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

时间= 0;%开始时间dT = 0.1;%时间步长%启动所有轨道。allTracks=struct（[]）；%初始变量比较MATLAB和MEX仿真。numTracks = 0(元素个数(lidarData), 2);%遍历数据为了i = 1:元素个数(lidarData)％ 更新时间time = time + dT;％获取当前LIDAR扫描currentlidar = lidardata {i};%激光雷达扫描的发电机检测。(检测、obstacleIndices groundIndices croppedIndices] = detectorModel (currentLidar、时间);%计算每条轨道的可探测性。detectableTracksInput=helperCalcDetectability（所有轨道，[1 3 6]）；%通过检测跟踪。[confirmedTracks, tentativeTracks allTracks信息]=追踪(检测、时间detectableTracksInput);numTracks(1) =元素个数(confirmedTracks);%从IMM滤波器的每个航迹得到模型概率跟踪器的% getTrackFilterProperties函数。modelProbs = 0(2,元素个数(confirmedTracks));为了c = getTrackFilterProperties(tracker,confirmedTracks(k). log . log . log . log . log . log . log . log . log . log . log。TrackID,“ModelProbabilities”）;modelProbs c1 (:, k) = {1};结尾%更新显示如果isvalid (displayObject.PointCloudProcessingDisplay.ObstaclePlotter)获取当前图像扫描的参考图像currentImage = imageData {};%更新显示对象displayObject(检测、confirmedTracks currentLidar obstacleIndices,.．.地面索引、裁剪的骰子、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, 120140,“imm”，“细节”）;结尾

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

生成C代码

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

MATLAB编码器要求指定所有输入参数的属性。一种简单的方法是通过在命令行中使用arg游戏选项。有关更多信息，请参阅在命令行通过示例定义输入属性(MATLAB编码器)．请注意，顶级输入参数不能是对象处理类。因此，函数接受x，y和z点云的位置作为输入。从存储的点云中，可以使用位置财产pointCloud对象。这些信息也可以作为激光雷达传感器的原始数据直接获得。

％输入列表inputExample = {lidarData{1}。位置,0};%为MEX生成创建配置cfg = coder.config（'mex'）;%将cfg替换为以下文件生成静态库并执行% software-in-the-loop模拟。这需要嵌入式编码器许可证。％% CFG = code .config('lib');%静态库%cfg.VerificationMode='SIL'；%循环中的软件%如果文件不存在，则生成代码。如果~ (“mexLidarTracker_mex”，“文件”) h = msgbox({“生成代码。这可能需要几分钟……”；“完成后，此消息框将关闭。}，“Codegen消息”）;% -config允许指定代码生成配置％-o允许指定输出文件的名称codegen配置cfg-omexLidarTracker_mexmexLidarTrackerarg游戏输入示例关闭(h);别的清楚的mexLidarTracker_mex；结尾

代码生成成功。

用MEX代码重新运行模拟

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

时间= 0;为了i = 1：numel（lidardata）时间=时间+ dt;currentlidar = lidardata {i};[DetectionSmex，AppractIndicesMex，GroundIndicesMex，CroppedIndicesMex，.．.confirmedTracksMex，modelProbsMex]=mexLidarTracker_-mex（当前激光雷达位置、时间）；%记录数据，与MATLAB执行比较。numTracks(我,2)=元素个数(confirmedTracksMex);结尾

比较MATLAB和MEX执行的结果

disp（等质量（numtrack（：，1），numtrack（：，2））；

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

结果

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

跟踪维护

上面的动画显示时间= 3秒和时间之间的模拟= 16秒。请注意，在时间跨度期间，诸如T10和T6之类的曲目维护其ID和轨迹。然而，轨道T9丢失，因为通过传感器未遗漏（未检测到）履带式车辆。另外，请注意，跟踪物体能够通过将检测定位到车辆的可见部分上来维持其形状和运动中心。例如，当轨道T7向前移动时，边界箱检测开始落在其可见后部，并且轨道保持车辆的实际尺寸。这说明了在测量功能中建模的偏移和收缩效果。

捕获机动

动画显示，使用IMM滤波器有助于跟踪器在机动车辆上保持轨迹。注意，T4跟踪的车辆在ego车辆后面改变了车道。跟踪器能够在这个机动事件期间在车辆上保持一个轨迹。在显示中还可以注意到，在变道机动过程中，其遵循恒定转弯模型(以ct表示)的概率增加。

联合概率数据关联

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

总结

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

金宝app支持文件

helperLidarModel

此函数定义激光雷达模型，以模拟边界框测量的收缩和中心点偏移。此函数用于helperCvmeasCuboid和Helperctmeascuboid.用于从状态获取边界框测量值的函数。

作用量= helperLidarModel (pos、昏暗的偏航)给定给定的值，该函数返回预期的边界框测量值物体的位置、尺寸和偏航角。%版权归MathWorks公司所有。得到x,y和z。x=pos（1，：）；y=pos（2，：）；z=pos（3，：）-2；%激光雷达安装在高度= 2米。%得到球面测量。(阿兹,~,r) = cart2sph (x, y, z);%收缩速度s = 3/50;% 3米径向长度在50米。深圳= 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,深圳* r);Hs = H - Hshrink;类似的移动是在x和y方向。shift = Lshrink.*cosd(偏航)+ Wshrink.*sind(偏航);= Lshrink.*sind(偏航)+ Wshrink.*cosd(偏航);shiftZ = Hshrink;%长方体原点偏移影响的建模x = x -符号(x).*shiftX/2;y = y -符号(y).*shiftY/2;z = z + shift /2 + 2;%测量格式量= [x, y, z,偏航;Ls; Ws; Hs);结尾

helperInverseLidarModel

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

作用[pos、posCov昏暗,dimCov,偏航,yawCov] = helperInverseLidarModel(量、measCov)这个函数使用边界返回位置，尺寸，偏航%箱测量。%版权归MathWorks公司所有。%缩小率。s = 3/50;深圳= 2/50;％x，y和z测量x =量(1);: y =量(2);z =量(3);(阿兹,~,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 - shiftZ/2;pos = [x, y, z]; posCov = measCov(1:3,1:3,:); yaw = meas(4,:); yawCov = measCov(4,4,:);%尺寸初始化为标准乘用车的低％不确定性。DIM = [4.7; 1.8; 1.4];Dimcov = 0.01 *眼睛（3）;结尾

HelperBoundingBoxDetector

这是支持课程金宝appHelperBoundingBoxDetector接受点云输入并返回列表objectDetection

classdefHelperBoundingBoxDetector% HelperBoundingBoxDetector分割点云的helper类%进入边界框检测。对对象的step调用做以下事情:％% 1。移除限制之外的点云。% 2。从幸存的点云，分割出地面%3.从障碍点云开始，形成簇并放置边界%框。％裁剪属性属性场景的XLimitsXLimits = [-70 70];%YLimits现场的YLimitsYLimits = [-6 6];场景的ZLimitsZLimits = [-2 10];结尾%地面分割属性属性％地面达到接地平面的最大点的最大距离GroundMaxDistance = 0.3;% ground dreference vector地平面的参考向量GroundReferenceVector = [0 0 1];%地面最大角度距离点到参考向量的最大角度距离地面最大角度距离=5；结尾包围框分割属性属性% SegmentationMinDistance分割的距离阈值SegmentationMinDistance = 1.6;%MinDetectionsPerCluster每个群集的最小检测数MinDetectionsPerCluster = 2;% MaxZDistanceCluster集群的最大z坐标MaxZDistanceCluster = 3;％minzdistancebluster最小z坐标群组MinZDistanceCluster=-3；结尾%Ego车辆半径以移除Ego车辆点云。属性自我车辆半径自我车辆的半径埃戈拉迪乌斯=3；结尾属性% measumentnoise包围盒检测的测量噪声MeasurementNoise = blkdiag(眼(3)10眼(3));结尾属性（非无法）MeasurementParameters=struct.empty（0,1）；结尾方法作用obj = HelperBoundingBoxDetector(varargin) setProperties(obj,nargin,varargin{:})结尾结尾方法（访问=受保护）作用[bboxDets, obstacleIndices groundIndices croppedIndices] = stepImpl (obj, currentPointCloud,时间)%裁剪点云[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(obj.MeasurementParameters) measParams = {};别的measParams = obj.MeasurementParameters;结尾bboxDets = assembleDetections (detBBoxes, obj.MeasurementNoise、measParams、时间);结尾结尾结尾作用检测= assembleDetections (bboxes measNoise measParams,时间)％此方法在ObjectDetection格式中组装检测。numBoxes =大小(bboxes, 2);检测=细胞(numBoxes, 1);为了i = 1:numBoxes detection {i} = objectDetection(time,cast(bboxes(:，i))，'双倍的'），.．.“测量噪音”、双(measNoise),“ObjectAttributes”、结构、.．.'测量参数', measParams);结尾结尾作用bboxes=GetBoundingBox（ptCloud、MindDistance、minDetsPerCluster、maxZDistance、minZDistance）%该方法在每个簇上使用一些基本的边界框%的规则。%群集必须至少具有minDetsPerCluster点。%它的平均值z必须在maxZDistance和minZDistance之间。%长度，宽度和高度使用最小值和最大值计算%的维度。[标签,numClusters] = pcsegdist (ptCloud minDistance);pointData = ptCloud.Location;numClusters bboxes =南(7日,“喜欢”, pointData);isValidCluster = false(1、numClusters);为了i = 1:numClusters thisPointData = pointData(标签== i，:);meanPoint =意味着(thisPointData, 1);如果> minDetsPerCluster &&.．.meanPoint(3) < maxZDistance && meanPoint(3) > minZDistance cuboid = pcfitcuboid(pointCloud(thisPointData));偏航= cuboid.Orientation (3);L = cuboid.Dimensions (1);W = cuboid.Dimensions (2);H = cuboid.Dimensions (3);如果Abs (yaw) > 45可能= yaw + [-90;90];[~,选择]= min (abs(可能性));偏航=可能性(选择);temp = L;L = W;W = temp;结尾bboxes（：，i）= [cuboid.Center yaw l w h]'IsValidCluster（i）= L <20＆W <20;结尾结尾bboxes = bboxes (:, isValidCluster);结尾作用[ptCloudOut, obstacleIndices groundIndices] = removeGroundPlane (ptCloudIn、maxGroundDist referenceVector, maxAngularDist, currentIndices)%这个方法从点云中移除地平面% pcfitplane。[〜，groundindices，异常值] = pcfitplane（ptcloudin，maxgrounddist，CentrabVector，Maxangulardist）;ptcloudout = select（ptcloudin，异常值）;epplationInces = CurrentInce（异常值）;groundindices = pressindices（groundindices）;结尾作用[ptcloudout，indices，croppedIndices] = crocpointcloud（ptcloudin，xlim，ylim，zlim，Egovehicleradius）%此方法选择限制内的点云并删除%自我车辆点云使用findNeighborsInRadius位置= ptCloudIn.Location;位置=重塑(地点,[],3);insideX = locations(:，1) < xLim(2) & locations(:，1) > xLim(1);insideY =位置(:,2)< yLim(2)和地点(:,2)> yLim (1);insideZ = locations(: 3) < zLim(2) & locations(: 3) > zLim(1);内部= insideX & insideY & insideZ;％拆下自我车辆nearIndices = findNeighborsInRadius(ptCloudIn，[0 0 0]，egoVehicleRadius);nonEgoIndices = true (ptCloudIn.Count, 1);nonEgoIndices (nearIndices) = false;validIndices = inside & nonEgoIndices;指数=找到(validIndices);croppedIndices =找到(~ validIndices);ptCloudOut =选择(ptCloudIn、指标);结尾

mexLidarTracker

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

作用(检测、obstacleIndices groundIndices croppedIndices,.．.confirmedTracks, modelProbs] = mexLidarTracker(ptCloudLocations,time)持续的detectorModel跟踪器detectableTracksInput currentNumTracks如果isempty(detectorModel) || isempty(tracker) || isempty(detectableTracksInput) || isempty(currentNumTracks)%使用与MATLAB相同的起始种子在SIL中重现结果%的模拟。rng (2018);％一个边界盒检测器模型。detectorModel = HelperBoundingBoxDetector (.．.'xlimits',[-50 75],.．.% min-max'ylimits', 5 [5],.．.% min-max“ZLimits”,[-2 5],.．.% min-max'semonationmindistance',1.8,.．.最小欧几里得距离“MinDetectionsPerCluster”,1，.．.每簇的最小点％“测量噪音”眼睛,blkdiag(0.25 *(3), 25岁,眼(3)),.．.检测报告中的百分比测量噪声。“GroundMaxDistance”，0.3）;%接地点与地平面的最大距离assignmentGate=[75 1000]；%赋值阈值;confThreshold = [7 10];历史逻辑的确认阈值delThreshold=[8 10]；%历史逻辑删除阈值Kc = 1 e-9;单位体积误报率%filterInitFcn = @helperInitIMMFilter;追踪= trackerJPDA (“FilterInitializationFcn”filterInitFcn,.．.'tracklogic'，“历史”，.．.'AssignmentThreshold'assignmentGate,.．.“ClutterDensity”Kc,.．.“ConfirmationThreshold”confThreshold,.．.“DeletionThreshold”，德拉什德，.．.“HasDetectableTrackIDsInput”,真的,.．.“InitializationThreshold”0,.．.“MaxNumTracks”,30,.．.“HitMissThreshold”, 0.1);DetectaberetracksInput = Zeros（跟踪器），2）;currentnumtracks = 0;结尾ptcloud = pointcloud（ptcloudlocations）;%探测器模型(检测、obstacleIndices groundIndices croppedIndices] = detectorModel (ptCloud、时间);%的电话跟踪[confirmedTracks ~, allTracks] =追踪(检测、时间detectableTracksInput (1: currentNumTracks,:));%更新检测能力输入currentNumTracks=numel（所有轨道）；detectableTracksInput（1:currentNumTracks，：）=HelperCalcDetectable（所有轨道，[1 3 6]）；得到模型概率modelProbs = 0(2,元素个数(confirmedTracks));如果isLocked(跟踪)为了c = getTrackFilterProperties(tracker,confirmedTracks(k). log . log . log . log . log . log . log . log . log . log . log。TrackID,“ModelProbabilities”）;probs = c1 {1};ModelProbs（1，K）= probs（1）;ModelProbs（2，k）= probs（2）;结尾结尾结尾

helperCalcDetectability

该函数计算每个轨道的检测概率。该函数用于生成“detectabletracksid”输入trackerJPDA．

作用DetectableTracksInput = Helpercalcdetecty（轨道，Posindices）％这是计算检测概率的辅助功能%激光雷达跟踪示例的轨迹。它可能在将来被移除％ 释放。%版权归MathWorks公司所有。%边界盒检测器分割点云的概率很低％进入边界盒的距离大于40米。这个功能%使用状态依赖的检测概率来模拟这种效应每个追踪%。在最大范围后，Pd被设置为一个较高的值%允许以更快的速度删除轨道。如果isempty(tracks) detectableTracksInput = 0(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）；如果编码目标(MATLAB的)TrackID=[tracks.TrackID]；别的trackIDs = 0(1,元素个数(跟踪)‘uint32’）;为了i=1:numel（轨道）TrackID（i）=轨道（i）.TrackID；结尾结尾[~，~，r]=cart2sph（pos（：，1），pos（：，2），pos（：，3））；probDetection=0.9*one（numel（tracks），1）；probDetection（r>rAmbig）=0.4；probDetection（r>rMax）=0.99；detectableTracksInput=[double（trackId（：）probDetection（：）；结尾

loadlidarandimagedata.

缝合激光雷达和相机数据处理使用初始和最终时间规定。

作用[lidarData，imageData]=loadLidarAndImageData（datasetFolder，initTime，finalTime）initFrame=max（1，floor（initTime*10））；lastFrame=min（350，单元（最终时间*10））；加载（完整文件）（数据集文件夹，“imageData_35seconds.mat”），“allImageData”）;imageData = allImageData (initFrame: lastFrame);numFrames = lastFrame - initFrame + 1;lidarData =细胞(numFrames, 1);%每个文件包含70帧。initFileIndex=floor（initFrame/70）+1；lastFileIndex=ceil（lastFrame/70）；帧索引=[1:70:numFrames numFrames+1]；计数器=1；为了i=initFileIndex:lastFileIndex startFrame=frameindex（计数器）；endFrame=frameindex（计数器+1）-1；load（完整文件（数据集文件夹['lidardata_'，num2str（i）]），“currentLidarData”）;lidardata（Startframe：endframe）= CurrentLidardata（1 :( endFrame + 1  -  StartFrame））;计数器=计数器+ 1;结尾结尾

工具书类

[1] Arya Senna Abdul Rachman，Arya。“3D-LIDAR自主驾驶多目标跟踪：城市道路不确定性下的多目标检测和跟踪。”（2017）。