最近本科导师投了一篇文章,审稿人问一个点云平均间距的问题… 本想着挺简单的,结果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
点云