基于地面控制点的密集点云道路面识别及MATLAB实现（一）

前言

地面控制点一般选用道路、水泥地等一些接近路面的场地，因此通过多个地面控制点拟合平面，利用点云到该平面的距离，再给予一定的距离阈值来识别路面是可以实现的。相比于传统的平面栅格、点云法向量、模型拟合、面元网格，该方法具有效率快速，无需先验条件的优势。 注：本文主要是记录识别路面的思路，识别结果有很强的鲁棒性以及较高的冗余度，且无法适用于道路具有较高的坡度的路面。

一、利用地面控制点识别路面

（1）首先对控制点进行预处理，剔除控制点的高程过大的点，若是高程过大，会使后面的路面识别出现较大的误差。通过统计可将控制点按高程分段，找到最底层且点数最多的一层，作为符合条件地面控制点。

foint_data=pcread("last_point.ply");
figure;
pcshow(foint_data);
GNSS_data=importdata("vic\GNSS控制点.txt");
GNSS_X=GNSS_data.data(:,1);
GNSS_Y=GNSS_data.data(:,2);
GNSS_Z=GNSS_data.data(:,3);
figure;
scatter3(GNSS_X,GNSS_Y,GNSS_Z,'red','.');
title('以X-Y平面查看控制点');
set(gca,'XLim',foint_data.XLimits);
set(gca,'YLim',foint_data.YLimits);
set(gca,'ZLim',foint_data.ZLimits);
view(0,90)
figure;
scatter3(GNSS_X,GNSS_Y,GNSS_Z,'black','.');
title('以X-Z平面查看控制点');
set(gca,'XLim',foint_data.XLimits);
set(gca,'YLim',foint_data.YLimits);
set(gca,'ZLim',foint_data.ZLimits);
view(90,0)
hold on

space=fix(abs(max(GNSS_Z)-min(GNSS_Z))/3);
dic1=find(GNSS_Z<min(GNSS_Z)+space);
dic2=find(GNSS_Z<min(GNSS_Z)+2*space & GNSS_Z>min(GNSS_Z)+space);
dic3=find(GNSS_Z>min(GNSS_Z)+2*space & max(GNSS_Z));
gnss_base1=GNSS_data.data(dic1,:);
gnss_base2=GNSS_data.data(dic2,:);
gnss_base3=GNSS_data.data(dic3,:);

p=plot3(gnss_base1(:,1),gnss_base1(:,2),gnss_base1(:,3),'r',gnss_base2(:,1),gnss_base2(:,2),gnss_base2(:,3),'b', ...
    gnss_base3(:,1),gnss_base3(:,2),gnss_base3(:,3),'gr');
p(1).LineWidth = 1;p(2).LineWidth = 1;p(3).LineWidth = 1;
m=legend('控制点高程分布',['区间一高程∈',num2str(min(GNSS_Z)),'~',num2str(min(GNSS_Z)+space)], ...
       ['区间二高程∈',num2str(min(GNSS_Z)+space),'~',num2str(min(GNSS_Z)+2*space)],...
   ['区间三高程∈',num2str(min(GNSS_Z)+2*space),'~',num2str(max(GNSS_Z))]);
m.Location='northwest';
grid off;
hold off;

（2）利用符合条件的地面控制点拟合平面，注意这里是拟合！平面！，并非拟合曲面，估计拟合曲面也可以，具体可以靠读者自己探索，本文采用MATLAB中 pcfitplane 函数拟合平面，函数具体用法如何：

假设该平面的多项式方程为AX+BY+CZ+D=0

函数	model=pcfitplane(rase,0.2,[0,0,1],5)
model	返回参数，分model.Parameters以及model.Normal，前者为拟合平面的方程系数（为1×4的行向量），后者为平面的法向量（为1×4的行向量）
rase	需要进行拟合的点云，这里是地面控制点，可以用pointCloud函数将控制点构建成点云格式
maxDistance = 0.02	平面拟合的最大点到平面距离 （2cm）
referenceVector = [0,0,1]	平面的法线向量
maxAngularDistance = 5	将最大角距离设置为 5 °

当采用第二种方法时，其控制点高程平均值H所在平面的公式为：CZ+H=0

rase=pointCloud(gnss_base1);
model=pcfitplane(rase,0.2,[0,0,1],5);
A=model.Parameters(1);B=model.Parameters(2);C=model.Parameters(3);D=model.Parameters(4);
%==========AX+BY+CZ+D=0===========
%==========Z=(-A/C)X+(-B/C)Y+(-D/C)============
X=meshgrid(min(GNSS_X):fix(abs(max(GNSS_X)-min(GNSS_X)))/100:max(GNSS_X));
Y=(meshgrid(min(GNSS_Y):fix(abs(max(GNSS_Y)-min(GNSS_Y)))/100:max(GNSS_Y)))';
Z=(-A/C).*X+(-B/C).*Y-D/C;
%===================================================
figure;
scatter3(GNSS_X,GNSS_Y,GNSS_Z,'black','filled');
title('以X-Z平面查看拟合平面');
set(gca,'XLim',foint_data.XLimits);
set(gca,'YLim',foint_data.YLimits);
set(gca,'ZLim',foint_data.ZLimits);
hold on
mesh(X,Y,Z,'edgecolor','r');
m=legend('控制点高程分布','拟合平面');
m.Location='northwest';
view(125,1)

p_coord=foint_data.Location;
distance=A.*p_coord(:,1)+B.*p_coord(:,2)+C.*p_coord(:,3)+D./(A^2+B^2+C^2)^0.5;
figure;
cc=histogram(distance);
cc.FaceColor='g';
xlabel('distance(/m)');
ylabel('point number')
title('点云到控制点拟合平面的距离直方图')

Ao=0;Bo=0;Co=1;Do=-floor(mean(GNSS_Z));
distance_aver=Ao.*p_coord(:,1)+Bo.*p_coord(:,2)+Co.*p_coord(:,3)+Do./(Ao^2+Bo^2+Co^2)^0.5;
figure;
Z_aver=ones(length(X),length(Y)).*(-Do);
scatter3(GNSS_X,GNSS_Y,GNSS_Z,'black','filled');
title('以X-Z平面查看拟合平面');
set(gca,'XLim',foint_data.XLimits);
set(gca,'YLim',foint_data.YLimits);
set(gca,'ZLim',foint_data.ZLimits);
hold on
mesh(X,Y,Z_aver,'edgecolor','r');
m=legend('控制点高程分布','控制点平均高程值平面');
m.Location='northwest';
view(125,1)

figure;
cc=histogram(distance_aver);
cc.FaceColor='g';
xlabel('distance(/m)');
ylabel('point number')
title('点云到控制点平均值的垂直距离直方图')

以下两幅图是点云到拟合平面的距离直方图，可以为距离阈值T的选取提供参考。
其中距离到平面的距离公式采用：

二、结果

T=2;
road_doc=find(abs(distance)<T);
dd=[0.5,0.5,0];
color=repmat(dd,length(foint_data.Location(road_doc,:)),1);
road_base=pointCloud(foint_data.Location(road_doc,:),'Color',color);
figure;
pcshow(road_base);
view(0,90);
title('利用地面控制点拟合平面识别路面');

T_aver=1.5;
road_doc_aver=find(abs(distance_aver)<T_aver);
dd=[0.5,0.5,0];
color_aver=repmat(dd,length(foint_data.Location(road_doc_aver,:)),1);
road_base_aver=pointCloud(foint_data.Location(road_doc_aver,:),'Color',color_aver);
figure;
pcshow(road_base_aver);
view(0,90);
title('利用地面控制点高程平均值识别路面');