matlab ply格式点云读入、显示、三角化

%2019.10.21 suyunzzz

%将ply格式的点云数据，先转换为txt文件，
% 然后以mat格式读入并显示，然后将点云网格化。
%Q：为什么不直接从ply格式读取呢？
% A：因为matlab自带的点云工具箱貌似没有直接对ply或者pcd文件进行重构的。
% % % 所以得需要将ply文件转换为mat格式。


filename = './bunny/bunny/reconstruction/bun_zipper_res2.ply';
ptcloud = pcread(filename);
% pcshow(ptcloud);

    Data(:,1)= double(ptcloud.Location(1:5:end,1));   %提取所有点的三维坐标
    Data(:,2)= double(ptcloud.Location(1:5:end,2));
    Data(:,3)= double(ptcloud.Location(1:5:end,3)); 
       namesplit=strsplit(filename,'.ply');           %分割ply文件的名称，分成文件名与ply后缀名
        frontname=namesplit{1};                           %提取文件名，舍弃后缀名
%  eval('fid=fopen(''1_buny.txt'',''wt'');');   
   eval(['fid=fopen(''',frontname,'.txt'',''wt'');']);      
     [b1,b2]=size(Data);    
    for i=1:b1                   %将二维数组Data写入txt格式文件中
        for j=1:b2-1
            fprintf(fid,'%.4f\t ',Data(i,j));           %所有坐标数据保留小数点后四位
        end
        fprintf(fid,'%.4f\n',Data(i,b2));
    end
    clear Data;                 
    fclose(fid);
    
    
    filename_read =[frontname,'.txt'];%将字符串拼接起来

    fprintf('\n');
    fprintf(filename_read);
    fprintf('\n');
    load(filename_read);    %读入txt文件为mat文件

     
    %将X1_buny赋值给一个p
    
    p = bun_zipper_res2;
    
    %显示点云
    figure(1);
hold on
axis equal
title('点云','fontsize',14)
plot3(p(:,1),p(:,2),p(:,3),'r.')
view(-37.5,30)


[t]=MyCrust(p);

%% plot of the oyput triangulation
figure(2)
        hold on
        title('三角化输出','fontsize',14)
        axis equal
%         trisurf(t,p(:,1),p(:,2),p(:,3),'facecolor','g','edgecolor','r')%plot della superficie trattata
         trimesh(t,p(:,1),p(:,2),p(:,3),'edgecolor','r')%plot della superficie trattata
        view(-37.5,30)

 MyCrust.m

%% MyCrust
%
%Simple surface recostruction program based on Crust algorithm
%Given a set of 3D points returns a triangulated tight surface.
%
%The more points there are the best the surface will be fitted,
%although you will have to wait more. For very large models an 
%help memory errors may occurs.
%It is important even the point distribution, generally uniformly 
% distributed points with denser zones in high curvature features
% give the best results.
% 
%

% ͨ�����ڸ����������о��н��ܼ�����ľ��ȷֲ�����ṩ��ѽ����


% Remember  crust algorithom needs a cloud representing a volume 
% so open surface may give inaccurate results. 
%  
%  
% If any problems occurs in execution, or if you found a bug,
% have a suggestion or question just contact me at:  
%   
% giaccariluigi@msn.com
% 
%
%
%
%Here is a simple example:
%
%load Dino.mat%load input points from mat file
%
%[t]=MyCrust(p);
%
% figure(1)
%         hold on
%         title('Output Triangulation','fontsize',14)
%         axis equal
%         trisurf(t,p(:,1),p(:,2),p(:,3),'facecolor','c','edgecolor','b')
%
%Input:
%              p is a Nx3 array containing the 3D set of points
%Output:
%              t are points id contained in triangles Nx3 array too.
%
% See also qhull, voronoin, convhulln, delaunay, delaunay3, tetramesh.
%
%Author:Giaccari Luigi
%Last Update: 1/12/2008
%Creation: 10/10/2008


