PTD渐进三角网加密|点云滤波|附matlab代码

clc;clear;close all;
warning off
path = 'C:\Users\1900\Desktop\cloud\7noroad.las';
lasReader = lasFileReader(path);
pc = readPointCloud(lasReader);
%pc=pcread("forestData.pcd");
pt = double(pc.Location);
count = pc.Count;
pt1=pt;
for counts=1:count
    pt1(counts,4)=counts;%带索引
end
%% 参数设置
grid = 5;%网格大小
distThre = 1;%距离阈值
angleThre = 5*pi/180;%角度阈值
numIter=3;%迭代次数
%% 渐进三角网加密滤波
gridbottom=GridMin(pc,grid);%网格种子点
gridbottom1=EdgePoint(pt);%边缘种子点
gridbottom=vertcat(gridbottom,gridbottom1);%合并种子点
grdidx=gridbottom(:,4);
grdflag=zeros(count,1);
grdflag(grdidx,:)=1;%0,1索引
numseed=length(gridbottom);
fprintf('种子点数量为%d个\n',numseed)
numgrdrec=[];
nullrec=[];
for i=1:numIter
    tic
    if i>1
        distThre = distThre/2;
        angleThre = angleThre/2;
    end
    fprintf('\n第%d次迭代...\n',i)
    %% 预处理
    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);
    fprintf('第一次三角网数量为%d个\n',numDT)
    v=[0,0,1];%平面法向量
    for j=1:numDT
        plane(j,:)=ThreePtPlane(...
            grdpt(DT(j,1),:),...
            grdpt(DT(j,2),:),...
            grdpt(DT(j,3),:));
        theta(j,:)=acosd(...
            (plane(j,1:3)*v')/(norm(plane(j,1:3)*norm(v))));
    end
    idxangle=find(theta<70);
    DT=(DT(idxangle,:));
    numDT=size(DT,1);
    fprintf('第二次三角网数量为%d个\n',numDT)
    [idxcir,trisort]=TriangleRadiusKNN(numDT,grdpt,DT,nongrdpcprj);
    [clip,clip1,null,mark]=DelaunayAreaJudge...
        (numnongrd,numDT,grdptprj,DT,idxcir,nongrdptprj,nongrdpt,nongrdpt1);
    nullrec=[nullrec,null];
    grdflag=TriangleThresJudge(numDT,trisort,clip,clip1,angleThre,distThre,grdflag);
    num=length(find(grdflag==1));
    numgrdrec=[numgrdrec,num];
    fprintf('地面点数量为%d个。\n',num)
    toc
    if i>1
        if numgrdrec(i-1)==numgrdrec(i)
            break
        end
    end    
end
%% 结果可视化
grdflag=logical(grdflag);
Agrdpt=select(pc,grdflag);
Anongrdpt=select(pc,~grdflag);
 
figure
pcshowpair(Anongrdpt,Agrdpt,MarkerSize=50, BackgroundColor=[1 1 1])
xlabel('x')
ylabel('y')
zlabel('z')
set(gca,'Color','w')
set(gcf,'position',[0,0,1920,1080]);
title('地面点与非地面点')
 
figure
pcshow(Agrdpt,MarkerSize=50, BackgroundColor=[1 1 1])
xlabel('x')
ylabel('y')
zlabel('z')
set(gca,'Color','w')
set(gcf,'position',[0,0,1920,1080]);
title('地面点')
 
figure
pcshow(Anongrdpt,MarkerSize=50,BackgroundColor=[1 1 1])
xlabel('x')
ylabel('y')
zlabel('z')
set(gca,'Color','w')
set(gcf,'position',[0,0,1920,1080]);
title('非地面点')
%% 种子点可视化
% % figure
% % pcshow(gridbottom(:,1:3),MarkerSize=30)
% % xlabel('x')
% % ylabel('y')
% % zlabel('z')
% % set(gcf,'position',[0,0,1920,1080]);
% 
% % figure
% % scatter3(gridbottom(:,1),gridbottom(:,2),gridbottom(:,3),...
% %     100,'.')
% % xlabel('x')
% % ylabel('y')
% % zlabel('z')
% % set(gcf,'position',[0,0,1920,1080]);
% 
% % figure
% % trimesh(DT,grdpt(:,1),grdpt(:,2),grdpt(:,3))
% % xlabel('x')
% % ylabel('y')
% % zlabel('z')
% % set(gcf,'position',[0,0,1920,1080]);
% 
% % figure
% % triplot(DT,grdpt(:,1),grdpt(:,2))
% % set(gcf,'position',[0,0,1920,1080]);
 
 
 
function [clip,clip1,null,mark]=DelaunayAreaJudge(numnongrd, ...
    numDT,grdptprj,DT,idxcir,nongrdptprj,nongrdpt,nongrdpt1)
    clip=cell(numDT,1);%存储三角形内点云位置
    clip1=cell(numDT,1);
    flag=zeros(numnongrd,1);
    null=0;
    for j=1:numDT
        if isempty(idxcir{j})
            null=null+1;
            continue
        else
            mark=0;
            A=grdptprj(DT(j,1),:);
            B=grdptprj(DT(j,2),:);
            C=grdptprj(DT(j,3),:);
            for k=1:length(idxcir{j})
                idxpt=idxcir{j}(k);
                P=nongrdptprj(idxpt,:);
                Sabc=0.5*sqrt(sum((cross(B-A,C-A,2).^2)')');
                S1=0.5*sqrt(sum((cross(A-P,B-P,2).^2)')');
                S2=0.5*sqrt(sum((cross(A-P,C-P,2).^2)')');
                S3=0.5*sqrt(sum((cross(C-P,B-P,2).^2)')');
                diff{j}(k)=abs(Sabc-S1-S2-S3);
                if diff{j}(k)<10^(-10)&&flag(idxpt,1)==0
                    mark=mark+1;
                    clip{j,1}(mark,1:3)=nongrdpt(idxpt,:);
                    clip1{j,1}(mark,1:3)=nongrdpt(idxpt,:);
                    clip1{j,1}(mark,4)=nongrdpt1(idxpt,4);
                    flag(idxpt,1)=1;
                end
            end
        end
    end
end
 


function gridbottom1=EdgePoint(pt)
%%
    x=pt(:,1);
    y=pt(:,2);
    idxedge=boundary(x,y);
    edgept=pt(idxedge,:);
    edgepc=pointCloud(edgept);
    groundPtsIdx=segmentGroundSMRF(edgepc,...
        MaxWindowRadius=18,...
        SlopeThreshold=0.15,...
        ElevationThreshold=0.5);
    gridbottom1=edgept(groundPtsIdx==1,:);
    gridbottom1(:,4)=idxedge(groundPtsIdx==1,:);    
    
    % figure
    % pcshow(gridbottom1(:,1:3),MarkerSize=30)
    % set(gcf,'position',[0,0,1920,1080]);
    % xlabel('x')
    % ylabel('y')
    % zlabel('z')
end
 

function gridbottom=GridMin(pc,grid)
    pt = double(pc.Location);
    xlimit = pc.XLimits;
    ylimit = pc.YLimits;
    zlimit = pc.ZLimits;
    count = pc.Count;
    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);%前三列是坐标，第四列是索引。    
end


function grdflag=TriangleThresJudge(numDT,trisort,clip,clip1,angleThre,distThre,grdflag)
    for i=1:numDT
        if isempty(clip{i})
            continue        
        else
            p1=trisort{i}(1,:);%三角点1
            p2=trisort{i}(2,:);%三角点2
            p3=trisort{i}(3,:);%三角点3
            plane{i}=ThreePtPlane(p1,p2,p3);
            A=plane{i}(1);B=plane{i}(2);
            C=plane{i}(3);D=plane{i}(4);
            v0{i}=[A,B,C];%平面法向量
            norm0{i}=norm(v0{i});%平面法向量模
            x=clip{i}(:,1);
            y=clip{i}(:,2);
            z=clip{i}(:,3);
            dist{i}=abs(A*x+B*y+C*z+D)/sqrt(A^2+B^2+C^2);%距离
            v1{i}=clip{i}-p1;%向量1
            v2{i}=clip{i}-p2;%向量2
            v3{i}=clip{i}-p3;%向量3
            norm1{i}=norm(v1{i});%模1
            norm2{i}=norm(v2{i});%模2
            norm3{i}=norm(v3{i});%模3
            angle1{i}=asin( ...
                abs(v1{i}*v0{i}')/(norm0{i}*norm1{i}));%角1
            angle2{i}=asin( ...
                abs(v2{i}*v0{i}')/(norm0{i}*norm2{i}));%角2
            angle3{i}=asin( ...
                abs(v3{i}*v0{i}')/(norm0{i}*norm3{i}));%角3
            parameter{i}(:,1)=angle1{i};%角1
            parameter{i}(:,2)=angle2{i};%角2
            parameter{i}(:,3)=angle3{i};%角3
            parameter{i}(:,4)=dist{i};%距离
            parameter{i}(:,5)=clip1{i}(:,4);
        end
    end
    record=[];
    for j=1:numDT
        if isempty(clip{j})
            continue
        else
            for k=1:size(parameter{j},1)
                if parameter{j}(k,1)<angleThre&&...
                        parameter{j}(k,2)<angleThre&&...
                        parameter{j}(k,3)<angleThre&&...
                        parameter{j}(k,4)<distThre
                    record=[record,parameter{j}(k,5)];
                else
                    continue
                end
            end
        end
    end
    grdflag(record,:)=1;
end




function [R,P0] = ThreePtCircle(P1, P2, P3)
    x1 = P1(1);    
    x2 = P2(1);    
    x3 = P3(1);
    y1 = P1(2);    
    y2 = P2(2);    
    y3 = P3(2);
    z1 = x2^2 + y2^2 - x1^2 - y1^2;
    z2 = x3^2 + y3^2 - x1^2 - y1^2;
    z3 = x3^2 + y3^2 - x2^2 - y2^2;
    %计算3点间相互距离
    A = [(x2-x1), (y2-y1); (x3-x1), (y3-y1); (x3-x2), (y3-y2)];
    B = 0.5*[z1;  z2;  z3];
    P0 = (A'*A)\A'*B;
    R1 = sqrt( (P0(1) - P1(1))^2 + (P0(2) - P1(2))^2 );
    R2 = sqrt( (P0(1) - P2(1))^2 + (P0(2) - P2(2))^2 );
    R3 = sqrt( (P0(1) - P3(1))^2 + (P0(2) - P3(2))^2 );
    R = (R1 + R2 + R3)/3;
    P0 = P0';
end
 


function plane=ThreePtPlane(p1,p2,p3)  
    p1p2=p2-p1;
    p1p3=p3-p1;
    n=cross(p1p2,p1p3);%法向量
    D=-dot(n,p1);%常数项
    plane=[n,D];
end


function [idxcir,trisort]=TriangleRadiusKNN(numDT,grdpt,DT,nongrdpcprj)
    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);
        trisort{i}=sortrows(triangle{i},[-2,1]);%先排序第2列，再排序第1列。
    end
end   

结果：