深度图与彩色图对齐算法

---

符号说明：

$$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$$