函数量= helperLidarModel (pos、昏暗的偏航)函数返回给定的边界框测量值物体的位置、尺寸和偏航角。版权所有2019 The MathWorks, Inc.%得到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 - H收缩;%测量是由最小-最大探测器给出的，因此长度和宽度必须%沿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 - sign(x).*shiftX/2;y = y - sign(y).*shiftY/2;z = z + shiftZ/2 + 2;%测量格式量= [x, y, z, Lmeas; Wmeas; Hs);结束

函数[pos、posCov昏暗,dimCov,偏航,yawCov] = helperInverseLidarModel(量、measCov)这个函数使用边界返回位置、尺寸和偏航度%箱测量。版权所有2019 The MathWorks, Inc.%缩小率。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 + sign(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);结束

classdefHelperBoundingBoxDetector < matlab。系统用于分割点云的助手类%转换为边界框检测。对对象的步骤调用做以下事情:％% 1。移除限制之外的点云。% 2。从幸存的点云中，可以看到地面% 3。从障碍点云，形成集群并放置边界%框。%裁剪属性属性场景的XLimitsXLimits = [-70 70];场景的ylimitYLimits = [-6 6];场景的限制ZLimits = [-2 10];结束%地面分割特性属性% GroundMaxDistance点到接地面的最大距离GroundMaxDistance = 0.3;% GroundReferenceVector接地平面参考向量GroundReferenceVector = [0 0 1];点到参考向量的最大角距离GroundMaxAngularDistance = 5;结束包围框分割属性属性分割距离阈值分割的距离SegmentationMinDistance = 1.6;MinDetectionsPerCluster每个集群的最小检测数MinDetectionsPerCluster = 2;% MaxZDistanceCluster集群最大z坐标MaxZDistanceCluster = 3;% MinZDistanceCluster集群最小z坐标MinZDistanceCluster = 3;结束%自我车辆半径移除自我车辆点云。属性% EgoVehicleRadius自我车辆的半径EgoVehicleRadius = 3;结束属性% MeasurementNoise包围盒检测的测量噪声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);%组装检测如果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检测{i} = objectDetection(time,cast(bboxes(:，i)，“双”)，.．.“MeasurementNoise”、双(measNoise),“ObjectAttributes”、结构、.．.“MeasurementParameters”, measParams);结束结束函数bboxes = getBoundingBoxes (ptCloud minDistance、minDetsPerCluster maxZDistance, minZDistance)此方法在每个集群上使用一些基本的%的规则。% Cluster必须至少有minDetsPerCluster点。它的平均z必须在maxZDistance和minZDistance之间。长、宽、高分别用最小值和最大值计算%的维度。[标签,numClusters] = pcsegdist (ptCloud minDistance);pointData = ptCloud.Location;numClusters bboxes =南(6日,“喜欢”, pointData);isValidCluster = false(1、numClusters);为i = 1:numClusters thisPointData = pointData(labels == i，:);meanPoint =意味着(thisPointData, 1);如果size(thisPointData,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 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 = locations(:，1) < xLim(2) & locations(:，1) > xLim(1);insideY = locations(:，2) < yLim(2) & locations(:，2) > yLim(1);insideZ = locations(:，3) < zLim(2) & locations(:，3) > zLim(1);inside = insideX & insideY & insideZ;%移除自我载具nearIndices = findNeighborsInRadius(ptCloudIn，[0 0 0]，egoVehicleRadius);nonEgoIndices = true (ptCloudIn.Count, 1);nonEgoIndices (nearIndices) = false;validindexes = inside & nonegoindexes;指数=找到(validIndices);croppedIndices =找到(~ validIndices);ptCloudOut =选择(ptCloudIn、指标);结束

函数(检测、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“SegmentationMinDistance”, 1.6,.．.%最小欧几里得距离“MinDetectionsPerCluster”, 1.．.每个集群的最小点数%“MeasurementNoise”,眼(6),.．.%检测报告中的测量噪声。“GroundMaxDistance”, 0.3);接地点到接地面的最大距离assignmentGate = [50 100];%赋值阈值;confThreshold = [7 10];%历史逻辑的确认阈值delThreshold = [8 10];%历史逻辑的删除阈值Kc = 1 e-5;单位体积误报率%filterInitFcn = @helperInitIMMFilter;追踪= trackerJPDA (“FilterInitializationFcn”filterInitFcn,.．.“TrackLogic”，“历史”，.．.“AssignmentThreshold”assignmentGate,.．.“ClutterDensity”Kc,.．.“ConfirmationThreshold”confThreshold,.．.“DeletionThreshold”delThreshold,.．.“HasDetectableTrackIDsInput”,真的,.．.“InitializationThreshold”,0，.．.“MaxNumTracks”, 30);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: numl (confirmedTracks) c1 = getTrackFilterProperties(tracker,confirmedTracks(k)。TrackID,“ModelProbabilities”）;聚合氯化铝c1 = {1};modelProbs (k) =聚合氯化铝(1);modelProbs (2 k) =聚合氯化铝(2);结束结束结束

函数posIndices detectableTracksInput = helperCalcDetectability(跟踪)这是一个计算检测概率的辅助函数%跟踪用于激光雷达跟踪示例。将来它可能会被移除%释放。版权所有2019 The MathWorks, Inc.包围盒探测器分割点云的概率很低%进入边界框的距离大于40米。这个函数%使用状态相关的检测概率来模拟这种效应每个追踪%。在达到最大范围后，将Pd设置为一个较高的值%启用删除轨道的速度更快。如果isempty(tracks) 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:numel(tracks) trackIDs(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(:)];结束