39、视觉SLAM技术全解析：从相机标定到多传感器融合

视觉SLAM技术全解析：从相机标定到多传感器融合

1. 双目相机外参标定

双目相机外参主要指左右相机之间的相对位姿，标定双目相机外参意味着求解左右相机坐标系之间的变换关系。在单目相机标定中，如果已经得到了单应矩阵 $H = [h_1 h_2 h_3]$ 和相机内参矩阵 $K$，则可以结合相关方程求出比例因子 $\lambda$：
$| r_1 | = | \lambda K^{-1} h_1 | = 1$，进而得到 $\lambda = 1 / | K^{-1} h_1 |$。

设相机坐标系到世界坐标系的旋转矩阵为 $R = [r_1 r_2 r_3]$，根据旋转矩阵的性质 $r_3 = r_1 \times r_2$，可以求出旋转矩阵 $R$ 和平移矩阵 $t$，它们就是相机相对于标定板的外参。

在双目相机外参标定过程中，如果分别求出左右相机相对于同一标定板的外参 $R_l$、$t_l$、$R_r$ 和 $t_r$，则某一目标点 $P$ 在左右相机坐标系中的坐标 $p_l$ 和 $p_r$ 可以表示为：
$p_l = R_l P + t_l$
$p_r = R_r P + t_r$

消去上述方程中的 $P$，可得：
$p_r = R_r R_l^{-1} p_l + t_r - R_r R_l^{-1} t_l$

令左相机坐标系到右相机坐标系的旋转矩阵为 $ {l}^{r}R$，平移矩阵为 $ {l}^{r}t$，则有：
$p_r = {l}^{r}R p_l + {l}^{r}t$
$ {l}^{r}R = R_r R_l^{-1}