使用激光雷达跟踪车辆:从点云来跟踪列表

这个示例使用:

打开脚本

这个例子展示了如何使用测量跟踪车辆从激光雷达传感器安装在车辆的自我。激光雷达传感器报告测量的点云。MATLAB®中的示例演示了工作流处理点云与跟踪的对象。对于一个仿真万博1manbetx软件®版本的示例,请参考轨道车辆在仿真软件中使用激光雷达数据万博1manbetx在这个例子中,使用激光雷达数据记录从高速公路驾驶场景。在本例中,您使用记录数据跟踪车辆的联合概率数据关联(JPDA)跟踪器和一个交互多模型(IMM)的方法。

3 d边界框探测器模型

由于高分辨率激光雷达传感器的功能,每个传感器的扫描包含大量的点,一般称为点云。这个原始数据必须预处理提取感兴趣的对象,如汽车、自行车和行人。在本例中,您使用传统的分割算法使用基于距离的聚类算法。更多细节关于分割的地面激光雷达数据为对象,如飞机和障碍,请参考使用激光雷达地面飞机和障碍物检测(自动驾驶工具箱)的例子。深入学习细分工作流程,请参考使用激光雷达探测、分类和跟踪车辆(激光雷达工具箱)的例子。在这个例子中,点云属于障碍进一步分为集群使用pcsegdist功能,每个集群转化为一个边界框检测用以下格式:

$美元(x、y、z \{\θ}\ l \ w \ h]美元$

x美元 , y美元和 z美元指的是x - y z-positions边界框的, ${\θ}$ 是指其偏航角和 l美元 , w美元和 h美元指的是它的长度、宽度和高度,分别。的pcfitcuboid(激光雷达工具箱)函数使用l型的空间匹配算法来确定边界框的偏航角。

探测器由一个支持类实现万博1manbetxHelperBoundingBoxDetector包装在点云分割和集群功能。这个类接受一个的一个对象pointCloud输入并返回的列表objectDetection对象与边界框测量。

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

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

下载数据文件到临时目录中,其位置由MATLAB的指定tempdir函数。如果你想将文件放置到一个不同的文件夹,修改目录名称在随后的指令。

%如果无法加载数据。激光雷达数据存储单元阵列的% pointCloud对象。如果~ (“lidarData”,“var”)dataURL =“https://ssd.mathworks.com/万博1manbetxsupportfiles/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),…% min-max“YLimits”,5 [5],…% min-max“ZLimits”(2 - 5),…% min-max“SegmentationMinDistance”,1.8,…%最小欧式距离“MinDetectionsPerCluster”,1…%最低分集群“MeasurementNoise”眼睛,blkdiag(0.25 *(3), 25岁,眼(3)),…%测量噪音检测报告“GroundMaxDistance”,0.3);%从地平面最大距离地面点

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

跟踪一个物体的第一步是定义其状态,和定义的过渡状态的模型和相应的测量。这两组方程统称为目标状态空间模型。模型的车辆使用激光雷达跟踪,这个示例使用一个长方体模型与以下约定:

$$ x =[间{亲属}\{\θ}\ l \ w \ h]美元$

美元间{亲属}$ 指的是部分国家运动的运动学控制中心 $\θ美元$ 偏航角。长度、宽度和高度的长方体被建模为常数,其估计的发展及时调整阶段的过滤器。

在本例中,您使用两个状态方程模型:一个恒定的速度(cv)长方体模型和一个常数周转周期(ct)长方体模型。这些模型的定义不同运动状态的一部分,如下所述:

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

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

对他们的状态转换的信息,请参考helperConstvelCuboid和helperConstturnCuboid本例中使用的函数。

的helperCvmeasCuboid和helperCtmeasCuboid测量模型描述传感器感知的恒定速度和恒定的周转周期状态分别和他们返回边界框测量。因为国家包含目标的大小信息,测量模型包括中心偏移和边界框收缩的影响,感知的传感器,由于遮挡等影响[1]。这种效应建模的收缩率成正比的距离跟踪车辆的传感器。

