通过深度图计算各点的三维坐标

通过深度图计算各点的三维坐标

1、公式

说明：
Z：是深度图像中每个像素点的值。是一个n×m的矩阵。
u和v：分别是二维矩阵Z中每个点所对应的x和y像素点坐标，范围分别是1-m和1-n。
fx、fy、Cx和Cy是拍摄相机的相机参数。
X和Y为所需要求得的空间三维坐标。

2、MATLAB计算代码

% fopen fclose fprint meshgrid
im=imread('0717_depth.png');

fx=2966.1;
fy=2963.4;
cx=412.6385;
cy=269.7441;

% whos
% mesh(double(im));
im_Z=double(im)/100.0;   %初始化Z矩阵中各点

row=size(im,1);
column=size(im,2);

[u,v]=meshgrid(1:column,1:row);

im_X=im_Z.*(u-cx)/fx;   %注意是点乘
im_Y=im_Z.*(v-cy)/fy;

f=fopen('0717_depth.ASC','w');
for r=1:row
    for c=1:column
        if(im_Z(r,c)>1)      %过滤掉零零点
            fprintf(f,'%f',im_X(r,c));
            fprintf(f,'%s',',');
            fprintf(f,'%f',im_Y(r,c));
            fprintf(f,'%s',',');
            fprintf(f,'%f',im_Z(r,c));
            fprintf(f,'\r\n');
        end;
    end;
end;
fclose(f);

% Data = [im_X(:),im_Y(:),im_Z(:)];
% Data(all(Data==0,2),:) = [];   %去除太小的00零点
% Data=single(Data);
% ptCloud = pointCloud(Data(:,1:3));
% pcwrite(ptCloud, 'myTest.pcd', 'Encoding', 'ascii'); %将程序中的xyz数据写入pcd文件中
% pc = pcread('myTest.pcd');
% pcshow(pc); %显示点云

3、用meshlab查看ASC数据

1、下载和安装meshlab：网上搜索下载meshlab即可，安装时默认安装。
2、用meshlab打开’×××.asc’文件，弹出对话框选择项如下：

3、结果显示：

4、原始图像：