最近在做ICP点云配准,但是点云点数实在太太太多了,ICP完全跑不动啊。
先做个降采样吧:
// % 计算出点云的范围
minX = min(total(:,1));
minY = min(total(:,2));
minZ = min(total(:,3));
maxX = max(total(:,1));
maxY = max(total(:,2));
maxZ = max(total(:,3));
// % 按照2mm把整个点云范围分割成一个一个小方块
cellsize = 0.002;
W = floor((maxX - minX) / cellsize) + 1;
H = floor((maxY - minY) / cellsize) + 1;
D = floor((maxZ - minZ) / cellsize) + 1;
// % 把每个像方块里面的点放在一起
voxel = cell(W,H,D);
for i=1:size(total,1)
I = floor((total(i,1)-minX)/cellsize)+1;
J = floor((total(i,2)-minY)/cellsize)+1;
K = floor((total(i,3)-minZ)/cellsize)+1;
voxel{I,J,K} = [voxel{I,J,K};total(i,:)];
end
// % 取每个小方块里面的第一个点,即为降采样后的新点
compressed = [];
for i=1:W
for j=1:H
for k=1:D
if isempty(voxel {i,j,k})==0
d415_compressed = [d415_compressed;d415_voxel{i,j,k}(1,:)];
end
end
end
end