前言
相机位姿的下标定义方式通常被省略了,地图点在相机坐标系与世界坐标系之间的转换可以有两种表达方式。本文主要对这两个部分进梳理清,便于理解最基础的概念。
一、从点的下标定义到相机位姿的下标定义
相机c的光心点在世界坐标系下的位置(不考虑旋转,并且指的是相机的光心)坐标被定义为
p
w
c
∈
R
3
p_{w c} \in \mathbb{R}^{3}
pwc∈R3
它对应的是世界坐标系原点指向相机的光心的向量,并且是在世界坐标系下定义的向量。从这个定义可以看出
p
w
c
≠
−
p
c
w
p_{wc}\neq-p_{cw}\
pwc=−pcw
这是因为一个是在世界坐标系下的定义,一个是在相机坐标系下的定义,这两个坐标系之间可能会存在一个旋转的关系。
同理,一个地图点f在相机坐标系下的坐标下标为
p
w
f
∈
R
3
p_{w f} \in \mathbb{R}^{3}
pwf∈R3
很多时候为了方便,会省略点的下标定义,但我们依然需要心里有数。
相机的位姿通常指的是相机光心在世界坐标系下的位置和姿态,即
T
=
[
R
t
0
1
]
4
×
4
⇔
T
w
c
=
[
R
w
c
t
w
c
0
1
]
4
×
4
T=\begin{bmatrix}R&t\\\mathbf0&1\end{bmatrix}_{4\times4}\Leftrightarrow T_{wc}=\begin{bmatrix}R_{wc}&t_{wc}\\\mathbf0&1\end{bmatrix}_{4\times4}
T=[R0t1]4×4⇔Twc=[Rwc0twc1]4×4
当然了,这里的
R
w
c
R_{wc}
Rwc 的意思是从相机坐标系转换到世界坐标系的旋转过程,并且
R
w
c
=
(
R
c
w
)
−
1
=
(
R
c
w
)
T
R_{wc}=(R_{cw})^{-1}=(R_{cw})^{T}
Rwc=(Rcw)−1=(Rcw)T
还有一个需要注意的一点,既然
p
w
c
≠
−
p
c
w
p_{wc}\neq-p_{cw}
pwc=−pcw不成立,那怎么去获得
p
c
w
p_{cw}
pcw 呢?其实可以整体对转换矩阵 T 求逆再提取即可。这里求逆的时候也不要傻乎乎的直接对整个矩阵求逆,可以通过下面的公式获得
T
−
1
=
(
R
T
−
R
T
⋅
t
0
1
)
T^{-1}=\begin{pmatrix} R^{T}&- R^{T}\cdot t\\\mathbf0&1\end{pmatrix}
T−1=(RT0−RT⋅t1)
证明如下:
对于一个 4x4 的齐次变换矩阵 T 和它的逆矩阵
T
−
1
T^{-1}
T−1 满足以下关系
T
⋅
T
−
1
=
I
4
×
4
{T} \cdot {T}^{-1}={I}_{4 \times 4}
T⋅T−1=I4×4
假设
T
−
1
T^{-1}
T−1 有一个新的形式,则我们只需求解
R
n
e
w
R_{new}
Rnew 和
t
n
e
w
t_{new}
tnew
T
−
1
=
(
R
new
t
new
0
1
)
{T}^{-1}=\left(\begin{array}{cc} {R}_{\text {new }} & {t}_{\text {new }} \\ \mathbf{0} & 1 \end{array}\right)
T−1=(Rnew 0tnew 1)
下面开始丝滑推导
T
⋅
T
−
1
=
(
R
t
0
1
)
⋅
(
R
new
t
new
0
1
)
=
(
R
⋅
R
new
R
⋅
t
new
+
t
0
1
)
{T} \cdot {T}^{-1}=\left(\begin{array}{cc} {R} & {t} \\ \mathbf{0} & 1 \end{array}\right) \cdot\left(\begin{array}{cc} {R}_{\text {new }} & {t}_{\text {new }} \\ {0} & 1 \end{array}\right)=\left(\begin{array}{cc} {R} \cdot {R}_{\text {new }} & {R} \cdot {t}_{\text {new }}+{t} \\ \mathbf{0} & 1 \end{array}\right)
T⋅T−1=(R0t1)⋅(Rnew 0tnew 1)=(R⋅Rnew 0R⋅tnew +t1)
我们只需要求解这个方程即可
R
⋅
R
new
=
I
3
×
3
R
⋅
t
new
+
t
=
0
\begin{array}{l} {R} \cdot {R}_{\text {new }}=\mathbf{I}_{3 \times 3} \\ {R} \cdot {t}_{\text {new }}+{t}=\mathbf{0} \end{array}
R⋅Rnew =I3×3R⋅tnew +t=0
最后自然得到
R
n
e
w
=
R
−
1
=
R
T
t
n
e
w
=
−
R
T
t
R_{new}=R^{-1}=R^{T} \\ t_{new}=-R^{T} t
Rnew=R−1=RTtnew=−RTt
二、地图点从世界坐标系到相机坐标系的两种转换过程
假设在某时刻,相机 c 观测到了某地图点 f ,其中相机在世界坐标系下的位置为 p w c p_{wc} pwc,姿态为 R w c R_{wc} Rwc,地图点在世界坐标系下的表示为 p w f p_{wf} pwf,则地图点在相机坐标系下的表示可以有两种,分别是先平移再旋转或者先旋转再平移。
但通过下面的推导可以发现,其实是互通的,区别在于一个用了
p
w
c
p_{wc}
pwc,一个用了
p
c
w
p_{cw}
pcw。由于这个代表着相机光心与世界坐标系原点的距离,也可以写作我们熟知的
t
w
c
t_{wc}
twc 和
t
w
c
t_{wc}
twc
p
c
f
=
R
w
c
T
(
p
w
f
−
p
w
c
)
=
R
c
w
p
w
f
+
(
−
R
w
c
T
p
w
c
)
=
R
c
w
p
w
f
+
p
c
w
p_{c f}=R_{w c}{ }^{T}\left(p_{w f}-p_{w c}\right)=R_{c w} p_{w f}+\left(-R_{w c}{ }^{T} p_{w c}\right)=R_{c w} p_{w f}+p_{c w}
pcf=RwcT(pwf−pwc)=Rcwpwf+(−RwcTpwc)=Rcwpwf+pcw