深度图与彩色图对齐算法

---

符号说明：

$$K_d:深度摄像头内参$$
$$K_c:彩色摄像头内参$$
$$T_{w2d}:世界坐标系转换到深度摄像头坐标系的外参矩阵$$
$$T_{w2c}:世界坐标系转换到彩色摄像头坐标系的外参矩阵$$
$$T_{d2c}：深度摄像头坐标系转换到彩色摄像头坐标系的外参矩阵$$
$$P_{dc}：深度图下某点在深度摄像头坐标系下的三维点坐标$$
$$P_{u,v}^d：深度图下某点在深度图像坐标系下的二维点坐标$$
$${\dot{P}}_{dc}:深度图下某点像素还原到深度坐标系下的空间点$$
$$P_{cc}：彩色图下某点在彩色摄像头坐标系下的三维点坐标$$
$$P_{u,v}^c：彩色图下某点在彩色图像坐标系下的二维点坐标$$
$${\dot{P}}_{cc}:由世界坐标系转换到彩色坐标系下的空间点$$
$$P_w:深度图转换到世界坐标系下的点$$

---

像素坐标系、图像坐标系、相机坐标系、世界坐标系的转换：

---

$$z_c\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& u_0& 0\\ 0& f_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$$

根据以上公式，可得出像素坐标系（u，v）到相机坐标系的三维点（$$x_c，y_c，z_c$$）之间的转换关系为：

$$\{\begin{array}{l}{x}_c=z_c\cdot \left(u-u_0\right)\cdot dx/f\\ y_c=z_c\cdot \left(v-v_0\right)\cdot dy/f\\ z_c=z_c\end{array}$$
其中，fx =f/dx ，为u轴上的尺度因子，即u轴上的归一化焦距。

---

（1）第一步，将深度图的像素点还原到深度坐标系下
$${\dot{P}}_{dc}=Z{K}_d^{-1}{P}_{u,v}^d$$
（2）第二步：将深度空间坐标系的深度点转换到世界坐标系下

$$P_w=T_{w2d}^{-1}{\dot{P}}_{dc}$$
（3）第三步：将世界坐标系的深度点转换到彩色摄像头坐标系

$${\dot{P}}_{cc}=T_{w2c}{P}_w$$
（4）第四步：将彩色摄像头坐标系下的深度点映射到Z=1的彩色平面上

$$P_{u,v}^c=K_c\left({\dot{P}}_{cc}/z\right)$$
​ 其中，$$\left({\dot{P}}_{cc}/z\right)$$ 表示按z轴归一化，即该点的xyz都分别除以z值
（5）以上四步，就完成了深度平面图的点转换到彩色平面上，给彩色平面上的像素点增加一个信息通道{深度}

（6）实际处理中，一般把第二第三步直接结合起来，即获取一个从深度摄像头坐标系到彩色摄像头坐标系的欧氏变换矩阵。具体推算如下：
$${\dot{P}}_{cc}=T_{w2c}{P}_w=T_{w2c}{T}_{w2d}^{-1}{\dot{P}}_{dc}$$

$$T_{d2c}=T_{w2c}{T}_{w2d}^{-1}=\left[\begin{array}{cc}{R}_{w2c}{R}_{w2d}^{-1}& t_{w2c}-R_{w2c}{R}_{w2d}^{-1}{t}_{w2d}\\ 0& 1\end{array}\right]$$

有了这个，就可以直接将深度摄像头空间坐标系下的点转换到彩色摄像头空间坐标系下了。