function [t]=MyCrust(p)
%%   Main
starttime=clock;


%add points to the given ones, this is usefull
%to create outside tetraedrom
tic
p=AddShield(p);
fprintf('Addedded Shield: %4.4f s\n',toc)


tic
tetr=delaunayn(p);%creating tedraedron ����������
tetr=int32(tetr);%use integer to save memory
fprintf('Delaunay Triangulation Time: %4.4f s\n',toc)


%connectivity data
%find triangles to tetraedrom and tetraedrom to triangles connectivity data
tic
[t2tetr,tetr2t]=Connectivity(tetr);
fprintf('Connectivity Time: %4.4f s\n',toc)
tic
[cc,r]=CC();%Circumcenters of tetraedroms
fprintf('Circumcenters Time: %4.4f s\n',toc)
clear n




tic
t=Walking();%Flagging tetraedroms as inside or outside%�������忴���ǵ�����ⲿ��
fprintf('Walking Time: %4.4f s\n',toc)


time=etime(clock,starttime);
fprintf('Total Time: %4.4f s\n',time)





%% Circumcenters(Nested)
    function [cc,r]=CC()
         %finds circumcenters fro a set of tetraedrom
        
         %points of tetraedrom 
        p1=(p(tetr(:,1),:));
        p2=(p(tetr(:,2),:));
        p3=(p(tetr(:,3),:));
        p4=(p(tetr(:,4),:));

        %vectors of tetraedrom edges
        v21=p(tetr(:,1),:)-p(tetr(:,2),:);
        v31=p(tetr(:,3),:)-p(tetr(:,1),:);
        v41=p(tetr(:,4),:)-p(tetr(:,1),:);

        %preallocation
        cc=zeros(size(tetr,1),3);






         %Solve the system using cramer method
        d1=sum(v41.*(p1+p4)*.5,2);
        d2=sum(v21.*(p1+p2)*.5,2);
        d3=sum(v31.*(p1+p3)*.5,2);

        det23=(v21(:,2).*v31(:,3))-(v21(:,3).*v31(:,2));
        det13=(v21(:,3).*v31(:,1))-(v21(:,1).*v31(:,3));
        det12=(v21(:,1).*v31(:,2))-(v21(:,2).*v31(:,1));

        Det=v41(:,1).*det23+v41(:,2).*det13+v41(:,3).*det12;



        detx=d1.*det23+...
            v41(:,2).*(-(d2.*v31(:,3))+(v21(:,3).*d3))+...
            v41(:,3).*((d2.*v31(:,2))-(v21(:,2).*d3));



        dety=v41(:,1).*((d2.*v31(:,3))-(v21(:,3).*d3))+...
            d1.*det13+...
            v41(:,3).*((d3.*v21(:,1))-(v31(:,1).*d2));



        detz=v41(:,1).*((v21(:,2).*d3)-(d2.*v31(:,2)))...
            +v41(:,2).*(d2.*v31(:,1)-v21(:,1).*d3)...
            +d1.*(det12);

        %Circumcenters
        cc(:,1)=detx./Det;
        cc(:,2)=dety./Det;
        cc(:,3)=detz./Det;


         %Circumradius
        r=((sum((p2-cc).^2,2))).^.5;%quadrato del raggio
    end



%% Connectivity(Nested)

    function [t2tetr,tetr2t]=Connectivity(tetr)

       

        numt = size(tetr,1);
        vect = 1:numt;
        t = [tetr(:,[1,2,3]); tetr(:,[2,3,4]); tetr(:,[1,3,4]);tetr(:,[1,2,4])];%triangles not unique
        [t,j,j] = unique(sort(t,2),'rows');%triangles
        t2tetr = [j(vect), j(vect+numt), j(vect+2*numt),j(vect+3*numt)];%each tetraedrom has 3 triangles


        % triang-to-tetr connectivity
      
        nume = size(t,1);
        tetr2t  = zeros(nume,2,'int32');
        count= ones(nume,1,'int8');
        for k = 1:numt

            for j=1:4
                ce = t2tetr(k,j);
                tetr2t(ce,count(ce)) = k;
                count(ce)=count(ce)+1;
            end

        end


    end      % connectivity()