下图演示了度量模型在不同状态样品操作。注意到建模的影响边界框收缩和中心偏移对象自我移动车辆。

建立跟踪和可视化

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

现在,设置中使用的跟踪和可视化的例子。

联合概率数据关联跟踪器(trackerJPDA)加上一个IMM滤波器(trackingIMM在这个例子中)是用来跟踪对象。IMM滤波器使用一个恒定的速度和常数周转周期模型和使用支持函数初始化,万博1manbetxhelperInitIMMFilter包含在这个例子。IMM方法帮助跟踪运动模式之间切换,从而达到良好的估计精度在操纵或车道改变之类的事件。下面的动画显示效果混合的恒定速度和恒定的周转周期模型在预测阶段的IMM滤波器。

每个模型的IMM滤波器更新概率修正时的检测对象。下面的动画显示了估计的轨道车辆在车道上更改事件和相应的估计的概率模型。

设置HasDetectableTrackIDsInput跟踪器的属性真正的,这使您能够指定依赖政府的检测概率。的探测概率跟踪计算的helperCalcDetectability函数,列出的这个例子。

assignmentGate = (75 - 1000);%赋值阈值;confThreshold = 10 [7];%确定阈值对历史逻辑delThreshold = 10 [8];%历史删除阈值逻辑Kc = 1 e-9;每单位体积%虚警率% IMM滤波器的初始化函数filterInitFcn = @helperInitIMMFilter;%的联合概率数据关联与IMM滤波器跟踪追踪= trackerJPDA (“FilterInitializationFcn”filterInitFcn,…“TrackLogic”,“历史”,…“AssignmentThreshold”assignmentGate,…“ClutterDensity”Kc,…“ConfirmationThreshold”confThreshold,…“DeletionThreshold”delThreshold,…“HasDetectableTrackIDsInput”,真的,…“InitializationThreshold”0,…“HitMissThreshold”,0.1);

可视化这些主要类别分为:

激光预处理和跟踪,这显示了原始点云,分割,和障碍。它也显示了生成的检测器的检测模型和生成的跟踪车辆的跟踪。
自我车辆显示——这显示显示了二维场景的鸟瞰图。它显示了障碍点云,边界框检测,生成的轨迹跟踪。供参考,它也显示了图像记录从一个相机安装在自我车辆和其视野。
跟踪细节——这显示了场景围绕着自我。它还显示了更好的跟踪细节,如误差协方差估计的位置每个跟踪及其运动模型概率,用简历和ct。

%创建显示displayObject = HelperLidarExampleDisplay (imageData {1},…“PositionIndex”(1 3 6),…“VelocityIndex”(2 4 7),…“DimensionIndex”(9 10 11),…“YawIndex”8…“MovieName”,”,…%指定一个电影的名字纪录电影。“RecordGIF”、假);%指定新gif真实记录

遍历数据

遍历记录激光雷达数据,生成检测从当前使用探测器模型点云,然后使用跟踪器的检测过程。

时间= 0;%开始时间dT = 0.1;%时间步%初始化所有的痕迹。allTracks =结构([]);%初始化变量比较MATLAB仿真和墨西哥人。numTracks = 0(元素个数(lidarData), 2);%循环数据为i = 1:元素个数(lidarData)%更新时间时间=时间+ dT;%得到当前激光雷达扫描currentLidar = lidarData {};%发电机从激光雷达扫描检测。(检测、obstacleIndices groundIndices croppedIndices] = detectorModel (currentLidar、时间);%计算每个跟踪的检测能力。detectableTracksInput = helperCalcDetectability (allTracks [1 3 6]);%通过检测跟踪。[confirmedTracks, tentativeTracks allTracks信息]=追踪(检测、时间detectableTracksInput);numTracks(1) =元素个数(confirmedTracks);%得到概率模型的IMM滤波器每个跟踪使用% getTrackFilterProperties追踪的功能。modelProbs = 0(2,元素个数(confirmedTracks));为k = 1:元素个数(confirmedTracks) c1 = getTrackFilterProperties(追踪,confirmedTracks .TrackID (k),“ModelProbabilities”);modelProbs c1 (:, k) = {1};结束%更新显示如果isvalid (displayObject.PointCloudProcessingDisplay.ObstaclePlotter)%得到当前图像扫描图片供参考currentImage = imageData {};%更新显示对象displayObject(检测、confirmedTracks currentLidar obstacleIndices,…groundIndices、croppedIndices currentImage modelProbs);结束%拍图时间= 18如果abs (- 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”,“细节”);结束

