函数meas = helperLidarModel(pos,dim，偏航)此函数返回给定值的期望边界框测量值对象的位置，尺寸和偏航角。The MathWorks, Inc.版权所有%得到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;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 - sign(x).*shiftX/2;y = y - sign(y).*shiftY/2;z = z + shiftZ/2 + 2;测量格式meas = [x;y;z;偏航;Ls;Ws;Hs];结束

函数[pos,posCov,dim,dimCov,yaw,yawCov] = helperInverseLidarModel(meas,measCov)这个函数返回使用边界的位置，维度，偏航包装盒尺寸。The MathWorks, Inc.版权所有%收缩率。S = 3/50;Sz = 2/50;% x,y和z的测量X = 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 +符号(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,:);%尺寸初始化为一个标准乘用车low% uncertainity。Dim = [4.7;1.8;1.4];dimCov = 0.01*眼(3);结束

classdefHelperBoundingBoxDetector < matlab。系统用于分割点云的辅助类%进入包围框检测。对对象的步骤调用做以下事情:％% 1。移除限制之外的点云。% 2。从幸存的点云，分割出地面% 3。从障碍物点云，形成簇并进行边界%框。裁剪特性%属性XLimits场景的XLimitsXLimits = [-70 70];场景的YLimitsYLimits = [-6 6];场景的ZLimitsZLimits = [-2 10];结束%地面分割属性属性% GroundMaxDistance点到接地面的最大距离GroundMaxDistance = 0.3;% GroundReferenceVector地平面参考向量grounddreferencevector = [0 0 1];% GroundMaxAngularDistance点到参考向量的最大角距离GroundMaxAngularDistance = 5;结束边框分割属性属性SegmentationMinDistance分割距离阈值SegmentationMinDistance = 1.6;% MinDetectionsPerCluster每个集群的最小检测数MinDetectionsPerCluster = 2;% MaxZDistanceCluster集群最大z坐标MaxZDistanceCluster = 3;% MinZDistanceCluster集群最小z坐标MinZDistanceCluster = -3;结束%自我战车半径移除自我战车点云。属性% EgoVehicleRadius自我车辆半径EgoVehicleRadius = 3;结束属性% measuentnoise包围盒检测的测量噪声测量噪声= blkdiag(眼睛(3)，10，眼睛(3));结束属性(不可调)测量参数= struct.empty(0,1);结束方法函数obj = HelperBoundingBoxDetector(varargin) setProperties(obj,nargin,varargin{:})结束结束方法(访问=受保护)函数[bboxDets,obstacleIndices,groundIndices,croppedIndices] = stepImpl(obj,currentPointCloud,time)%作物点云[pc, vedindices,croppedIndices] = cropPointCloud(currentPointCloud,obj.XLimits,obj.YLimits,obj.ZLimits,obj.EgoVehicleRadius);移除地平面[pcObstacles,obstacleIndices,groundIndices] = removeGroundPlane(pc, 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 =组装检测(detBBoxes,obj.MeasurementNoise,measParams,time);结束结束结束函数detections = assembleDetections(bboxes,measNoise,measParams,time)此方法以objectDetection格式组装检测。numBoxes = size(bboxes,2);检测= cell(numBoxes,1);为i = 1:numBoxes detection {i} = objectDetection(time,cast(bboxes(:，i)，“双”)，.．.“MeasurementNoise”、双(measNoise),“ObjectAttributes”、结构、.．.“MeasurementParameters”, measParams);结束结束函数bboxes = getBoundingBoxes(ptCloud,minDistance,minDetsPerCluster,maxZDistance,minZDistance)这个方法在每个集群上匹配一些基本的包围框%的规则。集群必须至少有minDetsPerCluster点。它的平均值z必须在maxZDistance和minZDistance之间。%的长度，宽度和高度计算使用最小和最大从每个%的维度。[labels,numClusters] = pcsegdist(ptCloud,minDistance);pointData = ptCloud.Location;bboxes = nan(7,numClusters，“喜欢”, pointData);isValidCluster = false(1,numClusters);为i = 1:numClusters thisPointData = pointData(labels == i，:);meanPoint = mean(thisPointData,1);如果size(thisPointData,1) > minDetsPerCluster &&.．.meanPoint(3) < maxZDistance && meanPoint(3) > minZDistance cuboid = pcfitcuboid(pointCloud(thisPointData));偏航=长方体。方位(3);L =长方体。维数(1);W =长方体。维数(2);H =长方体。维数(3);如果Abs(偏航)> 45个可能性=偏航+ [-90;90];[~，toChoose] = min(abs(可能));偏航=可能性(toChoose);温度= 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,outliers] = pcfitplane(ptCloudIn,maxGroundDist,referenceVector,maxAngularDist);ptCloudOut = select(ptCloudIn,outliers);obstacleIndices = currentIndices(异常值);groundIndices = currentIndices(groundIndices);结束函数[ptCloudOut,indices,croppedIndices] = cropPointCloud(ptCloudIn,xLim,yLim,zLim,egoVehicleRadius)此方法选择限制内的点云并删除车辆点云使用findNeighborsInRadiuslocations = 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;validIndices = inside & nonEgoIndices;indexes = find(validIndices);croppedIndices = find(~validIndices);ptCloudOut = select(ptclouddin, indexes);结束

