%下载激光雷达数据lidarData = helperGetDataset;

%选择第一帧ptCloud = lidarData {1};%可视化输入点云图pcshow (ptCloud)标题(“输入激光雷达点云”15)轴([0 50 -15 5 5])视图(35 [-42])

%定义ROI米55 xlim = [5];ylim = 3 [3];zlim = 1 [4];投资回报率= [xlim ylim zlim];%作物使用ROI点云指数= findPointsInROI (ptCloud roi);croppedPtCloud =选择(ptCloud、指标);%去除地平面maxDistance = 0.1;referenceVector = (0 0 1);(模型、内围层异常值)= pcfitplane (croppedPtCloud、maxDistance referenceVector);groundPts =选择(croppedPtCloud,内围层);图pcshow (groundPts)标题(地平面的)视图(3)

histBinResolution = 0.2;[histVal, yvals] = helperComputeHistogram (groundPts histBinResolution);图绘制(yvals histVal,“——k”甘氨胆酸)组(,“XDir”,“反向”)举行在

(峰值、loc) = helperfindpeaks (histVal);startYs = yvals (loc);巷宽= 4;[startLanePoints, detectedPeaks] = helperInitialWindow (startYs、山峰、巷宽);情节(startYs,山峰,“* r”)情节(startLanePoints detectedPeaks,的噩)传说(“直方图”,“高峰”,“检测峰”,“位置”,“北”)标题(峰值检测的)举行从

vBinRes = 1;hBinRes = 0.8;numVerticalBins =装天花板((groundPts.XLimits (2)——groundPts.XLimits (1)) / vBinRes);groundPts.XLimits laneStartX = linspace (groundPts.XLimits (1), (2), numVerticalBins);%显示本窗口图pcshow (groundPts)视图(2)helperDisplayBins (laneStartX, startLanePoints (1) hBinRes / 2, groundPts,“红色”);helperDisplayBins (laneStartX startLanePoints (2), hBinRes / 2, groundPts,“蓝”);标题(“滑动窗口初始化”)

显示= true;车道= helperDetectLanes (groundPts hBinRes,…numVerticalBins startLanePoints,显示);

%的阴谋最终弄点lane1 =车道{1};lane2 =车道{2};图pcshow (groundPts)标题(“发现弄点”)举行在p1 = plot3 (lane1 (: 1), lane1 (:, 2), lane1 (:, 3),“性感”);p2 = plot3 (lane2 (: 1), lane2 (:, 2), lane2 (:, 3),“* r”);持有从视图(2)lgnd =传奇((p1 p2) {“左车道点”,“右车道点”});集(lgnd,“颜色”,“白色”,“位置”,“southoutside”)

[P1, error1] = helperFitPolynomial (lane1 (:, 1:2), 2, 0.1);[P2, error2] = helperFitPolynomial (lane2 (:, 1:2), 2, 0.1);xval = linspace (5, 80);yval1 = polyval (P1, xval);yval2 = polyval (P2, xval);% z坐标估计modelParams = model.Parameters;zWorld1 = (-modelParams (1) * xval - modelParams (2) * yval1 - modelParams (4)) / modelParams (3);zWorld2 = (-modelParams (1) * xval - modelParams (2) * yval2 - modelParams (4)) / modelParams (3);%安装可视化巷图pcshow (croppedPtCloud)标题(“多项式拟合巷”)举行在p1 = plot3 (xval yval1 zWorld1,“y”,“线宽”,0.2);p2 = plot3 (xval yval2 zWorld2,“r”,“线宽”,0.2);lgnd =传奇((p1 p2) {“左车道点”,“右车道点”});集(lgnd,“颜色”,“白色”,“位置”,“southoutside”(2)持有)视图从