%% Walking(Nested)
    function t=Walking()
        np=size(p,1)-540;%540 = number of shield points put at the end of array
        numtetr=size(tetr,1);
        nt=size(tetr2t,1);


        deleted=true(numtetr,1);%deleted tetraedroms
        checked=false(numtetr,1);%checked tetraedros 
        onfront=false(nt,1);%tetraedroms that need to be checked
        countchecked=0;
        %First flag as outsde tetraedroms with Shield points
        for i=1:numtetr
            for j=1:4
                if tetr(i,j)>np;
                    deleted(i)=true;
                    checked(i)=true;
                    onfront(t2tetr(i,:))=true;
                    countchecked=countchecked+1;
                    break
                end
            end
        end

        %tollerances to mark as in or out
        toll=zeros(nt,1)+.95; 
        level=0;
        
        alpha=zeros(nt,1);%intersection factor
        %it is computed from radius of the tetraedroms circumscribed sphere
        % and the distance between their center
        for i=1:nt
            if tetr2t(i,2)>0 %jump boundary tetraedrom
                    distcc=sum((cc(tetr2t(i,1),:)-cc(tetr2t(i,2),:)).^2,2);%distance from circumcenters
                    %intersection factor
                    alpha(i)=(-distcc+r(tetr2t(i,1))^2+r(tetr2t(i,2))^2)/(2*r(tetr2t(i,1))*r(tetr2t(i,2)));
            end
        end
        clear cc
        
%         Now we scan all tetraedroms. When one is scanned put on front is
%         neighobur. This means that now even the neighobour can be
%         checked. At the begining only tetraedroms with shield points are
%         on front, because we are sure the are out.
%         Tetraedrom with high intersction factor  will be marked as equal
%         else different.
%         When I say high i mean under a set tollerance that becames lower as the algorithm
%         progresses. This Aims to avoid errors propagation when a tetraedrom is wrong marked.
%         
        
        while countchecked<numtetr && level<50
             level=level+1;%level of scan reached
             
            for id=1:nt%loop trough triangles

                if onfront(id)

                    tetr1=tetr2t(id,1);tetr2=tetr2t(id,2);%tetraedroms linked to triangle under analysis
                    if  tetr2==0 %do not check boundary triangles
                        onfront(id)=false;
                        continue
                       
                    elseif (checked(tetr1) && checked(tetr2)) %tetraedroms are already checked
                         onfront(id)=false;
                        continue
                       
                    end

                    if alpha(id)>=toll(id) %flag as equal
                        if checked(tetr1)%find the checked one between the two
                            deleted(tetr2)=deleted(tetr1) ;%flag as equal
                            checked(tetr2)=true;%check
                            countchecked=countchecked+1;
                            onfront(t2tetr(tetr2,:))=true;%put on front all tetreadrom triangles
                        else
                            deleted(tetr1)=deleted(tetr2) ;%flag as equal
                            checked(tetr1)=true;%check
                            countchecked=countchecked+1;
                            onfront(t2tetr(tetr1,:))=true;%put on front all tetreadrom triangles
                        end
                         onfront(id)=false;%remove from front
                   
                    
                    elseif alpha(id)<-toll(id)%flag as different
                        if checked(tetr1)%find the checked one between the two
                            deleted(tetr2)=~(deleted(tetr1)) ;%flag as different
                            checked(tetr2)=true;%check
                            countchecked=countchecked+1;
                            onfront(t2tetr(tetr2,:))=true;%put on front all tetreadrom triangles
                        else
                            deleted(tetr1)=~(deleted(tetr2)) ;%flag as different
                            checked(tetr1)=true;%check
                            countchecked=countchecked+1;
                            onfront(t2tetr(tetr1,:))=true;%put on front all tetreadrom triangles
                        end
                         onfront(id)=false;%remove from front

                         
                    else
                        toll(id)=toll(id)-.05;%tolleraces were too high next time will be lower

                    end



                end
            end
            
            if level==31 %brute continuation(this may appens when the triangulation is corrupt)
                 warning('Brute continuation necessary')
                onfront(t2tetr(~(checked),:))=true;%force onfront collocation
            end
        end
        
        %delete tetraedroms marked as outside 
        tetr(deleted,:)=[];
        
        
        
        %take boundary triangles from tetraedroms
        %this is the raw surface and needs improvements to be used in CAD
        %systems. Maybe in my next revision I will add surface post treatments.
        %Anyway for grafical purpose this should be good.

        %extract boundary triangles 
        t=BoundTriangles(tetr);
        
        
        %Output Data
        numchecked=countchecked/numtetr;
        if level==50
            warning([num2str(level),' th level was reached\n'])
        else
         fprintf('%4.4f th level was reached\n',level)
        end
        fprintf('%4.4f %% of Tetraedrom were checked\n',numchecked*100)



    end




