高效计算点云平均间距（附Matlab代码）

最近本科导师投了一篇文章，审稿人问一个点云平均间距的问题… 本想着挺简单的，结果matlab一写，400万的点云得算3000分钟…..
于是就写了一个类八叉树的方法计算，结果速度提高300倍，大概10分钟能算完..(C++大神忽略..）

思想很简单
1、首先确定八叉树的间距oc_dis，也就是下图最小的正方体的边长

2、计算点云的X Y Z的最大值和最小值，通过计算得到八叉树格子的数量。
*X_num=(max_x-min_x)/oc_dis
Y_num=(max_y-min_y)/oc_dis
Z_num=(max_z-min_z)/oc_dis*实际代码记得取整
接着就可以生成cell型的Oc_tree了,Octree=cell(X_num,Y_num,Z_num);
3、对所有点进行计算所属八叉树的位置（Matlab对矩阵计算速度较快）
例如，fix(x/oc_dis)+1即可求出X所属树,Y Z等同
4、遍历点云将所有点云的序号放入所属的树里；
5、计算平均点间距时只需要找到这个点所在的树，再找到其周围的树加入计算即可（这里是直接通过点的序调用，所以速度特别快）
下面附上函数

% calculate the average point spacing
% 计算平均点间距
% written by forlin 2016.12.17

%data 前三列为X,Y,Z
function result=APS(data,oc_dis)
    data=data(:,1:3);

    num=size(data,1);
    data(:,7)=[1:num]';
    aps=0;
    min_x=min(data(:,1));
    min_y=min(data(:,2));
    min_z=min(data(:,3));


    data(:,1)=data(:,1)-min_x;
    data(:,2)=data(:,2)-min_y;
    data(:,3)=data(:,3)-min_z;
    oc_x=fix(max(data(:,1))/oc_dis)+1;
    oc_y=fix(max(data(:,2))/oc_dis)+1;
    oc_z=fix(max(data(:,3))/oc_dis)+1;

    oc_tree=cell(oc_x,oc_y,oc_z);
    data(:,4)=fix(data(:,1)/oc_dis)+1;
    data(:,5)=fix(data(:,2)/oc_dis)+1;
    data(:,6)=fix(data(:,3)/oc_dis)+1;

    for i=1:num
        oc_tree{data(i,4),data(i,5),data(i,6)}=[oc_tree{data(i,4),data(i,5),data(i,6)};data(i,7)];
    end


    for i=1:num
        ind_mid=oc_tree{data(i,4),data(i,5),data(i,6)};
        ind2=ind_mid==i;
        ind_mid=ind_mid(~ind2,:);

        if data(i,4)>1
            ind_left=oc_tree{data(i,4)-1,data(i,5),data(i,6)};
        else
            ind_left=[];
        end
        if data(i,4)<oc_x
            ind_right=oc_tree{data(i,4)+1,data(i,5),data(i,6)};
        else
            ind_right=[];
        end

        if data(i,5)>1
            ind_qian=oc_tree{data(i,4),data(i,5)-1,data(i,6)};
        else
            ind_qian=[];
        end
        if data(i,5)<oc_y
            ind_hou=oc_tree{data(i,4),data(i,5)+1,data(i,6)};
        else
            ind_hou=[];
        end

        if data(i,6)>1
            ind_up=oc_tree{data(i,4),data(i,5),data(i,6)-1};
        else
            ind_up=[];
        end
        if data(i,6)<oc_z
            ind_down=oc_tree{data(i,4),data(i,5),data(i,6)+1};
        else
            ind_down=[];
        end

        ind=[ind_mid;ind_right;ind_left;ind_qian;ind_hou;ind_up;ind_down];
        if size(ind,1)>1
            dis=sum((repmat(data(i,1:3),size(ind,1),1)-data(ind,1:3)).^2,2);
            value=min(dis);
            aps=aps+value(1,1);
        else
            dis=sum((repmat(data(i,1:3),num,1)-data(:,1:3)).^2,2);
            value=min(dis);
            aps=aps+value(1,1);
        end

        clear dis value
    end
    result=aps/num;
end

点云