深刻理解相机的pose

之前从未仔细想过到底相机的pose 意味着什么，或者准确地来说，两个物体之间的pose意味着什么，以及在实际计算的时候,该注意什么.

从kitti数据集谈起

看 kitti_raw的论文时会发现各种传感器的位置如图

比如在用pykitii这个工具包时，会发现在raw.py 里面关于坐标变换是这么算的.以3号相机为例.

假设我要得到从 velo到cam3的 pose信息.

代码里面经过了这样操作

 T_cam0unrect_velo = self._load_calib_rigid(velo_to_cam_file)
 data['T_cam0_velo_unrect'] = T_cam0unrect_velo
 data['T_cam3_velo'] = T3.dot(R_rect_00.dot(T_cam0unrect_velo))

这里 T_cam0unrect_velo 经过代码验证，发现就是下面的 ex_matrix

with open("/Users/john/Downloads/2011_09_26/calib_velo_to_cam.txt", "r") as f:
    lines = f.readlines()
    rvec = lines[1].strip().split(":")[-1].strip().split(" ")
    rvec = np.array(rvec, dtype="float32").reshape((3, 3))
    tvec = lines[2].strip().split(":")[-1].strip().split(" ")
    tvec = np.array(tvec, dtype="float32").reshape((3, 1))
ex_matrix = np.eye(4)
ex_matrix[:3, :3] = rvec
ex_matrix[:3, [3]] = tvec

这里的 R_rect_00 可以通过下面的代码读出来，确认和pykitti中的一样

with open("/Users/john/Downloads/2011_09_26/calib_cam_to_cam.txt", "r") as f:
    lines = f.readlines()
    R_rect_00 = lines[8].strip().split(":")[-1].strip().split(" ")
    R_rect_00 = np.array(R_rect_00, dtype="float32").reshape((3, 3))
    R_rect = np.eye(4)
    R_rect[:3,:3] = R_rect_00
    print("R_rect_00: ", R_rect_00)

相机的内参 P_rect_30 也确认了kitti里面和txt读出来的是对的

with open("/Users/john/Downloads/2011_09_26/calib_cam_to_cam.txt", "r") as f:
    lines = f.readlines()
 

    P_rect_03 = lines[-1].strip().split(":")[-1].strip().split(" ")
    P_rect_03 = np.array(P_rect_03, dtype="float32").reshape((3, 4))
 
    print("P_rect_03: ", P_rect_03)

这个和pykiit中的K_cam3 = dataset.calib.P_rect_30 是一样的.

这里的 T3是这么定义的

     	T0 = np.eye(4)
        T0[0, 3] = P_rect_00[0, 3] / P_rect_00[0, 0]
        T1 = np.eye(4)
        T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0]
        T2 = np.eye(4)
        T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0]
        T3 = np.eye(4)
        T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0]

打印下来之后发现是这样的

T0:  [[1. 0. 0. 0.]
 [0. 1. 0. 0.]
 [0. 0. 1. 0.]
 [0. 0. 0. 1.]]
T1:  [[ 1.          0.          0.         -0.53715059]
 [ 0.          1.          0.          0.        ]
 [ 0.          0.          1.          0.        ]
 [ 0.          0.          0.          1.        ]]
T2:  [[1.       0.       0.       0.062169]
 [0.       1.       0.       0.      ]
 [0.       0.       1.       0.      ]
 [0.       0.       0.       1.      ]]
T3:  [[ 1.          0.          0.         -0.47055642]
 [ 0.          1.          0.          0.        ]
 [ 0.          0.          1.          0.        ]
 [ 0.          0.          0.          1.        ]]

再来看一下上面那张图

会发现 明明 2号相机，在0号相机的左边0.06m,但是为啥T2[0,3]却是正的,而1,3号相机是在右边为何是负的，这和0号相机的坐标系刚好相反呀。

最初以为是pykitti里面搞错了.

当时是这么想的,
按照文档上来说，T2应该是相机cam0到cam2的pose信息. 那从cam0到cam2明明是发生了 (-0.06,0,0) 这么个平移，为啥T2里面却是正的.
其实这就是因为之前没有好好地想过这个问题. 就是关于pose和相机变换的一些关系.

实际上我理解就是区别两点，一个是点的移动，一个是相机自身的移动.

点的移动

假设 有一个点 $P_1$, 经过了旋转$R$，和平移$T$, 到达了 $P_2$ 这个点, 那么则有

$$P_2=R{P}_1+T,$$

其实这个式子成立是在同一个坐标系下成立的。 比如现在就以某个相机为坐标系，那么$P_1,P_2$ 其实都是相对于这个坐标系下的位置,不光如此，这里的平移和旋转 T也都是相对于这个坐标系的. 比如假设旋转矩阵为单位阵，$P_1=(0,0,0)$ 为坐标原点，那么 $T$ 其实就是 $P_2$ 相对于该坐标系下的位置. 这个时候

$$P_2=P_1+T,$$
就可以简单的理解为向量相加, 也可以理解为向量 $P_1$ 经过运动 $T$ 到达了向量 $P_2$.

我之前就是这么理解的，所以认为 cam2相对于cam0的T2里面应该是一个负的0.06.

但是其实就是因为搞混了.

坐标系的移动

现在不考虑同一个坐标系下的点的移动了，来考虑坐标系的移动.

即 有一个相机$P1$ 经过了旋转和平移到了相机$P2$, 这里把相机和其位置 对应了起来. 假设 世界坐标系下有一个点 $P$, 它在两个相机各自的坐标系下可以分别表示为

$$P_w=[e_1,e_2,e_3]\cdot P_{c1}$$
$$P_w=[a_1,a_2,a_3]\cdot P_{c2}$$
这里的 $P_{ci}$ 代表这个点在两个相机各自坐标系下的坐标.
从而

$$P_{c1}=K{P}_{c2}$$

这个式子的意思是说两个坐标系下的变换.
其实 $K$ 就是我们所说的pose信息.即两个坐标系下的变换.

再看这个公式

 data['T_cam2_velo'] = T2.dot(R_rect_00.dot(T_cam0unrect_velo))

的话，假设 R_rect_00, T_cam0unrect_velo 都是单位阵的话，以cam0为坐标系来看,点 $(0,0,0)$, 对于cam2来说确实就是 $(0.06,0,0)$.

也就是说 从A 经过旋转$R$和平移$T$到达了B，从pose的角度来看,即从A到B的pose变换矩阵中的$[R1,T1]$ 应该是 $[\ast ,-T]$, pose，即从A坐标系下来看，B的位置.
这里的 $\ast$, 我猜 是$R^{-1}$.

这些其实在SLam14讲的第三章的最后提了一下，当时没细看.