lane3d1 = [xval yval1 zWorld1”);lane3d2 = [xval yval2 zWorld2”);%将多项式与沿轴向一个高分%的多项式很低的分数如果error1 > error2 lanePolynomial = P2;如果lane3d1 (1、2) > 0 lanePolynomial (3) = lane3d2(1、2) +巷宽;其他的lanePolynomial (3) = lane3d2(1、2) -巷宽;结束lane3d1 (:, 2) = polyval (lanePolynomial, lane3d1 (: 1));lanePolynomials = [lanePolynomial;P2);其他的lanePolynomial = P1;如果lane3d2 (1、2) > 0 lanePolynomial (3) = lane3d1(1、2) +巷宽;其他的lanePolynomial (3) = lane3d1(1、2) -巷宽;结束lane3d2 (:, 2) = polyval (lanePolynomial, lane3d2 (: 1));lanePolynomials = [P1;lanePolynomial]结束%可视化车道后平行安装图pcshow (ptCloud)轴([0 50 -15 15 5 5])在p1 = plot3 (lane3d1 (: 1), lane3d1 (:, 2), lane3d1 (:, 3),“y”,“线宽”,0.2);p2 = plot3 (lane3d2 (: 1), lane3d2 (:, 2), lane3d2 (:, 3),“r”,“线宽”,0.2);视图(90[-90])标题(“上道”)lgnd =传奇((p1 p2) {“左车道点”,“右车道点”});集(lgnd,“颜色”,“白色”,“位置”,“southoutside”)举行从

%初始值curveInitialParameters = lanePolynomials (1:2);driftInitialParameters = lanePolynomials (:, 3)”;initialEstimateError = (1 1 1) * 1 e 1;motionNoise = (1 1 1) * 1 e;measurementNoise = 10;%配置卡尔曼滤波器curveFilter = configureKalmanFilter (“ConstantAcceleration”,…curveInitialParameters、initialEstimateError motionNoise measurementNoise);driftFilter = configureKalmanFilter (“ConstantAcceleration”,…driftInitialParameters、initialEstimateError motionNoise measurementNoise);

%初始化随机数生成器rng (2020) numFrames =元素个数(lidarData);探测器= helperLaneDetector (“投资回报”,40 3 3 4 1 [5]);%打开显示球员= pcplayer (50 [0], [-15 15], [5] 5);漂移= 0 (numFrames, 1);filteredDrift = 0 (numFrames, 1);curveSmoothness = 0 (numFrames, 1);filteredCurveSmoothness = 0 (numFrames, 1);为i = 1: numFrames ptCloud = lidarData {};%检测车道detectLanes(探测器,ptCloud);%从卡尔曼滤波器预测多项式预测(curveFilter);预测(driftFilter);%正确使用卡尔曼滤波器多项式lanePolynomials = detector.LanePolynomial;漂移(i) =意味着(lanePolynomials (:, 3));curveSmoothness (i) =意味着(lanePolynomials (: 1));updatedCurveParams =正确(curveFilter lanePolynomials (1:2));updatedDriftParams =正确(driftFilter lanePolynomials (:, 3) ');%更新巷多项式updatedLanePolynomials = [repmat (updatedCurveParams 1 [2]), updatedDriftParams”);%多项式估计新巷点更新车道= updateLanePolynomial(检测器,updatedLanePolynomials);filteredDrift (i) =意味着(updatedLanePolynomials (:, 3));filteredCurveSmoothness (i) =意味着(updatedLanePolynomials (: 1));%可视化车道后平行安装ptCloud。颜色= uint8 (repmat ([0 0 255] ptCloud.Count 1));lane3dPc1 = pointCloud(车道{1});lane3dPc1。颜色= uint8 (repmat ([255 0 0] lane3dPc1.Count 1));lanePc = pccat ([ptCloud lane3dPc1]);lane3dPc2 = pointCloud(车道{2});lane3dPc2。颜色= uint8 (repmat (lane3dPc2.Count (255 255 0), 1));lanePc = pccat ([lanePc lane3dPc2]); view(player,lanePc)结束

图绘制(漂移)在情节(filteredDrift)从标题(“莱恩随波逐流轴”)传说(“漂移值”,“过滤漂移值”)

图绘制(curveSmoothness)在情节(filteredCurveSmoothness)从标题(曲线平滑的)传说(曲线平滑的,“曲线平滑过滤”)

函数reflidarData = helperGetDataset () outputFolder = fullfile (tempdir,批发价格指数的);url =“//www.tatmou.com/金宝appsupportfiles/lidar/data/WPI_LidarData.tar.gz”;lidarDataTarFile = fullfile (outputFolder,“WPI_LidarData.tar.gz”);如果~存在(lidarDataTarFile“文件”mkdir (outputFolder);disp (“下载WPI激光雷达驾驶数据(760 MB)……”);websave (lidarDataTarFile、url);解压(lidarDataTarFile outputFolder);结束%检查焦油。gz文件下载,但不是未压缩的如果~ (fullfile (outputFolder,存在“WPI_LidarData.mat”),“文件”)解压(lidarDataTarFile outputFolder);结束%载激光雷达数据负载(fullfile (outputFolder“WPI_LidarData.mat”),“lidarData”);%选择地区著名的强度值reflidarData =细胞(300 1);数= 1;投资回报率=(-50 -30 30负无穷到正无穷);为我= 81:380 pc = lidarData {};印第安纳州= findPointsInROI (pc, roi);reflidarData{数}=选择(pc,印第安纳州);数=计数+ 1;结束结束

函数[yval, detectedPeaks] = helperInitialWindow (yvals,山峰,巷宽)leftLanesIndices = yvals > = 0;rightLanesIndices = yvals < 0;leftLaneYs = yvals (leftLanesIndices);rightLaneYs = yvals (rightLanesIndices);peaksLeft =山峰(leftLanesIndices);peaksRight =山峰(rightLanesIndices);diff = 0(和(leftLanesIndices)和(rightLanesIndices));为i = 1: sum (leftLanesIndices)为j = 1: sum (rightLanesIndices) diff (i, j) = abs(巷宽- (leftLaneYs (i) - rightLaneYs (j)));结束结束[~,minIndex] = min (diff (:));(行,坳)= ind2sub(大小(diff), minIndex);yval = [leftLaneYs(行)rightLaneYs (col)];detectedPeaks = [peaksLeft(行)peaksRight (col)];estimatedLaneWidth = leftLaneYs(行)——rightLaneYs (col);%如果计算车道宽度范围内,%返回巷的最高峰如果abs (estimatedLaneWidth -巷宽)> 0.5如果马克斯(peaksLeft)马克斯(peaksRight) yval = [leftLaneYs (maxLeftInd)南];detectedPeaks = [peaksLeft (maxLeftInd)南);其他的yval =[南rightLaneYs (maxRightInd)];detectedPeaks =[南rightLaneYs (maxRightInd)];结束结束结束

函数[P,分数]= helperFitPolynomial(分,学位,分辨率)P = fitPolynomialRANSAC(分,学位,决议);ptsSquare = (polyval (P分(:1))-分(:,2))^ 2;分数=√意味着(ptsSquare));结束

