抑制3 d激光雷达点云检测和跟踪
这个例子展示了如何在激光雷达探测和跟踪的限制点云。一个抑制是石头或混凝土的线,连接道路和人行道上。限制作为道路的通行区域的分隔符。
在没有GPS信号的情况下,检测控制的位置是很重要的路径规划和安全导航的自主驾驶的应用程序。
这个示例使用的一个子集PandaSet数据集来自Hesai和规模。PandaSet包含激光雷达点云扫描各种城市通过Pandar64传感器捕获的场景。
介绍
抑制激光雷达点云检测涉及到检测左右道路对自我的限制。激光雷达传感器安装在车的顶部。
使用以下步骤检测限制在点云数据被激光雷达传感器:
提取感兴趣的区域(ROI)
行车和越野点进行分类
确定使用越野路角点
检测候选人抑制点使用行车点
激光雷达数据集下载
数据集的大小是109 MB,它包含50预处理组织点云。每个点云被指定为64 - 1856 - 3数组。地面实况数据包含的语义分割为13类标签。点云存储在PCD格式,和地面真理PNG格式的数据存储。
下载PandaSet数据集使用helperDownloadPandasetDatahelper函数,定义在这个例子。
%设置随机种子产生可重复的结果rng (“默认”);[ptCloudData, labelsData] = helperDownloadPandasetData;
选择第一帧的数据集进行进一步处理。可视化的点云。
ptCloud = ptCloudData {1};标签= labelsData {1};%可视化输入点云与相应的地面真值标签图pcshow (ptCloud.Location、标签)轴在视图(2)标题(“输入激光雷达点云”)
数据进行预处理
作为检测的预处理步骤限制点,首先从点云提取感兴趣的区域和分类的点在行车或越野点。
基于激光雷达传感器的安装位置,点云数据稀疏超过一定距离。确保你提取足够密集的点云进一步处理,指定一个ROI有限距离内的传感器。
%定义ROI米xLim = 20 [-35];yLim = 25 (-25);roiIdx = ptCloud.Location (:: 1) > = xLim (1) &…ptCloud.Location (:,: 1) < = xLim (2) &…ptCloud.Location (:: 2) > = yLim (1) &…ptCloud.Location (:: 2) < = yLim (2);
点云分类到行车和越野点使用这些标签:
1 -无标号
2 -植被
3 -地面
4 -路
5 -道路标记
6边走
7 -汽车
8 -卡车
9 -其他车辆
10 -行人
11 -道路障碍
12 -迹象
13 -建筑
标签(~ roiIdx) =南;%的行车分由地面、道路和人行道上onRoadLabelsIdx =(标签= = = = = = 4 | 3 |标签标签6);%的越野点由建筑物和其他物体offRoadLabelsIdx =(标签= = = = 11 | 2 |标签…标签= = 12 |标签= = 13);%可视化点云,随着行车和越野点。图次要情节(1、3、1)pcshow(选择(ptCloud roiIdx))轴在视图(2)标题(“裁剪输入点云”次要情节(1、3、2)pcshow(选择(ptCloud onRoadLabelsIdx))轴在视图(2)标题(“路点云”次要情节(1,3,3)pcshow(选择(ptCloud offRoadLabelsIdx))轴在视图(2)标题(“越野点云”)
如果语义标签没有可用的数据集,您可以使用深学习网络计算它们。有关更多信息,请参见激光雷达点云的语义分割使用SqueezeSegV2深度学习网络的例子。
检测道路形状
使用detectRoadAngles函数来检测道路角度在越野点云。
梁模型适用于越野功能点。关于梁模型的更多信息,请参阅[1]和[2]。
然后,它适用于一个定制版本的toe-finding算法[3]归一化梁长度找到道路角度。
%选择越野点云offRoadPtCloud =选择(ptCloud offRoadLabelsIdx);%使用detectRoadAngles函数来检测角度的必经之路roadAngles = detectRoadAngles (offRoadPtCloud);
检测道路限制
使用segmentCurbPoints函数部分的抑制点行车点云。函数使用以下步骤计算抑制点:-
1)提取特征抑制点的路点。
识别控制分行车点云、空间特性用于模型后道路限制:
每一个点的函数计算这些特征每个扫描线。的点满足以上特性的特征阈值特性抑制点。
2)计算候选抑制点特性控制的点。
功能抑制点可能包含假阳性。进一步去除假阳性,函数过程特性抑制点使用基于RANSAC二次多项式拟合提取候选抑制点。
%选择越野点云onRoadPtCloud =选择(ptCloud onRoadLabelsIdx OutputSize =“全部”);%使用segmentCurbPoints功能段的限制行车点云[~,candidateCurbPoints] = segmentCurbPoints (onRoadPtCloud roadAngles,…HorizontalAngularResolution = 0.24);%可视化与候选人的点云覆盖抑制点图pcshow (select (ptCloud roiIdx) .Location,“w”(2)持有)视图在pcshow (candidateCurbPoints.Location“r”MarkerSize = 200)视图(2)从轴在标题(“候选人抑制点检测”)
跟踪控制分
循环和过程激光雷达数据提取和跟踪候选人抑制点。跟踪抑制点提高抑制检测的鲁棒性。你可以停止的控制跟踪分割道路,并重启时自我车辆离开分割道路。控制跟踪涉及到对XY-points使用多项式拟合二表示为多项式 ,在那里一个,b,c多项式的参数。控制跟踪是一个两步的过程:
跟踪控制多项式参数一个和b控制的曲率多项式。
跟踪控制多项式参数c控制漂移的多项式。
更新这些参数通过使用卡尔曼滤波器以一个恒定的速度运动模型。使用configureKalmanFilter函数初始化一个卡尔曼滤波器。
rng (“默认”)球员= pcplayer (20 [-35]、[-25 25] -10 [10], MarkerSize = 6);candidateCurbQueue = [];candidateCurbQueueSize = 3;candidateCurbQueueCount = 1;prevOffRoadPtCloud = [];prevCandidateCurbPoints = [];prevAngle = [];rmse = 1;%为卡尔曼滤波初始值initParams。initialEstimateError = (1 1 1) * 1 e 1;initParams。motionNoise = (1 1 1) * 1 e;initParams。measurementNoise = 1 e3;leftTracker。漂移= [];leftTracker。filteredDrift = [];leftTracker。curveSmoothness = []; leftTracker.filteredCurveSmoothness = []; rightTracker.drift = []; rightTracker.filteredDrift = []; rightTracker.curveSmoothness = []; rightTracker.filteredCurveSmoothness = []; isTracking = false; isLeftCurbTrackingInitialised = false; isRightCurbTrackingInitialised = false;%找到点躺在ROIxLim = 20 [-35];yLim = 25 (-25);[ptCloudData, labelsData] = helperDownloadPandasetData;为fileIdx = 1:元素个数(ptCloudData)%加载点云的地面真值标签ptCloud = ptCloudData {fileIdx};标签= labelsData {fileIdx};%找到点躺在ROIroiIdx = ptCloud.Location (:: 1) > = xLim (1) &…ptCloud.Location (:,: 1) < = xLim (2) &…ptCloud.Location (:: 2) > = yLim (1) &…ptCloud.Location (:: 2) < = yLim (2);标签(~ roiIdx) =南;%点进路点和越野点进行分类onRoadLabelsIdx =(标签= = = = = = 4 | 3 |标签标签6);offRoadLabelsIdx =(标签= = = = = = 11 | 2 |标签标签12 |…标签= = 13);offRoadPtCloud =选择(ptCloud offRoadLabelsIdx);onRoadPtCloud =选择(ptCloud onRoadLabelsIdx,“OutputSize”,“全部”);%计算使用越野路角点roadAngles = detectRoadAngles (offRoadPtCloud);%计算道路段在前面和后面%自我车辆numFrontRoadSegments =总和(roadAngles < 110 | roadAngles > 250);numRearRoadSegments =总和(~ (roadAngles < 110 | roadAngles > 250));%检查控制跟踪应该启用或禁用如果~ isTracking如果(numFrontRoadSegments < numRearRoadSegments) | |…(元素个数(roadAngles) = = 2) isTracking = true;结束其他的如果numFrontRoadSegments > numRearRoadSegments isTracking = false;isLeftCurbTrackingInitialised = false;isRightCurbTrackingInitialised = false;结束结束%将候选点从前面的帧%当前帧如果~ isempty (prevOffRoadPtCloud) gridStep = 0.5;[tform, rmse] = pcregistericp (prevOffRoadPtCloud offRoadPtCloud);prevCandidateCurbPoints = pctransform (pccat (candidateCurbQueue),…tform);结束%计算抑制点使用行车点如果rmse > 0.7 [~, candidateCurbPoints] = segmentCurbPoints (onRoadPtCloud,…roadAngles HorizontalAngularResolution = 0.24);其他的[~,candidateCurbPoints] = segmentCurbPoints (onRoadPtCloud,…roadAngles prevCandidateCurbPoints,…HorizontalAngularResolution = 0.24);结束如果isempty (candidateCurbPoints) prevOffRoadPtCloud = offRoadPtCloud;prevAngle = roadAngles;继续;结束curbPoints = candidateCurbPoints.Location;xVal = linspace (0, xLim (2), 80);如果isTracking leftCurb = [];rightCurb = [];模型= pcfitplane(选择(ptCloud onRoadLabelsIdx), 0.1,…(0 0 1);modelParams = model.Parameters;candidateAngles = atan2d (curbPoints (:, 2), curbPoints (: 1));candidateAngles (candidateAngles < 0) = candidateAngles (…candidateAngles < 0) + 360;%将候选人控制分,分为左和右抑制点如果(roadAngles (1) > = 270 | | roadAngles (1) & & < 90)…(roadAngles (2) > = 90 & & roadAngles (2) < 270) idx = candidateAngles > = roadAngles (1) &…candidateAngles < roadAngles (2);其他的idx = candidateAngles > = roadAngles (2) |…candidateAngles < roadAngles (1);结束leftCandidateCurbPoints = curbPoints (idx:);rightCandidateCurbPoints = curbPoints (~ idx:);%计算多项式跟踪左和右的限制leftPolynomial = fitPolynomialRANSAC (…leftCandidateCurbPoints (: 1:2), 2, 0.1);rightPolynomial = fitPolynomialRANSAC (…rightCandidateCurbPoints (: 1:2), 2, 0.1);如果~ isempty (leftPolynomial)如果~ isLeftCurbTrackingInitialised [leftFilter leftCurb] = helperTrackCandidateCurbs (…leftPolynomial、xVal modelParams initParams);isLeftCurbTrackingInitialised = true;其他的[leftPolynomial, leftCurb leftTracker] = helperTrackCandidateCurbs (…leftPolynomial、xVal modelParams,…leftFilter leftTracker);结束结束如果~ isempty (rightPolynomial)如果~ isRightCurbTrackingInitialised [rightFilter rightCurb] = helperTrackCandidateCurbs (…rightPolynomial、xVal modelParams initParams);isRightCurbTrackingInitialised = true;其他的[rightPolynomial, rightCurb rightTracker] = helperTrackCandidateCurbs (…rightPolynomial、xVal modelParams,…rightFilter rightTracker);结束结束ptCloud =选择(ptCloud roiIdx);%使用helperVisualiseOutput helper函数,连接%的例子作为支持文件想象最终的万博1manbetx输出outputPtCloud = helperVisualiseOutput (ptCloud leftCurb rightCurb,…[]);其他的ptCloud =选择(ptCloud roiIdx);%使用helperVisualiseOutput helper函数,连接%的例子作为支持文件想象最终的万博1manbetx输出outputPtCloud = helperVisualiseOutput (ptCloud curbPoints);结束candidateCurbQueue = [candidateCurbQueue; candidateCurbPoints];如果candidateCurbQueueCount < = candidateCurbQueueSize candidateCurbQueueCount = candidateCurbQueueCount + 1;其他的candidateCurbQueue = candidateCurbQueue(2:结束);结束prevOffRoadPtCloud = offRoadPtCloud;prevAngle = roadAngles;如果isOpen(球员)视图(球员,outputPtCloud);视图(球员。一个xes,[0 90]) camva(player.Axes,0)结束结束
分析控制跟踪的漂移和平滑度
分析控制检测结果,比较它们对跟踪控制多项式通过绘制数据。每一个情节比较有和没有的卡尔曼滤波器的参数。第一个图比较了漂移的限制轴,和第二个图比较平滑的抑制多项式。平滑度的变化率是抑制多项式的斜率。
图ax =次要情节(2,1,1);情节(leftTracker.drift)在情节(leftTracker.filteredDrift)从标题(“左沿着轴抑制漂移”)ax.Position (2) = ax.Position (2) - 0.08;ax =次要情节(2,1,2);情节(rightTracker.drift)在情节(rightTracker.filteredDrift)从标题(“对抑制随波逐流轴”)ax.Position (2) = ax.Position (2) - 0.06;Lgnd =传奇(“漂移价值观”,“过滤漂移价值观”,…“定位”,“水平”);Lgnd.Position (1:2) = (0.29 - 0.9);
图ax =次要情节(2,1,1);情节(leftTracker.curveSmoothness);持有在情节(leftTracker.filteredCurveSmoothness)从标题(“左曲线平滑度”)ax.Position (2) = ax.Position (2) - 0.08;ax =次要情节(2,1,2);情节(rightTracker.curveSmoothness);持有在情节(rightTracker.filteredCurveSmoothness)从标题(“正确的曲线光滑”)ax.Position (2) = ax.Position (2) - 0.06;Lgnd =传奇(“曲线平滑度”,“曲线平滑过滤”,…“定位”,“水平”);Lgnd.Position (1:2) = (0.23 - 0.9);
辅助函数
的helperDownloadPandasetDatahelper函数将激光雷达数据集加载到MATLAB工作区。
函数[ptCloudData, labelsData] = helperDownloadPandasetData () lidarDataTarFile = matlab.internal.examples.downlo万博1manbetxadSupportFile (激光雷达的,…“数据/ CurbPointCloudData.tar”);filepath = fileparts (lidarDataTarFile);outputFolder = fullfile (filepath,“CurbPointCloudData”);%检查tar文件下载,但不是未压缩的。如果(~存在(fullfile (outputFolder“激光雷达”),“文件”))…& & (~ (fullfile (outputFolder,存在“semanticLabels”),“文件”)解压(lidarDataTarFile outputFolder);结束lidarFiles = dir (fullfile (outputFolder激光雷达的,‘* .pcd‘));labelFiles = dir (fullfile (outputFolder“semanticLabels”,‘* . png”));ptCloudData =细胞(元素个数(lidarFiles), 1);labelsData =细胞(元素个数(labelFiles), 1);为fileIdx = 1:元素个数(lidarFiles) ptCloud = pcread (fullfile (lidarFiles (fileIdx) .folder lidarFiles (fileIdx) . name));%的示例遵循约定的自我车辆运动%的轴,因此,将点云θ= -90;反式= (0 0 0);tform = rigidtform3d([0 0θ),反式);ptCloudData {fileIdx} = pctransform (ptCloud tform);labelsData {fileIdx} = imread (fullfile (labelFiles (fileIdx) .folder labelFiles (fileIdx) . name));结束结束
的helperTrackCandidateCurbshelper函数跟踪候选人使用卡尔曼滤波器抑制点。
函数varargout = helperTrackCandidateCurbs(变长度输入宗量)多项式=变长度输入宗量{1};xVal =变长度输入宗量{2};modelParams =变长度输入宗量{3};如果元素个数(变长度输入宗量)= = 4 initParams =变长度输入宗量{4};initParams。initialEstimateError = initParams.initialEstimateError (1:2);initParams。motionNoise = initParams.motionNoise (1:2);curveInitialParameters =多项式(1:2);driftInitialParameters =多项式(3);%配置卡尔曼滤波器过滤器。curveFilter = configureKalmanFilter(…“ConstantVelocity”curveInitialParameters,…initParams.initialEstimateError,…initParams.motionNoise,…initParams.measurementNoise);过滤器。driftFilter = configureKalmanFilter (…“ConstantVelocity”driftInitialParameters,…initParams.initialEstimateError,…initParams.motionNoise,…initParams.measurementNoise);varargout{1} =过滤器;其他的过滤器=变长度输入宗量{4};追踪=变长度输入宗量{5};预测(filter.curveFilter);预测(filter.driftFilter);跟踪器。漂移= [tracker.drift;多项式(3)];跟踪器。curveSmoothness = [tracker.curveSmoothness; polynomial(1)];%正确使用卡尔曼滤波器多项式updatedCurveParams =正确(filter.curveFilter,…多项式(1:2));updatedDriftParams =正确(filter.driftFilter,…多项式(3));%更新多项式多项式= [updatedCurveParams, updatedDriftParams];跟踪器。filteredDrift = [tracker.filteredDrift;多项式(3)];跟踪器。filteredCurveSmoothness = [tracker.filteredCurveSmoothness;多项式(1)];varargout{1} =多项式;varargout{3} =追踪;结束%坐标估计yVal = polyval(多项式,xVal);% z坐标估计zVal = (-modelParams (1) * xVal - modelParams (2) * yVal - modelParams (4))…/ modelParams (3);%的最终控制抑制= [xVal yVal zVal”);varargout{2} =抑制;结束
引用
[1]张、Yihuan Xiaonian Wang Jun Wang和约翰·m·多兰。“Road-Segmentation-Based抑制检测方法通过3 d-lidar传感器自动驾驶。”IEEE智能交通系统19日,没有。12(2018年12月):3981 - 91。https://doi.org/10.1109/TITS.2018.2789462。
[2]王、Guojun剑,瑞他和本田。“速度和准确性权衡基于激光雷达数据的道路边界检测。”IEEE / CAA自动化杂志》上8,不。(2021年6月6日):1210 - 20。https://doi.org/10.1109/JAS.2020.1003414。
[3],Jiuxiang Anshuman拉兹丹,约翰·c·Femiani明崔,彼得旺卡。“从空中道路网络提取和交叉检测图像通过跟踪足迹。”IEEE地球科学和遥感45岁的没有。12(2007年12月):4144 - 57。https://doi.org/10.1109/TGRS.2007.906107。