end

%% AddAddShield
function pnew=AddShield(p)

%find the bounding box
maxx=max(p(:,1));
maxy=max(p(:,2));
maxz=max(p(:,3));
minx=min(p(:,1));
miny=min(p(:,2));
minz=min(p(:,3));

%give offset to the bounding box
step=max(abs([maxx,minx,maxy,miny,maxz,minz]));

maxx=maxx+step;
maxy=maxy+step;
maxz=maxz+step;
minx=minx-step;
miny=miny-step;
minz=minz-step;
step=step/100;
N=10;



%creating a grid lying on the bounding box
vx=linspace(minx,maxx,N);
vy=linspace(miny,maxy,N);
vz=linspace(minz,maxz,N);




[x,y]=meshgrid(vx,vy);
facez1=[x(:),y(:),ones(N*N,1)*maxz];
facez2=[x(:),y(:),ones(N*N,1)*minz];
[x,y]=meshgrid(vy,vz-step);
facex1=[ones(N*N,1)*maxx,x(:),y(:)];
facex2=[ones(N*N,1)*minx,x(:),y(:)];
[x,y]=meshgrid(vx-step,vz);
facey1=[x(:),ones(N*N,1)*maxy,y(:)];
facey2=[x(:),ones(N*N,1)*miny,y(:)];

%add points to the p array
pnew=[p;
 facex1;
 facex2;
 facey1;
 facey2;
 facez1;
 facez2];

% figure(2)
% plot3(pnew(:,1),pnew(:,2),pnew(:,3),'.g')


end



%% BoundTriangles
function t=BoundTriangles(tetr)

numt = size(tetr,1);
vect = 1:numt;
t = [tetr(:,[1,2,3]); tetr(:,[2,3,4]); tetr(:,[1,3,4]);tetr(:,[1,2,4])];
[t,j,j] = unique(sort(t,2),'rows');
t2tetr = [j(vect), j(vect+numt), j(vect+2*numt),j(vect+3*numt)];


% triang-to-tetr connectivity
% Each row has two entries corresponding to the triangle numbers
% associated with each triangle. Boundary triangles have only one tetraedrom
nume = size(t,1);
tetr2t  = zeros(nume,2,'int32');
count= ones(nume,1,'int32');
for k = 1:numt

    for j=1:4
        ce = t2tetr(k,j);
        tetr2t(ce,count(ce)) = k;
        count(ce)=count(ce)+1;
    end

end
tbound=false(numt,1);
tbound=tetr2t(:,2)==0;
t=t(tbound,:);
end