函数[histVal, yvals] = helperComputeHistogram (ptCloud histogramBinResolution) numBins =装天花板((ptCloud.YLimits (2) - ptCloud.YLimits (1)) / histogramBinResolution);numBins-1 histVal = 0 (1);ptCloud.YLimits binStartY = linspace (ptCloud.YLimits (1), (2), numBins);numBins-1 yvals = 0 (1);为i = 1: numBins-1 roi =[负15 binStartY (i) binStartY (i + 1)负无穷到正无穷];印第安纳州= findPointsInROI (ptCloud roi);subPc =选择(ptCloud,印第安纳州);如果subPc。计数histVal (i) = (subPc.Intensity)之和;yvals (i) = (binStartY(我)+ binStartY (i + 1) / 2;结束结束结束

函数[pkHistVal, pkIdx] = helperfindpeaks (histVal) pkIdxTemp =(1:尺寸(histVal 2) ';histValTemp =[南;histVal ';南);tempIdx =(1:长度(histValTemp)。';%只保留任何相邻的第一条相等的值(包括南)yFinite = ~ isnan (histValTemp);iNeq = [1;1 +找到(histValTemp (1: end-1) ~ = histValTemp(2:结束)&…(yFinite (1: end-1) | yFinite(2:结束))));tempIdx = tempIdx (iNeq);%首样导数的符号s =符号(diff (histValTemp (tempIdx)));%找到当地的最大值maxIdx = 1 +找到(diff (s) < 0);%索引到原始的索引向量没有南书挡pkIdx = tempIdx (maxIdx) - 1;%获取坐标的高峰pkHistVal = histVal (pkIdx);pkIdx = pkIdxTemp (pkIdx)”;结束