上图显示了三个显示时间= 18秒。轨道是由绿色边框。这个边界框检测由橙色的边界框。检测也有橙色点,代表点云分割障碍。所示的分段地紫色。裁剪或丢弃的点云所示蓝色。

生成C代码

你可以从MATLAB®代码生成C代码跟踪,并使用MATLAB编码器™预处理算法。C代码生成可以加速MATLAB代码仿真。生成C代码,必须重组算法MATLAB函数,可以编译成一个墨西哥人文件或一个共享库。为此,点云处理算法和跟踪算法被重组成一个MATLAB函数,mexLidarTracker。有些变量被定义为持续的保护他们的国家在多个调用函数(见持续的)。函数的输入和输出可观测到的函数描述“支持文件”一节中提供的这个例子。万博1manbetx

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

%输入列表inputExample = {lidarData {1}。位置,0};%为墨西哥人代创建配置cfg = coder.config (墨西哥人的);%用以下代码替换cfg生成静态库和执行% software-in-the-loop模拟。这需要嵌入式编码器许可证。%% cfg = coder.config(“自由”);%静态库% cfg。VerificationMode =“银”;% Software-in-the-loop如果文件不存在%生成代码。如果~ (“mexLidarTracker_mex”,“文件”h =对话框(){“生成代码。这可能需要几分钟……”;“这消息框完成后将关闭。”},“Codegen消息”);%配置允许指定codegen配置% - o允许指定输出文件的名称codegen配置cfg- omexLidarTracker_mexmexLidarTrackerarg游戏inputExample关闭(h);其他的清晰的mexLidarTracker_mex;结束

代码生成成功。

重新运行仿真与墨西哥人代码

重新运行仿真使用代码生成的墨西哥人,mexLidarTracker_mex。重置时间

时间= 0;为i = 1:元素个数=时间+ dT (lidarData)时间;currentLidar = lidarData {};[detectionsMex, obstacleIndicesMex groundIndicesMex croppedIndicesMex,…confirmedTracksMex modelProbsMex] = mexLidarTracker_mex (currentLidar.Location、时间);%的记录数据进行比较与MATLAB执行。numTracks(我,2)=元素个数(confirmedTracksMex);结束

MATLAB和墨西哥人执行之间的比较结果

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

注意确认跟踪的数量是相同的MATLAB和墨西哥人代码执行。这就确保了激光预处理和跟踪算法返回相同的结果生成的C代码与MATLAB代码。

结果

现在,分析不同的场景和事件的了解激光雷达测量模型的结合,联合概率数据关联和相互作用多模型滤波器,可以帮助实现一个好的评估车辆的跟踪。

跟踪维护

上面的动画显示之间的模拟时间= 3秒和时间= 16秒。注意跟踪如T10和T6维持他们的id和轨迹在时间跨度。然而,跟踪T9丢失,因为跟踪车辆错过(未检测到)很长一段时间的传感器。同时,注意跟踪对象是能够保持其形状和运动中心的定位检测到车辆的可见部分。例如,追踪T7推进,边界框检测开始落在其后方可见部分和跟踪维护车辆的实际大小。这说明了偏移量和收缩效应测量的建模功能。

捕捉动作

动画显示,使用一个IMM滤波器帮助追踪维护跟踪机动车辆。注意自我背后的车辆追踪的T4变更车道车辆。跟踪器可以保持在这机动车辆的跟踪事件。还要注意常数后显示,其概率的模型,用ct,增加在车道改变策略。

