点云降采样

最近在做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