轨道车辆使用雷达：从点云曲目

这个示例使用:

打开脚本

这个例子向您展示了如何使用安装在ego车辆顶部的激光雷达传感器来跟踪车辆。激光雷达传感器以点云的形式报告测量结果。该实例说明了MATLAB®中处理点云和跟踪目标的工作流程。有关示例的S金宝appimulink®版本，请参阅在Simulink中使用激光雷达数据跟踪车辆金宝app(传感器融合与跟踪工具箱)。该lidar data used in this example is recorded from a highway driving scenario. In this example, you use the recorded data to track vehicles with a joint probabilistic data association (JPDA) tracker and an interacting multiple model (IMM) approach.

3-d包围盒探测器型号

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

$[X \ Y \Ž\升\ W \ H]$

x美元， y美元和 z美元指的是X，Y和边框的Z位置和 $ L $ ， w美元和 $ H $ 指其长度，宽度，和高度，分别。

边界框是通过使用在每个维度上的点的坐标的最小值和最大值配合到每个簇。该检测器由支撑类实现金宝appHelperBoundingBoxDetector，其环绕的点云分割和聚类功能。这个类的对象接受点云输入并返回一个列表objectDetection与边框的测量对象。

该图示出了所涉及的包围盒检测器模型和计算机视觉工具箱™用于实现各处理功能的处理。它还表明，控制每个过程支撑类的属性。金宝app

％负载的数据，如果不可用。激光雷达数据被存储为的单元阵列% pointCloud对象。如果~ ('lidarData'，“变种”）%指定模拟的初始和最终时间。initTime = 0;finalTime = 35;[lidarData，为imageData] = loadLidarAndImageData（initTime，finalTime）;结束％设置随机数种子产生重复的结果。S = RNG（2018）;％A边界框检测器模型。detectorModel = HelperBoundingBoxDetector (...“XLimits”(-50 75),...% min-max'YLimits',5 [5],...% min-max“ZLimits”，[ -  2 5]，...% min-max'SegmentationMinDistance',1.6,...％最小欧几里得距离“MinDetectionsPerCluster”,1...每簇％的最低点“MeasurementNoise”,眼(6),...检测报告中测量噪声%“GroundMaxDistance”，0.3）;地面点到地面平面的最大距离

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

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

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

${X_亲属}$ 为控制运动中心运动学的状态部分 $\θ美元$ 是偏航角。的长度，宽度，长方体的高度被建模为一个常量，其估计在过滤器的校正阶段演变的时间。

在本例中，您使用了两个状态空间模型:恒速(cv)长方体模型和恒转率(ct)长方体模型。这些模型在定义状态的运动学部分的方式上有所不同，如下所述:

$X_ {CV} = [X \ {\点{X}} \ Y \ {\点{Y}} \Ž\ {\点{Z}} \ {\ THETA} \升\ W \ H]$

$X_ {克拉} = [X \ {\点{X}} \ Y \ {\点{Y}} \ {\点{\ THETA}} \Ž\ {\点{Z}} \ {\ THETA}\升\ W \ H]$

有关他们的状态变化信息，请参阅helperConstvelCuboid和helperConstturnCuboid本例中使用的函数。

该helperCvmeasCuboid和helperCtmeasCuboid测量模型描述所述传感器感知到恒定速度和恒定导率如何分别状态和它们返回边界框的测量。因为状态包含有关所述目标的大小的信息，所述测量模型包括中心点偏移和边界框的收缩的效果，如由传感器感知，由于像自遮挡[1]的效果。这种效果是由正比于从所述履带车辆到传感器的距离的收缩率进行建模。

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

建立跟踪和可视化

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

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

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

IMM过滤器更新每一个模型的概率，当它被纠正，从对象的检测。下面的动画展示了车辆在变道事件中估计的轨迹，以及相应的每个模型估计的概率。

设置HasDetectableTrackIDsInput跟踪器的属性为真正的，这使您可以指定检测的状态依赖概率。一磁迹的检测概率是由计算helperCalcDetectability功能，列出在本实施例中的端部。

assignmentGate = [50 100];%赋值阈值;confThreshold = [7 10];历史逻辑的确认阈值阈值= [8 10];历史逻辑的删除阈值KC = 1E-5;每单位体积的误报率%% IMM过滤器初始化功能filterInitFcn = @helperInitIMMFilter;与IMM滤波器的联合概率数据关联跟踪器跟踪= trackerJPDA（'FilterInitializationFcn'filterInitFcn,...'TrackLogic'，'历史'，...'AssignmentThreshold'assignmentGate,...“ClutterDensity”Kc,...'ConfirmationThreshold'，confThreshold，...'DeletionThreshold'，delThreshold，...'HasDetectableTrackIDsInput',真的,...“InitializationThreshold”,0);

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

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

％创建显示器displayObject的= HelperLidarExampleDisplay（的imageData {1}，...'PositionIndex'[1 3 6]...“VelocityIndex”(2 4 7),...“DimensionIndex”(9 10 11),...“YawIndex”8，...“MovieName”，“”，...指定要录制电影的电影名称。“RecordGIF”、假);%指定为true以记录新gif

