机载LiDAR点云滤波-PTD渐进三角网加密（MATLAB代码）

clc;clear;close all; % warning off % path = 'D:\Geosciences\MATLAB script\Photogrammetry\LiDAR2\YB3_down.las'; % lasReader = lasFileReader(path); % pc = readPointCloud(lasReader); data=importdata("samp11.txt"); pt=data(:,1:3); pc=pointCloud(pt); pt = double(pc.Location); xlimit = pc.XLimits; ylimit = pc.YLimits; zlimit = pc.ZLimits; count = pc.Count; pt1=pt; for counts=1:count pt1(counts,4)=counts; end %% 参数设置 grid = 10;%网格大小 distThre = 1;%距离阈值 angleThre = 3*pi/180;%角度阈值 %% 将点云投影到平面 tic fprintf("投影点云...\n"); row = floor((xlimit(2) - xlimit(1))/grid)+1; col = floor((ylimit(2) - ylimit(1))/grid)+1; gridnum = zeros(row,col); for counts= 1:1:count x0 = pt(counts,1); y0 = pt(counts,2); rID = floor((x0 - xlimit(1))/grid)+1;%点号 cID = floor((y0 - ylimit(1))/grid)+1; gridnum(rID,cID) = gridnum(rID,cID)+1; gridindex{rID,cID}(counts) = counts; end %% 提取网格内最低点 fprintf("提取网格内最低点...\n"); for rows=1:row for cols=1:col if isempty(gridindex{rows,cols}) continue else gridindex{rows,cols}(find(gridindex{rows,cols}==0))=[];%grid中点的索引 gridpoint{rows,cols}=pt(gridindex{rows,cols},:);%grid中点的坐标 gridpoint{rows,cols}(:,4)=gridindex{rows,cols}; gridsort{rows,cols}=sortrows(gridpoint{rows,cols},3,'ascend');%按照z值排序 gridbottom{rows,cols}=gridsort{rows,cols}(1,:);%选取网格内最低点 end end end gridbottom=reshape(gridbottom,[row*col,1]); gridbottom=cell2mat(gridbottom);%前三列是坐标，第四列是索引。 grdidx=gridbottom(:,4); %% 地面点与非地面点 grdflag=zeros(count,1); grdflag(grdidx,:)=1;%0,1索引 grdpt=pt(grdflag==1,:); grdpt1=pt1(grdflag==1,:);%带索引 nongrdpt=pt(grdflag==0,:); nongrdpt1=pt1(grdflag==0,:);%带索引 numgrd=length(grdpt); numnongrd=length(nongrdpt); %% grdptprj = grdpt; grdptprj(:,3) = 0; grdpcprj=pointCloud(grdptprj); nongrdptprj = nongrdpt; nongrdptprj(:,3) = 0; nongrdpcprj=pointCloud(nongrdptprj); %% 根据地面点建立三角网 DT = delaunay(grdpt(:,1),grdpt(:,2));%DT为构成三角形的点索引 numDT=size(DT,1);%三角网数量 % envelopes = zeros(numDT,4); thresmin = 10 ^ (-5); for i=1:numDT triangle{i} = grdpt(DT(i,:),:);%三角形 空间坐标 [R{i},center{i}] = ThreePtCircle( ... triangle{i}(1,1:2), ... triangle{i}(2,1:2), ... triangle{i}(3,1:2)); center{i}(:,3)=0; [idxcir{i},distcir{i}] = findNeighborsInRadius(nongrdpcprj,center{i},R{i},'sort',true); % minVal = min(triangle{i}); % maxVal = max(triangle{i}); % envelopes(i,1) = minVal(1);%minx 三角形边界四至 % envelopes(i,2) = minVal(2);%miny % envelopes(i,3) = maxVal(1);%maxx % envelopes(i,4) = maxVal(2);%maxy % enve(1,:)=[envelopes(i,1),envelopes(i,2)];%西南 % enve(2,:)=[envelopes(i,1),envelopes(i,4)];%西北 % enve(3,:)=[envelopes(i,3),envelopes(i,4)];%东北 % enve(4,:)=[envelopes(i,3),envelopes(i,2)];%东南 % enve(5,:)=[envelopes(i,1),envelopes(i,2)];%西南 tri{i}(1:3,:)=triangle{i}(1:3,:); tri{i}(4,:)=triangle{i}(1,:); trisort{i}=sortrows(triangle{i},[-2,1]);%先排序第2列，再排序第1列。 % plot(enve(:,1),enve(:,2)) % hold on % plot(tri{i}(:,1),tri{i}(:,2)) coef{i}(1,:)=slope(trisort{i}(1,1),trisort{i}(1,2),trisort{i}(2,1),trisort{i}(2,2)); coef{i}(2,:)=slope(trisort{i}(2,1),trisort{i}(2,2),trisort{i}(3,1),trisort{i}(3,2)); coef{i}(3,:)=slope(trisort{i}(3,1),trisort{i}(3,2),trisort{i}(1,1),trisort{i}(1,2)); % coef{i}(abs(coef{i}) < thresmin) = 0; coef{i}(coef{i} == inf) = 10^9; coef{i}(coef{i} == -inf) = -10^9; end coef1=coef'; coef2=sortrows(cell2mat(coef1),1); %% Delauay三角形裁剪 fprintf("Delauay三角形裁剪...\n"); clip=cell(numDT,1);%存储三角形内点云位置 flag=zeros(numnongrd,1); for j=1:numDT mark=0; if 10^9>coef{j}(1,1)&&coef{j}(1,1)>0&&... 10^9>coef{j}(2,1)&&coef{j}(2,1)>0&&... 10^9>coef{j}(3,1)&&coef{j}(3,1)>0 %情况1，边1斜率>0，边2斜率>0，边3斜率>0。 if coef{j}(3,1)>coef{j}(2,1) %k3>k2 ex01_1{j}=trisort{j}; ex01_1(cellfun(@isempty,ex01_1))=[]; for k=1:length(idxcir{j}) idxpt=idxcir{j}(k);%索引 point=nongrdpt(idxpt,:);%点 if point(2)>=coef{j}(1,1)*point(1)+coef{j}(1,2)... &&point(2)>=coef{j}(2,1)*point(1)+coef{j}(2,2)... &&point(2)<=coef{j}(3,1)*point(1)+coef{j}(3,2)... &&flag(idxpt,1)==0 mark=mark+1; clip{j,1}(mark,1:3)=point; clip1{j,1}(mark,1:3)=point; clip1{j,1}(mark,4)=nongrdpt1(idxpt,4); flag(idxpt,1)=1; end end elseif coef{j}(2,1)>coef{j}(3,1) %k2>k3 ex01_2{j}=trisort{j}; ex01_2(cellfun(@isempty,ex01_2))=[]; for k=1:length(idxcir{j}) idxpt=idxcir{j}(k); point=nongrdpt(idxpt,:); if point(2)<=coef{j}(1,1)*point(1)+coef{j}(1,2)... &&point(2)<=coef{j}(2,1)*point(1)+coef{j}(2,2)... &&point(2)>=coef{j}(3,1)*point(1)+coef{j}(3,2)... &&flag(idxpt,1)==0 mark=mark+1; clip{j,1}(mark,1:3)=point; clip1{j,1}(mark,1:3)=point; clip1{j,1}(mark,4)=nongrdpt1(idxpt,4); flag(idxpt,1)=1; end end else continue end elseif -10^9<coef{j}(1,1)&&coef{j}(1,1)<0&&... 10^9>coef{j}(2,1)&&coef{j}(2,1)>0&&... 10^9>coef{j}(3,1)&&coef{j}(3,1)>0 %情况2，边1斜率<0，边2斜率>0，边3斜率>0。 ex02{j}=trisort{j}; ex02(cellfun(@isempty,ex02))=[]; for k=1:length(idxcir{j}) idxpt=idxcir{j}(k); point=nongrdpt(idxpt,:); if point(2)<=coef{j}(1,1)*point(1)+coef{j}(1,2)... &&point(2)>=coef{j}(2,1)*point(1)+coef{j}(2,2)... &&point(2)<=coef{j}(3,1)*point(1)+coef{j}(3,2)... &&flag(idxpt,1)==0 mark=mark+1; clip{j,1}(mark,1:3)=point; clip1{j,1}(mark,1:3)=point; clip1{j,1}(mark,4)=nongrdpt1(idxpt,4); flag(idxpt,1)=1; end end elseif 10^9>coef{j}(1,1)&&coef{j}(1,1)>0&&... -10^9<coef{j}(2,1)&&coef{j}(2,1)<0&&... 10^9>coef{j}(3,1)&&coef{j}(3,1)>0 %情况3，边1斜率>0，边2斜率<0，边3斜率>0。 ex03{j}=trisort{j}; ex03(cellfun(@isempty,ex03))=[]; for k=1:length(idxcir{j}) idxpt=idxcir{j}(k); point=nongrdpt(idxpt,:); if point(2)<=coef{j}(1,1)*point(1)+coef{j}(1,2)... &&point(2)>=coef{j}(2,1)*point(1)+coef{j}(2,2)... &&point(2)>=coef{j}(3,1)*point(1)+coef{j}(3,2)... &&flag(idxpt,1)==0 mark=mark+1; clip{j,1}(mark,1:3)=point; clip1{j,1}(mark,1:3)=point; clip1{j,1}(mark,4)=nongrdpt1(idxpt,4); flag(idxpt,1)=1; end end elseif 10^9>coef{j}(1,1)&&coef{j}(1,1)>0&&... -10^9<coef{j}(2,