%下载激光雷达数据Lidardata =螺杆植物;

%选择第一帧ptcloud = lidardata {1};％可视化输入点云图pcshow (ptCloud)标题('输入激光乐点云'）轴（[0 50 -15 15 -5 5]）视图（[ -  42 35]）

用仪表定义ROIxlim = [5 55];ylim = [-3 3];zlim = [-4 1];ROI = [XLIM ylim zlim];使用ROI％作物点云indices = findpointsinroi（ptcloud，Roi）;croppedptcloud = select（ptcloud，索引）;%拆除接地面maxdistance = 0.1;参考矢量= [0 0 1];[型号，inliers，异常值] = pcfitplane（crocpedptcloud，maxdistance，caperencedvector）;地面= SELECT（CRACPEDPEPTCLOUD，INLIERS）;图pcshow（地面）标题（'地平面')视图(3)

histbinresolution = 0.2;[Histval，Yvals] = HelpercomputeHistograph（地面，Histbinresolution）;图绘图（Yvals，Histval，“——k”甘氨胆酸)组(,“XDir”那'逆转')举行在

(峰值、loc) = helperfindpeaks (histVal);startYs = yvals (loc);巷宽= 4;[startLanePoints, detectedPeaks] = helperInitialWindow (startYs、山峰、巷宽);情节(startYs,山峰,“* r”）绘图（STARTLANEPOINTS，DETTETINEDPEAKS，'og')传说('直方图'那'顶峰'那'检测到峰值'那“位置”那'北'） 标题（峰值检测的)举行从

