之前从未仔细想过到底相机的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 P_1 P1, 经过了旋转 R R R,和平移 T T T, 到达了 P 2 P_2 P2 这个点, 那么则有
P 2 = R P 1 + T , P_2= RP_1+T, P2=RP1+T,
其实这个式子成立是在同一个坐标系下成立的。 比如现在就以某个相机为坐标系,那么 P 1 , P 2 P_1, P_2 P1,P2 其实都是相对于这个坐标系下的位置,不光如此,这里的平移和旋转 T也都是相对于这个坐标系的. 比如假设旋转矩阵为单位阵, P 1 = ( 0 , 0 , 0 ) P_1=(0,0,0) P1=(0,0,0) 为坐标原点,那么 T T T 其实就是 P 2 P_2 P2 相对于该坐标系下的位置. 这个时候
P
2
=
P
1
+
T
,
P_2 = P_1+T,
P2=P1+T,
就可以简单的理解为向量相加, 也可以理解为向量
P
1
P_1
P1 经过运动
T
T
T 到达了向量
P
2
P_2
P2.
我之前就是这么理解的,所以认为 cam2相对于cam0的T2里面应该是一个负的0.06.
但是其实就是因为搞混了.
坐标系的移动
现在不考虑同一个坐标系下的点的移动了,来考虑坐标系的移动.
即 有一个相机 P 1 P1 P1 经过了旋转和平移到了相机 P 2 P2 P2, 这里把相机和其位置 对应了起来. 假设 世界坐标系下有一个点 P P P, 它在两个相机各自的坐标系下可以分别表示为
P
w
=
[
e
1
,
e
2
,
e
3
]
⋅
P
c
1
P_w= [e_1, e_2, e_3] \cdot P_{c1}
Pw=[e1,e2,e3]⋅Pc1
P
w
=
[
a
1
,
a
2
,
a
3
]
⋅
P
c
2
P_w=[a_1, a_2, a_3]\cdot P_{c2}
Pw=[a1,a2,a3]⋅Pc2
这里的
P
c
i
P_{ci}
Pci 代表这个点在两个相机各自坐标系下的坐标.
从而
P c 1 = K P c 2 P_{c1} = K P_{c2} Pc1=KPc2
这个式子的意思是说两个坐标系下的变换.
其实
K
K
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
)
(0,0,0)
(0,0,0), 对于cam2来说确实就是
(
0.06
,
0
,
0
)
(0.06,0,0)
(0.06,0,0).
也就是说 从A 经过旋转
R
R
R和平移
T
T
T到达了B,从pose的角度来看,即从A到B的pose变换矩阵中的
[
R
1
,
T
1
]
[R1,T1]
[R1,T1] 应该是
[
∗
,
−
T
]
[*, -T]
[∗,−T], pose,即从A坐标系下来看,B的位置.
这里的
∗
*
∗, 我猜 是
R
−
1
R^{-1}
R−1.
这些其实在SLam14讲的第三章的最后提了一下,当时没细看.