联合概率数据关联

这动画显示使用一个联合概率数据关联跟踪帮助在维护期间跟踪模棱两可的情况。在这里,车辆跟踪T43和T73低概率检测由于其巨大的距离传感器。注意,追踪期间能够保持跟踪事件的一个车辆未被检测到。在跟踪事件,第一次融合,这是一个已知的JPDA的现象,然后再一次一旦检测到车辆分开。

总结

这个例子展示了如何使用一个JPDA追踪与IMM滤波器使用激光雷达传感器来跟踪对象。您了解了如何将原始点云预处理为传统的追踪器,生成检测假设一个检测/对象/传感器扫描。您还了解了如何定义一个长方体模型来描述运动学,维度和测量扩展对象被跟踪的JPDA追踪。此外,您生成C代码的算法和MATLAB仿真验证其执行结果。

万博1manbetx支持文件

本节着重介绍一些重要的支持文件的代码中使用这个例子。万博1manbetx支持文件的完整列表可以在当前工作目录中找到万博1manbetx打开后MATLAB中的示例。

% * helperLidarModel *%%这个函数定义的激光雷达模型来模拟收缩%边界框测量和中心偏移量。这个函数使用%的| helperCvmeasCuboid |和| helperCtmeasCuboid |函数来获得%的边界框测量状态。%% <包括> helperLidarModel.m < /包括>%

helperInverseLidarModel

这个函数定义了逆激光雷达模型启动跟踪滤波器使用激光雷达测量边界框。这个函数是用于helperInitIMMFilter函数来获取状态估计测量边界框。

函数[pos、posCov昏暗,dimCov,偏航,yawCov] = helperInverseLidarModel(量、measCov)%这个函数返回的位置,尺寸,使用边界偏航%箱测量。% 2019年版权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)迹象。*机智的/ 2;z = z - shiftZ / 2;pos = [x, y, z]; posCov = measCov(1:3,1:3,:); yaw = meas(4,:); yawCov = measCov(4,4,:);%维度较低的初始化标准的轿车% uncertainity。昏暗的= (4.7;1.8;1.4);眼睛dimCov = 0.01 * (3);结束

HelperBoundingBoxDetector

这是支持类万博1manbetxHelperBoundingBoxDetector接受点云输入和返回的列表objectDetection