函数(检测、obstacleIndices groundIndices croppedIndices,.．.confirmedTracks, modelProbs] = mexLidarTracker(ptCloudLocations,time)持续的detectorModel跟踪器detectableTracksInput currentNumTracks如果isempty(detectorModel) || isempty(tracker) || isempty(detectableTracksInput) || isempty(currentNumTracks)%使用与MATLAB相同的起始种子在SIL中重现结果%的模拟。rng (2018);一个包围盒检测器模型。detectorModel = helpboundingboxdetector (.．.“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 = [7 10];历史逻辑的确认阈值delThreshold = [8 10];%历史逻辑删除阈值Kc = 1e-9;单位卷误报率%filterInitFcn = @ helperinitmfilter;追踪器=追踪器“FilterInitializationFcn”filterInitFcn,.．.“TrackLogic”，“历史”，.．.“AssignmentThreshold”assignmentGate,.．.“ClutterDensity”Kc,.．.“ConfirmationThreshold”confThreshold,.．.“DeletionThreshold”delThreshold,.．.“HasDetectableTrackIDsInput”,真的,.．.“InitializationThreshold”0,.．.“MaxNumTracks”30岁的.．.“HitMissThreshold”, 0.1);detectableTracksInput = 0 (trace . maxnumtracks,2);currentNumTracks = 0;结束ptCloud = pointCloud(ptCloudLocations);探测器型号[detectorModel(ptCloud,time);%呼叫跟踪器[confirmedTracks，~，allTracks] = tracker(detection,time,detectableTracksInput(1:currentNumTracks，:));更新可检测性输入currentNumTracks = numel(allTracks);detectableTracksInput(1:currentNumTracks，:) = helperCalcDetectability(allTracks，[1 3 6]);获取模型概率modelProbs =零(2，数字(confirmedTracks));如果isLocked(跟踪)为k = 1:数字(confirmedTracks) c1 = getTrackFilterProperties(跟踪器，confirmedTracks(k)。TrackID,“ModelProbabilities”）;Probs = c1{1};modelProbs(1,k) = probs(1);modelProbs(2,k) = probs(2);结束结束结束

函数detectableTracksInput = helperCalcDetectability(tracks,posIndices)%这是一个辅助函数，用于计算的检测概率%的轨迹为激光雷达跟踪的例子。将来可能会被移除%释放。The MathWorks, Inc.版权所有边界盒检测器分割点云的概率较低%进入边界框的距离大于40米。这个函数%使用状态相关的检测概率对这种效应进行建模%每个跟踪器。在一个最大范围后，Pd被设置为一个较高的值%允许以更快的速度删除轨道。如果isempty(tracks) detectableTracksInput = 0 (0,2);返回；结束rMax = 75;rAmbig = 40;statize = number (tracks(1).State);posSelector = 0 (3,stateSize);posSelector(1,posIndices(1)) = 1;posSelector(2,posIndices(2)) = 1;posSelector(3,posIndices(3)) = 1;pos = getTrackPositions(轨道，posSelector);如果coder.target (MATLAB的) trackid = [tracks.TrackID];其他的trackIDs = 0(1，数字(轨道)，“uint32”）;为我= 1:数字(轨道)trackid (i) =轨道(i).TrackID;结束结束[~，~，r] = cart2sph(pos(:，1)，pos(:，2)，pos(:，3));probDetection = 0.9*ones(nummel (tracks)，1);probDetection(r > rAmbig) = 0.4;probDetection(r > rMax) = 0.99;detectableTracksInput = [double(trackIDs(:)) probDetection(:)];结束