环通数据

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

时间= 0;％ 开始时间dT = 0.1;%时间步启动所有轨道。allTracks =结构（[]）;%初始变量，用于比较MATLAB和MEX仿真。numTracks =零（numel（lidarData），2）;循环遍历数据对于i = 1:元素个数(lidarData)％ 更新时间时间=时间+的dT;％获取当前激光雷达扫描currentLidar = lidarData {I};从激光雷达扫描中检测到的信号。[检测，obstacleIndices，groundIndices，croppedIndices] = detectorModel（currentLidar，时间）;计算每条轨迹的可检测性。detectableTracksInput = helpercalcdetecability (allTracks，[1 3 6]);%通过检测跟踪。[confirmedTracks, tentativeTracks allTracks] =追踪(检测、时间detectableTracksInput);numTracks(1) =元素个数(confirmedTracks);%从每个磁道的IMM滤波器中得到模型概率％getTrackFilterProperties功能跟踪的。modelProbs =零（2，numel（confirmedTracks））;对于k = 1:numel(confirmedTracks) c1 = getTrackFilterProperties(tracker,confirmedTracks(k).TrackID，“ModelProbabilities”);modelProbs c1 (:, k) = {1};结束％更新显示如果的isValid（displayObject.PointCloudProcessingDisplay.ObstaclePlotter）％获取参考图像当前图像扫描currentImage = imageData {};更新显示对象displayObject(检测、confirmedTracks currentLidar obstacleIndices,...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，ÿ和ž点云的位置作为输入。可以从存储的点云中提取这些信息位置财产点云对象。这些信息也可以作为激光雷达传感器的原始数据直接获得。

％输入列表inputExample = {lidarData {1} .Location，0};为MEX生成创建配置CFG = coder.config（'MEX');用以下代码替换cfg以生成静态库并执行％软件在环仿真。这需要嵌入式编码许可证。％% cfg = code .config('lib');%静态库％cfg.VerificationMode = 'SIL';％软件在中环％生成的代码，如果文件不存在。如果~ ('mexLidarTracker_mex'，“文件”）H = MSGBOX（{“生成代码。这可能需要几分钟……”;“完成后此消息框将关闭。”}，“代码生成消息”);％-config允许指定代码生成配置％-o允许指定输出文件的名称codegen配置CFG- omexLidarTracker_mexmexLidarTracker-argsinputExample关闭(h);其他明确mexLidarTracker_mex;结束

使用MEX代码重新运行模拟

使用所生成的代码MEX重新运行该仿真，mexLidarTracker_mex。

%复位时间时间= 0;对于I = 1：numel（lidarData）时间=时间+的dT;currentLidar = lidarData {I};[detectionsMex，obstacleIndicesMex，groundIndicesMex，croppedIndicesMex，...confirmedTracksMex，modelProbsMex] = mexLidarTracker_mex（currentLidar.Location，时间）;%记录数据，以便与MATLAB执行比较。numTracks(我,2)=元素个数(confirmedTracksMex);结束

比较MATLAB和MEX执行的结果

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

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

结果

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

养路

以上示出了动画时间= 3秒，时间= 16秒之间的模拟。请注意，曲目如T10和T6的时间跨度中保持自己的ID和轨迹。然而，跟踪T9被丢失，因为履带车辆被错过（未检测到），用于通过所述传感器的时间长。此外，请注意，跟踪对象是能够通过检测定位到车辆的可见部分，以保持其形状和运动中心。例如，如轨道T7向前移动时，包围盒检测开始落在其可见后部和轨道维护车辆的实际尺寸。这示出了在测量功能模型化的偏移量和收缩的效果。

捕捉动作

