2d-3d坐标转换

对于常规相机，SLAM里使用针孔相机模型
　

简而言之，一个空间点[x,y,z]和它在图像中的像素坐标[u,v,d] (d
指深度数据) 的对应关系是这样的：
$u=x⋅f_xz+cx,$u=x⋅fx​z+cx,
$v=y⋅f_yz+cy,$v=y⋅fy​z+cy,
$d=z⋅s,$d=z⋅s,
其中，fx,fy指相机在x,y两个轴上的焦距，cx,cy指相机的光圈中心，s指深度图的缩放因子。
这个公式是从(x,y,z)推到(u,v,d)的。反之，我们也可以把它写成已知(u,v,d)，推导(x,y,z)的方式。请读者自己推导一下。
不，还是我们来推导吧……公式是这样的：
$z=d/s$z=d/s
$x=(u−cx)⋅z/f_x ,$x=(u−cx)⋅z/fx​,
$y=(v−cy)⋅z/f_y$y=(v−cy)⋅z/fy​
怎么样，是不是很简单呢？事实上根据这个公式就可以构建点云啦。

通常，我们会把fx,fy,cx,cy这四个参数定义为相机的内参矩阵C，也就是相机做好之后就不会变的参数。相机的内参可以用很多方法来标定，详细的步骤比较繁琐，我们这里就不提了。给定内参之后呢，每个点的空间位置与像素坐标就可以用简单的矩阵模型来描述了：

和t是相机的姿态。R代表旋转矩阵，t代表位移矢量。因为我们现在做的是单幅点云，认为相机没有旋转和平移。所以，把R设成单位矩阵I，把t设成了零。s是scaling factor，即深度图里给的数据与实际距离的比例。由于深度图给的都是short (mm单位)，s通常为1000。