classdefHelperBoundingBoxDetector < matlab.System% HelperBoundingBoxDetector部分点云的助手类%到边界框检测。%的步骤调用对象做以下事情:%% 1。删除点云外的限制。% 2。幸存下来的点云,地面段% 3。从障碍点云,形成集群和边界在每个集群%的盒子。%裁剪属性属性% XLimits XLimits场景XLimits = (-70 - 70);% YLimits YLimits场景YLimits = (6 - 6);% ZLimits ZLimits fot现场ZLimits = 10 [2];结束%地面分割属性属性% GroundMaxDistance指向地平面的最大距离GroundMaxDistance = 0.3;% GroundReferenceVector参考矢量的地平面GroundReferenceVector = (0 0 1);% GroundMaxAngularDistance最大角距参考向量GroundMaxAngularDistance = 5;结束%边界框分割属性属性% SegmentationMinDistance距离阈值分割SegmentationMinDistance = 1.6;每个集群% MinDetectionsPerCluster最小数量的检测MinDetectionsPerCluster = 2;% MaxZDistanceCluster最大z坐标的集群MaxZDistanceCluster = 3;% MinZDistanceCluster最低z坐标的集群MinZDistanceCluster = 3;结束%自我车辆半径去除自我汽车点云。属性% EgoVehicleRadius半径的自我EgoVehicleRadius = 3;结束属性% MeasurementNoise边界框的测量噪声检测MeasurementNoise = blkdiag(眼(3)10眼(3));结束属性(Nontunable) MeasurementParameters = struct.empty (0,1);结束方法函数obj = HelperBoundingBoxDetector(变长度输入宗量)找(obj,输入参数个数,变长度输入宗量{:})结束结束方法(访问=保护)函数[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检测{我}= objectDetection(时间、投(bboxes(:,我)“双”),…“MeasurementNoise”、双(measNoise),“ObjectAttributes”、结构、…“MeasurementParameters”,measParams);结束结束函数bboxes = getBoundingBoxes (ptCloud minDistance、minDetsPerCluster maxZDistance, minZDistance)%这个方法适合边框与一些基本的每个集群%的规则。%集群必须至少minDetsPerCluster点。%的意思是z必须maxZDistance和minZDistance之间。%的长度,宽度和高度计算从每个使用min和max%的维度。[标签,numClusters] = pcsegdist (ptCloud minDistance);pointData = ptCloud.Location;numClusters bboxes =南(7日,“喜欢”,pointData);isValidCluster = false (1、numClusters);为i = 1: numClusters thisPointData = pointData(标签= =我:);meanPoint =意味着(thisPointData, 1);如果大小(thisPointData 1) > minDetsPerCluster & &…meanPoint (3) < maxZDistance & & meanPoint (3) > minZDistance长方体= pcfitcuboid (pointCloud (thisPointData));偏航= cuboid.Orientation (3);L = cuboid.Dimensions (1);W = cuboid.Dimensions (2);H = cuboid.Dimensions (3);如果abs(偏航)可疑> 45 =偏航+ (-90;90);[~,选择]= min (abs(可能性));偏航=可能性(选择);temp = L;L = W;W = temp;结束bboxes (:, i) =(长方体。中心偏航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 referenceVector, maxAngularDist);ptCloudOut =选择(ptCloudIn、异常值);obstacleIndices = currentIndices(异常值);groundIndices = currentIndices (groundIndices);结束函数[ptCloudOut,指数,croppedIndices] = cropPointCloud (ptCloudIn、xLim yLim, zLim, egoVehicleRadius)%这个方法选择限制和消除了内点云使用findNeighborsInRadius %自我汽车点云位置= ptCloudIn.Location;位置=重塑(地点,[],3);insideX =位置(:1)< xLim(2)和地点(:1)> xLim (1);insideY =位置(:,2)< yLim(2)和地点(:,2)> yLim (1);insideZ =位置(:,3)< zLim(2)和地点(:,3)> zLim (1);在= insideX & insideY & insideZ;%去除自我车辆nearIndices = findNeighborsInRadius ([0 0 0], ptCloudIn egoVehicleRadius);nonEgoIndices = true (ptCloudIn.Count, 1);nonEgoIndices (nearIndices) = false;validIndices = & nonEgoIndices内;指数=找到(validIndices);croppedIndices =找到(~ validIndices);ptCloudOut =选择(ptCloudIn、指标);结束

mexLidarTracker

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

函数(检测、obstacleIndices groundIndices croppedIndices,…confirmedTracks modelProbs] = mexLidarTracker (ptCloudLocations、时间)持续的detectorModel追踪detectableTracksInput currentNumTracks如果isempty (detectorModel) | | isempty(跟踪)| | isempty (detectableTracksInput) | | isempty (currentNumTracks)%开始使用相同的种子繁殖导致SIL的MATLAB%的模拟。rng (2018);%一个边界框探测器模型。detectorModel = HelperBoundingBoxDetector (…“XLimits”(-50 75),…% min-max“YLimits”,5 [5],…% min-max“ZLimits”(2 - 5),…% min-max“SegmentationMinDistance”,1.8,…%最小欧式距离“MinDetectionsPerCluster”,1…%最低分集群“MeasurementNoise”眼睛,blkdiag(0.25 *(3), 25岁,眼(3)),…%测量噪音检测报告。“GroundMaxDistance”,0.3);%从地平面最大距离地面点assignmentGate = (75 - 1000);%赋值阈值;confThreshold = 10 [7];%确定阈值对历史逻辑delThreshold = 10 [8];%历史删除阈值逻辑Kc = 1 e-9;每单位体积%虚警率filterInitFcn = @helperInitIMMFilter;追踪= trackerJPDA (“FilterInitializationFcn”filterInitFcn,…“TrackLogic”,“历史”,…“AssignmentThreshold”assignmentGate,…“ClutterDensity”Kc,…“ConfirmationThreshold”confThreshold,…“DeletionThreshold”delThreshold,…“HasDetectableTrackIDsInput”,真的,…“InitializationThreshold”0,…“MaxNumTracks”30岁的…“HitMissThreshold”,0.1);detectableTracksInput = 0 (tracker.MaxNumTracks, 2);currentNumTracks = 0;结束ptCloud = pointCloud (ptCloudLocations);%探测器模型(检测、obstacleIndices groundIndices croppedIndices] = detectorModel (ptCloud、时间);%的电话跟踪[confirmedTracks ~, allTracks] =追踪(检测、时间detectableTracksInput (1: currentNumTracks,:));%更新检测能力的输入currentNumTracks =元素个数(allTracks);detectableTracksInput (1: currentNumTracks,:) = helperCalcDetectability (allTracks [1 3 6]);%得到模型概率modelProbs = 0(2,元素个数(confirmedTracks));如果isLocked(跟踪)为k = 1:元素个数(confirmedTracks) c1 = getTrackFilterProperties(追踪,confirmedTracks .TrackID (k),“ModelProbabilities”);聚合氯化铝c1 = {1};modelProbs (k) =聚合氯化铝(1);modelProbs (2 k) =聚合氯化铝(2);结束结束结束

helperCalcDetectability

的概率函数计算每个跟踪检测。这个函数是用于生成“DetectableTracksIDs”输入的trackerJPDA。

函数posIndices detectableTracksInput = helperCalcDetectability(跟踪)%,这是一个helper函数计算的检测概率%跟踪激光雷达跟踪的例子。它可能在未来被移除%释放。% 2019年版权MathWorks公司。%的边界框探测器低概率分割点云%到边框的距离大于40米。这个函数%模型使用依赖政府的检测概率这一效应每个追踪%。在最大射程之后,Pd设置为高价值%启用删除跟踪速度。如果isempty(跟踪)detectableTracksInput = 0 (0, 2);返回;结束征求= 75;rAmbig = 40;stateSize =元素个数(跟踪(1).State);stateSize posSelector = 0(3日);posSelector (posIndices (1)) = 1;posSelector (posIndices (2)) = 1;posSelector (posIndices (3)) = 1;pos = getTrackPositions(跟踪、posSelector);如果coder.target (MATLAB的)trackIDs = [tracks.TrackID];其他的trackIDs = 0(1,元素个数(跟踪)“uint32”);为i = 1:元素个数(跟踪)trackIDs (i) =跟踪(i) .TrackID;结束结束(~,~,r) = cart2sph (pos (: 1), pos (:, 2), pos (:, 3));probDetection = 0.9 *(元素个数(跟踪),1);probDetection (r > rAmbig) = 0.4;probDetection (r >征求)= 0.99;detectableTracksInput =[双(trackIDs (:)) probDetection (:));结束

loadLidarAndImageData

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

函数[lidarData, imageData] = loadLidarAndImageData (datasetFolder、initTime finalTime) initFrame = max(1楼(initTime * 10));lastFrame = min(350年,装天花板(finalTime * 10));负载(fullfile (datasetFolder“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);counter = 1;为我= initFileIndex: lastFileIndex startFrame = frameIndices(柜台);端架= frameIndices(计数器+ 1)- 1;负载(fullfile (datasetFolder, (“lidarData_”num2str(我)]),“currentLidarData”);lidarData (startFrame:端架)= currentLidarData(1:(端架+ 1 - startFrame));counter = counter + 1;结束结束

引用

[1]塞纳阿卜杜勒·拉赫曼,Arya。“3 d-lidar多对象跟踪自主驾驶:城市道路下多目标检测和跟踪不确定性。”(2017).