动画显示，使用IMM滤波器有助于跟踪保持对机动车辆的轨道。注意，通过T4跟踪的车辆改变自主车辆后方车道。跟踪器能够操纵此事件过程中保持对车辆的轨道。在显示跟在恒定转模式的概率，由CT表示还会注意到，车道变换操作期间增加。

联合概率数据关联

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

摘要

此实施例表明如何使用JPDA跟踪器与IMM滤波器来跟踪使用激光雷达传感器对象。你学到了原始点云可以如何进行预处理以产生用于常规跟踪器，即假定每个对象每个传感器扫描一个检测检测。你也学习了如何定义一个长方体模型来描述运动，尺寸和扩展对象的测量由JPDA跟踪器跟踪。此外，还从算法生成的C代码，并与MATLAB仿真验证了其执行结果。

金宝app支持文件

helperLidarModel

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

函数MEAS = helperLidarModel（POS，暗淡，偏航）％这个函数返回预期的边框测量给出的对象的位置、尺寸和偏航角。％版权所有2019 MathWorks公司得到x y z。x = pos (1:);:y = pos (2);z = pos(3，:) - 2;％激光雷达安装在高度= 2米。%得到球形测量。[AZ，〜，R] = cart2sph（X，Y，Z）;％收缩率S = 3/50;% 3米径向长度在50米。SZ = 2/50;％2米的高度在50米。得到长度，宽度和高度。L =暗淡（1，:);W =暗淡（2，:);H =暗淡（3，:);AZ = AZ  -  deg2rad（偏航）;沿径向收缩长度%。Lshrink =分钟（L，ABS（S * R *（COS（AZ））））;LS = L  -  Lshrink;％沿径向方向收缩的宽度。Wshrink = min (W, abs (s * r。* (sin (az))));W - Wshrink;%下降高度。Hshrink = min (H,深圳* r);Hs = H - Hshrink;％测量由最小 - 最大定检测器因此长度和宽度必须%沿x和y投影。Lmeas = LS * COSD（偏航）+ ws *信德（偏航）。;Wmeas = LS *信德（偏航）+ ws * COSD（偏航）。;％类似的转变是x和y方向。shift = Lshrink.*cosd(yaw) + Wshrink.*sind(yaw);*sind(yaw) + Wshrink.*cosd(yaw);shiftZ = Hshrink;％造型盒产地的影响抵消x = x -符号(x).*shiftX/2;y = y -符号(y).*shiftY/2;z = z + shiftZ/2 + 2;%测量格式MEAS = [X; Y; Z; Lmeas; Wmeas; HS];结束

helperInverseLidarModel

此功能定义了激光雷达逆模型来发起使用激光雷达边界框测量跟踪滤波器。函数中使用此函数helperInitIMMFilter函数，以获得从一个边界框测量状态的估计。

函数[POS，posCov，暗淡，dimCov，偏航，yawCov] = helperInverseLidarModel（MEAS，measCov）％该函数返回的位置，尺寸，偏航使用边界％盒测量。％版权所有2019 MathWorks公司％收缩率。S = 3/50;SZ = 2/50;％的x，y和测量žx =量(1);:y =量(2);z =量(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;腥= Wshrink;shiftZ = Hshrink;X = X +符号（x）的* shiftX / 2。Y = Y +符号（Y）*腥/ 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);尺寸初始化为标准乘用车的尺寸％不确定度。暗淡= [4.7; 1.8; 1.4]。dimCov = 0.01 *眼（3）;结束

HelperBoundingBoxDetector

这是支持类金宝appHelperBoundingBoxDetector要接受点云输入并返回一个列表objectDetection