vBinRes = 1;hBinRes = 0.8;numverticalins = cell ((groundPts.XLimits(2) - groundPts.XLimits(1))/vBinRes);groundPts.XLimits laneStartX = linspace (groundPts.XLimits (1), (2), numVerticalBins);％显示宾窗图pcshow（地面）视图（2）Helperdisplaybins（LaneStartx，StartlanePoints（1），HBinres / 2，地面，'红色的'）;helperDisplayBins (laneStartX startLanePoints (2), hBinRes / 2, groundPts,'蓝色'）;标题（'初始化滑动窗口'）

display = true;Lanes = Helperdetectlanes（地面，Hbinres，．．．numverticalbins，startlanepoints，显示）;

％绘制最终车道点Lane1 =车道{1};Lane2 = Lanes {2};图pcshow（地面）标题（'检测到车道点')举行在P1 = Plot3（Lane1（：，1），Lane1（:,2），Lane1（:,3），'* y'）;P2 = Plot3（Lane2（：，1），Lane2（:,2），Lane2（:,3），“* r”）;持有从视图(2)LGND = legend([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,40,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 = legend([p1 p2]，{)“左车道点”那'右车道点'}）;集(lgnd,“颜色”那“白色”那“位置”那“southoutside”）查看（2）持有从

lane3d1 = [xval ` yval1 ` zWorld1 `];lane3d2 = [xval ` yval2 ` zWorld2 `];%沿着y轴移动多项式的高分%分数低的多项式如果error1 > error2 lane多项式= P2;如果> 0 lane多项式(3)= lane3d2(1,2) + laneWidth;其他的lane多项式(3)= lane3d2(1,2) - laneWidth;结束Lane3d1（:,2）= Polyval（Lanepolynomial，Lane3d1（：，1））;Lanepolynomials = [Lanepolynomial;P2];其他的lanePolynomial = P1;如果Lane3D2（1,2）> 0 Lanepolynomial（3）= Lane3D1（1,2）+ LaneWidth;其他的lane多项式(3)= lane3d1(1,2) - laneWidth;结束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,1：2）;DriveinitialParameters = Lanepolynomials（:,3）';initialEstimateError = [1 1 1] * 1E-1;MotionNoise = [1 1 1] * 1E-7;测量管理= 10;％配置卡尔曼滤波器curvefilter = configurekalmanfilter（“ConstantAcceleration”那．．．curveInitialParameters、initialEstimateError motionNoise measurementNoise);driftFilter = configureKalmanFilter (“ConstantAcceleration”那．．．DriveInitialParameters，IniStEstIniationError，MotionNoise，MeasurementNoise）;

初始化随机数生成器rng(2020) numFrames = nummel (lidarData);探测器= helperLaneDetector ('roi'，[5 40 -3 3 -4 1]）;％打开显示[0 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 {i};％检测通道探测器（探测器，Ptcloud）;％从卡尔曼滤波器预测多项式预测(curveFilter);预测(driftFilter);使用卡尔曼滤波器％正确多项式Lanepolynomials =探测器.Lanepolynomial;漂移（i）=平均值（Lanepolynomials（：，3））;Curveshoothness（i）=平均值（Lanepolynomials（：，1））;updatedcurveparams =正确（曲线Filter，Lanepolynomials（1,1：2））;UpdatedDriftParams =正确（Driftfilter，Lanepolynomials（:,3）'）;％更新车道多项式updatedlane多项式= [repmat(updatedCurveParams，[2 1])，updatedDriftParams'];％估计新车道点，更新多项式车道= updateLanePolynomial(检测器,updatedLanePolynomials);filteredDrift (i) =意味着(updatedLanePolynomials (:, 3));filteredCurveSmoothness (i) =意味着(updatedLanePolynomials (: 1));平行拟合后％可视化车道ptCloud。Color = uint8(repmat([0 0 255]，ptCloud.Count,1)); / /用户名lane3dPc1 = pointCloud(车道{1});lane3dPc1。Color = uint8(repmat([255 0 0]，lane3dPc1.Count,1));lanePc = pccat([ptCloud lane3dPc1]);lane3dPc2 = pointCloud(车道{2});lane3dPc2。Color = uint8(repmat([255 255 0]，lane3dPc2.Count,1));lanePc = pccat([lanePc lane3dPc2]); view(player,lanePc)结束

图绘图（漂移）保持在情节(filteredDrift)从标题（“y轴车道漂移”)传说('漂移值'那“过滤漂移值”）

图绘制(curveSmoothness)在绘图（过滤筛选）持有从标题（'曲线平滑度')传说('曲线平滑度'那“曲线平滑过滤”）

函数reflidarData = helperGetDataset() outputFolder = fullfile(tempdir，'wpi'）;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);结束％检查tar.gz文件是否已下载，但没有未压缩如果〜存在（FullFile（OutputFolder，'wpi_lidardata.mat')，“文件”）Untar（LidardataTarfile，Outputfolder）;结束％加载LIDAR数据负载(fullfile (outputFolder'wpi_lidardata.mat')，'lidardata'）;％选择突出强度值的区域reflidarData =细胞(300 1);数= 1;ROI = [-50 50 -30 30 -inf inf];为了i = 81:380 pc = lidarData{i};印第安纳州= findPointsInROI (pc, roi);reflidarData{数}=选择(pc,印第安纳州);Count = Count + 1;结束结束

函数[yval，detedpeaks] = HelperinitialWindow（Yvals，Peaks，Lanewidth）LetlallaneDindes = Yvals> = 0;RightLaneSindices = YVALS <0;LeftLaneys = YVALS（左边林种）;RightLaneys = YVALS（右润andices）;Peaksleft =峰（左边alindices）;peaksright =山峰（右润andices）;diff = zeros（sum（leftlanesindices），sum（rightlaneindices））;为了i = 1：sum（leftlanesindices）为了j = 1：sum（rightlaneindices）diff（i，j）= abs（lanewidth  - （liftlaneys（i） -  rightlaneys（j））））;结束结束[~, minIndex] = min (diff (:));(行,坳)= ind2sub(大小(diff), minIndex);yval = [leftLaneYs(row) rightLaneYs(col)]; / /显示当前位置detectedPeaks = [peaksLeft(row) peaksRight(col)]; / /检测峰值estimatedLaneWidth = leftLaneYs(row) - rightLaneYs(col);％如果计算的车道宽度不在界限内，返回峰值最高的车道如果ABS（估计LaneWidth  -  LaneWidth）> 0.5如果max(peaksLeft) > max(peaksRight) yval = [leftLaneYs(maxLeftInd) NaN];detectedPeaks = [peaksLeft(maxLeftInd) NaN];其他的yval = [NaN rightLaneYs(maxRightInd)];detectedPeaks = [NaN rightLaneYs(maxRightInd)];结束结束结束

函数[P,score] = helperfit多项式(pts,degree,resolution) P = fitpolynomial (pts,degree,resolution);ptsSquare = (polyval(P,pts(: 1)) - pts(: 2)).^2;分数=√意味着(ptsSquare));结束

函数[histval，yvals] = HelpercomputeHutehistograph（Ptcloud，直方图中）Numbins = CeIl（（Ptcloud.ylimits（2） -  ptcloud.ylimits（1））/直方图）;histval =零（1，numbins-1）;binstarty = linspace（ptcloud.ylimits（1），ptcloud.ylimits（2），numbins）;YVALS =零（1，NUMBINS-1）;为了i = 1: numbin -1 roi = [-inf 15 binStartY(i) binStartY(i+1) -inf inf];印第安纳州= findPointsInROI (ptCloud roi);subPc =选择(ptCloud,印第安纳州);如果subPc。Count histVal(i) = sum(subPc.Intensity);yvals(i) = (binStartY(i) + binStartY(i+1))/2;结束结束结束

函数[pkHistVal,pkIdx] = helperfindpeaks(histVal) pkIdxTemp = (1:size(histVal,2))';histValTemp =[南;histVal ';南);tempIdx =(1:长度(histValTemp)。';％只保留任何相等值的第一个（包括NaN）yfinite =〜isnan（histvaltemp）;Ineq = [1;1 + find（（histvaltemp（1：end-1）〜= histvaltemp（2：端））＆．．．（yfinite（1：end-1）| yfinite（2：端）））];tempidx = tempidx（Ineq）;取第一阶导数的符号s =符号（diff（histvaltemp（tempidx）））;％找到本地最大值maxIdx = 1 + find(diff(s)<0);% Index到原始索引向量中，不带NaN书端pkidx = tempidx（maxidx） -  1;％提取峰值的坐标pkHistVal = histVal (pkIdx);pkIdx = pkIdxTemp (pkIdx)”;结束