matlab对于点云栅格化，并且提取路面（粗糙版）

      

        能够实现将点云栅格化，然后提取路面；以下代码默认的栅格大小为0.5*0.5*0.5，如果需要精细则可自行更改size_x/size_y/size_z的大小。

        对于栅格区分地面的地面的原理更为粗暴：若一个栅格内的点在z轴上的差距超过0.2，则判定此栅格不是地面（对于此处，很多论文中给出了精确的理论和思路，可自行查阅）。

        程序优化思路：

        代码对于空间栅格搜索，占用内存比较大。每次判定点云在此栅格中点的时候，都会全部过一遍点集，这是不科学的，应当丢弃存入栅格的点集，以免后面重复计算。

        栅格搜索寻找点云的重心，从重心出开始搜索。

clc ;
clear ;
%读取点云，将点云数据读取到lidardata矩阵（n*4的矩阵），但实际中只用到了前三列n*3
lidardata=load('ac.txt');

%  显示原始点云
x2=lidardata(:,1);
y2=lidardata(:,2);
z2=lidardata(:,3);
figure(1);
plot3(x2,y2,z2,'.');
xlabel('X');
ylabel('Y');
zlabel('Z');
%计算x/y/z差值，以方便确定空间栅格的大小
minx=min(lidardata(:,1));
miny=min(lidardata(:,2));
minz=min(lidardata(:,3));
maxx=max(lidardata(:,1));
maxy=max(lidardata(:,2));
maxz=max(lidardata(:,3));
x_long=maxx-minx;
y_long=maxy-miny;
z_long=maxz-minz;
length_lidardata=length(lidardata);
after_lidardata=zeros(1,4);%数组的预分配内存

%after_lidardata1=zeros(100000,4);
% mycell{200,1} = [];

i_C=1;%作为cell数组的列数使用，也就代表空间栅格的个数
a_i=0;%测试for循环次数

%以下for循环就是将空间进行栅格化划分，从x/y/z依次以固定空间范围内搜索点
%将同一个空间栅格内的点放到一起，如果搜索的空间没有点，则跳出，不增加栅格数
%size_x size_y size_z   分别代表栅格在x/y/z上的大小
size_x=0.5;
size_y=0.5;
size_z=0.5;

for i_z=minz :size_z: maxz
    for i_y=miny:size_y:maxy
        for i_x=minx:size_x:maxx
           
            %前面三个for循环是依次搜索空间，以划分栅格
            %下面这个for循环  是搜索空间中所有的点，是不是在当前栅格中，如果是则赋给矩阵after_ladardata
            k=1;
            for i=1:length_lidardata
              if (    lidardata(i,1)>=i_x && lidardata(i,1)<=i_x+size_x && ...
                      lidardata(i,2)>=i_y && lidardata(i,2)<=i_y+size_y &&...
                      lidardata(i,3)>=i_z && lidardata(i,3)<=i_z+size_z)
                 
                 after_lidardata(k,1:4)=lidardata(i,:);
                 k=k+1;

             end
            end
            
            %这里检测after_ladardata数组是不是空集，如果是则跳出此次循环，不增加栅格数
            %如果不是空集，则将数组赋给cell，增加cell所代表的栅格数目
            TF=isempty(after_lidardata);
            a_i=a_i+1;
            if TF==1
                  continue;
            end
            mycell{i_C,1}= after_lidardata;
            i_C=i_C+1;
            after_lidardata=[];%用完之后，一定要致空after_ladardata
        end
    end
    disp("距离算法结束");
    disp(maxz-i_z);%显示进度所有，无实义
end
%seg_buf
seg_mat_B=zeros(1,4);
% A_B=cell2mat(mycell);
A_A=max(size(mycell));%读取mycell数组的维度，以便进行for循环
seg_buf_mat=[];
%这里的4522代表着cell中矩阵的数量，应尽快用程序解决这个问题
for i_p=2:1:A_A
    seg_buf_mat=cell2mat(mycell(i_p,1));
    seg_buf_number=length(seg_buf_mat);
    seg_buf_maxz=max(seg_buf_mat(:,3));
    seg_buf_minz=min(seg_buf_mat(:,3));
    seg_buf_distance=seg_buf_maxz-seg_buf_minz;
    if (seg_buf_number>10 && seg_buf_distance<0.1)
        seg_mat_A=cell2mat(mycell(i_p,1));
        seg_mat_B=[seg_mat_B;seg_mat_A];
    end
   % seg_buf_mat=[];
    
end

B_mycell=seg_mat_B;
B_mycell(all(B_mycell==0,2),:)=[];
A=all(B_mycell==0,2);
my_cell_x=B_mycell(:,1);
my_cell_y=B_mycell(:,2);
my_cell_z=B_mycell(:,3);
figure(2);
plot3(my_cell_x,my_cell_y,my_cell_z,'.');
 xlabel('X');
 ylabel('Y');
 zlabel('Z');