classdefHelperBoundingBoxDetector < matlab.System％HelperBoundingBoxDetector甲助手类来分割点云%进入边界框检测。％步骤调用对象做以下的事情：％的限制之外％1中移除了点云。% 2。从残存的点云中，分割出地面% 3。从障碍点云开始，形成集群并放置边界每个群集上％盒。％裁剪性能属性％XLimits XLimits的场景XLimits = [- 70,70];％YLimits YLimits的场景YLimits = [-6];限制，限制场景的限制ZLimits = [-2 10];结束％地分割属性属性点的％GroundMaxDistance最大距离到接地平面GroundMaxDistance = 0.3;%接地参考矢量GroundReferenceVector = [0 0 1];点的％GroundMaxAngularDistance最大角距离到参考矢量GroundMaxAngularDistance = 5;结束％边界框分割特性属性距离阈值分割SegmentationMinDistance = 1.6;每个簇的最小检测数MinDetectionsPerCluster = 2;集群的最大z坐标MaxZDistanceCluster = 3;％簇的MinZDistanceCluster最小z坐标MinZDistanceCluster = 3;结束%自我车辆半径，以消除自我车辆点云。属性自我车辆的半径EgoVehicleRadius = 3;结束属性测量噪声测量边界盒检测的噪声MeasurementNoise = blkdiag（眼（3），眼（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）;％装配检测如果= {};其他measParams = obj.MeasurementParameters;结束bboxDets = assembleDetections (detBBoxes, obj.MeasurementNoise、measParams、时间);结束结束结束函数检测= assembleDetections（bboxes，measNoise，measParams，时间）％该方法组装在objectDetection格式检测。numBoxes =尺寸（bboxes，2）;检测=细胞（numBoxes，1）;对于I = 1：numBoxes检测{I} = objectDetection（时间，铸造（bboxes（：，i）中，“双”）...“MeasurementNoise”，双（measNoise），'ObjectAttributes'，结构，...'MeasurementParameters',measParams);结束结束函数bboxes = getBoundingBoxes（ptCloud，minDistance才会，minDetsPerCluster，maxZDistance，minZDistance）％这种方法适合边框的每个集群上有一些基本的%的规则。群集必须具有至少minDetsPerCluster点。％，其平均ž必须maxZDistance和minZDistance之间。％的长度，宽度和高度是使用最小和最大从每个计算出的％ 尺寸。[标签，numClusters] = pcsegdist（ptCloud，minDistance才会）;pointData = ptCloud.Location;bboxes =楠（6，numClusters，“喜欢”，pointData）;isValidCluster =假（1，numClusters）;对于thisPointData = pointData(标签== i，:);meanPoint =意味着(thisPointData, 1);如果大小（thisPointData，1）> minDetsPerCluster &&...meanPoint（3） minZDistance XMIN =分钟（thisPointData（：，1））;XMAX = MAX（thisPointData（：，1））;YMIN =分钟（thisPointData（：，2））;YMAX = MAX（thisPointData（：，2））;ZMIN =分钟（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 groundIndices] = removeGroundPlane (ptCloudIn、maxGroundDist referenceVector, maxAngularDist, currentIndices)此方法使用从点云中移除地面平面％pcfitplane。[〜，groundIndices，离群] = pcfitplane（ptCloudIn，maxGroundDist，referenceVector，maxAngularDist）;ptCloudOut =选择（ptCloudIn，离群）;obstacleIndices = currentIndices（离群值）;groundIndices = currentIndices（groundIndices）;结束函数[ptCloudOut,指数,croppedIndices] = cropPointCloud (ptCloudIn、xLim yLim, zLim, egoVehicleRadius)％该方法选择的限度内的点群，并删除使用findNeighborsInRadius％自主车辆的点云位置= ptCloudIn.Location;位置=重塑（地点，[]，3）;insideX =位置（：，1） XLIM（1）;insideY =位置（：，2） yLim（1）;insideZ =位置（：，3） zLim（1）;内部= insideX＆insideY＆insideZ;％删除自车辆nearIndices = findNeighborsInRadius（ptCloudIn，[0 0 0]，egoVehicleRadius）;nonEgoIndices =真（ptCloudIn.Count，1）;nonEgoIndices（nearIndices）= FALSE;validIndices =里面＆nonEgoIndices;指数=找到（validIndices）;croppedIndices =找到（〜validIndices）;ptCloudOut =选择（ptCloudIn，索引）;结束

mexLidarTracker

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

