ToF三维点云重建

ToF sensor 导出的数据进行三维重建，根据激光从出发到经过散射之后到目标物再被探测器接收到这个时间信息来求解目标物的位置和立体形状扫描足够多的点的时候，就能够得到目标物上的做够多的点的距离值，只有可以恢复出目标物的3维形状。

clear all
close all
clc
fid=fopen（‘fpcd.bin‘‘r‘）;
orignaldata=fread（fid‘float32‘）;%数据格式转换
pixelarray=reshape（orignaldata30720084）;
a=pixelarray（:50）;
b=reshape（a476800）;
z_lvbo=b（3:）;
z_lvbo1=reshape（z_lvbo320240）;
  K = medfilt2（z_lvbo1）;
  b（3:）=reshape（K176800）;
for i=1:76800
   
        if b（3i）>0.535 || b（3i）<0.3
            b（3i）=0;
            b（2i）=0;
            b（1i）=0;
        end 
        
    
end
for i=1:76800
   
        if b（1i）>0.14
            b（3i）=0;
            b（1i）=0;
            b（2i）=0;
        end 
   
end
for i=1:76800
   
        if b（1i）<-0.2
            b（3i）=0;
            b（1i）=0;
            b（2i）=0;
        end 
   
end
for i=1:76800
   
        if b（2i）>0.14
            b（3i）=0;
            b（1i）=0;
            b（2i）=0;
        end 
   
end
x=b（1:）;
y=b（2:）;
z=b（3:）;
x（find（x==0））=[];
y（find（y==0））=[];
z（find（z==0））=[];  
Lx=size（x2）;
A=zeros（Lx3）;
A（:1）=x（1:）;
A（:2）=y（1:）;
A（:3）=z（1:）;
[idx dist] = knnsearch（AA‘dist‘‘euclidean‘‘k‘6）;   % 寻找每一个三维点最近的4个点（除去其本身）
 b1=sum（dist2）/5; %%最近邻5个点的距离和
b2（:1）=b1（:1）;