函数(检测、obstacleIndices groundIndices croppedIndices,...confirmedTracks，modelProbs] = mexLidarTracker（ptCloudLocations，时间）一贯detectorModel跟踪器如果的isEmpty（detectorModel）||的isEmpty（跟踪器）||的isEmpty（detectableTracksInput）||的isEmpty（currentNumTracks）%使用与MATLAB相同的启动种子在SIL中重现结果％模拟。rng (2018);％A边界框检测器模型。detectorModel = HelperBoundingBoxDetector (...“XLimits”(-50 75),...% min-max'YLimits',5 [5],...% min-max“ZLimits”，[ -  2 5]，...% min-max'SegmentationMinDistance',1.6,...％最小欧几里得距离“MinDetectionsPerCluster”,1...每簇％的最低点“MeasurementNoise”,眼(6),...在检测报告％的测量噪声。“GroundMaxDistance”，0.3）;地面点到地面平面的最大距离assignmentGate = [50 100];%赋值阈值;confThreshold = [7 10];历史逻辑的确认阈值阈值= [8 10];历史逻辑的删除阈值KC = 1E-5;每单位体积的误报率%filterInitFcn = @helperInitIMMFilter;跟踪= trackerJPDA（'FilterInitializationFcn'filterInitFcn,...'TrackLogic'，'历史'，...'AssignmentThreshold'assignmentGate,...“ClutterDensity”Kc,...'ConfirmationThreshold'，confThreshold，...'DeletionThreshold'，delThreshold，...'HasDetectableTrackIDsInput',真的,...“InitializationThreshold”0,...“MaxNumTracks”，30）;detectableTracksInput =零（tracker.MaxNumTracks，2）;currentNumTracks = 0;结束ptCloud =点云（ptCloudLocations）;%探测器模型[检测，obstacleIndices，groundIndices，croppedIndices] = detectorModel（ptCloud，时间）;％呼叫跟踪器[confirmedTracks ~, allTracks] =追踪(检测、时间detectableTracksInput (1: currentNumTracks,:));％更新可检测输入currentNumTracks =元素个数(allTracks);detectableTracksInput(1:currentNumTracks，:) = helpercalcdetecability (allTracks，[1 3 6]);%得到模型概率modelProbs =零（2，numel（confirmedTracks））;如果isLocked(跟踪)对于k = 1:numel(confirmedTracks) c1 = getTrackFilterProperties(tracker,confirmedTracks(k).TrackID，“ModelProbabilities”);probs = C1 {1};modelProbs（1，K）= probs（1）;modelProbs（2，K）= probs（2）;结束结束结束

helperCalcDetectability

该函数计算每个轨道检测的概率。该功能用于生成用于所述“DetectableTracksIDs”输入trackerJPDA。

函数detectableTracksInput = helperCalcDetectability（轨道，posIndices）％这是一个辅助函数来计算的检测概率%跟踪激光雷达跟踪的例子。它可能在未来被移除％ 发布。％版权所有2019 MathWorks公司％边框检测器具有分割点云的概率低％到边界框的距离超过40米以上。此功能％模型中使用的检测为一个状态依赖概率这种效果每个追踪%。在最大范围之后，Pd被设置为一个高值to允许以更快的速度删除轨道。如果isempty(tracks) detectableTracksInput = 0 (0,2);返回;结束RMAX = 75;rAmbig = 40;stateSize = numel（轨道（1）.STATE）;posSelector =零（3，stateSize）;posSelector（1，posIndices（1））= 1;posSelector（2，posIndices（2））= 1;posSelector（3，posIndices（3））= 1;POS = getTrackPositions（轨道，posSelector）;如果coder.target ('MATLAB') trackIDs = [tracks.TrackID];其他trackIDs = 0(1,元素个数(跟踪)'UINT32');对于i = 1:numel(tracks) trackIDs(i) = tracks(i).TrackID;结束结束(~,~,r) = cart2sph (pos (: 1), pos (:, 2), pos (:, 3));probDetection = 0.9 *(元素个数(跟踪),1);概率检测(r > rAmbig) = 0.4;概率检测(r > rMax) = 0.99;detectableTracksInput =[双(trackIDs(:)) probDetection(:)];结束

loadLidarAndImageData

使用指定的初始和最终时间对激光雷达和摄像机数据进行处理。

函数[lidarData,imageData] = loadLidarAndImageData(initTime,finalTime) initFrame = max(1,floor(initTime*10));lastFrame = min(350年,装天花板(finalTime * 10));负载('imageData_35seconds.mat'，'allImageData');的imageData = allImageData（initFrame：lastFrame）;numFrames = lastFrame  -  initFrame + 1;lidarData =细胞（numFrames，1）;每个文件包含70帧。initFileIndex =地板（initFrame / 70）+ 1;lastFileIndex =小区（lastFrame / 70）;frameIndices = [1：70：numFrames numFrames + 1];计数器= 1;对于i = initFileIndex:lastFileIndex startFrame = frameIndices(计数器);帧索引(计数器+ 1)- 1;负载(['lidarData_'，num2str（I）]，'currentLidarData');lidarData（startFrame将：endFrame）= currentLidarData（1：（endFrame + 1  -  startFrame将））;计数器=计数+ 1;结束结束

参考

[1]的Arya塞纳阿卜杜勒拉赫曼，艾莉亚。“3D激光雷达多目标跟踪的自主驾驶：根据城市道路的不确定性多目标检测与跟踪。”（2017年）。