1. 基本要素
O表示物体坐标系,C表示相机坐标系,W表示世界坐标系;
下标1表示时刻1,下标2表示时刻2;
2. 同一时刻内的关系
对于object上一个点,有下图关系;
①其在物体坐标系O到相机坐标系C的坐标变换表示物体
O
O
O相对于相机
C
C
C的位姿,记为
L
c
o
L_{co}
Lco,用绿色线表示;
该位姿往往是多目标追踪方法中给出的物体位姿形式;
②其在相机坐标系C到世界坐标系W的坐标变换表示相机的位姿,记为
T
w
c
T_{wc}
Twc,用绿色线表示;
该位姿往往是SLAM算法中给出的传感器位姿形式;
③此时有在物体坐标系O到世界坐标系W的坐标变换表示物体
O
O
O相对于世界坐标系
W
W
W的位姿,记为
L
w
o
L_{wo}
Lwo,如下图所示,用蓝色线表示
有
L
w
o
=
T
w
c
∗
L
c
o
L_{wo} = T_{wc}*L_{co}
Lwo=Twc∗Lco
3. 相邻时刻间的关系
此时定义两个变换
3.1 H m o t i o n H_{motion} Hmotion
点在时刻2物体坐标系下到时刻1物体坐标系的坐标变换,表示
O
1
O_1
O1相对于
O
2
O_2
O2的位姿变换,记为
H
m
o
t
i
o
n
H_{motion}
Hmotion,用玫红色线表示
有
H
m
o
t
i
o
n
=
L
w
o
1
−
1
∗
L
w
o
2
H_{motion} = {L_{wo_1}}^{-1}*{L_{wo_2}}
Hmotion=Lwo1−1∗Lwo2,其反应了目标坐标系
O
O
O在两个时刻之间的变化;
3.2 H p o i n t H_{point} Hpoint
点在世界坐标系下,在时刻1的坐标到时刻2的坐标的坐标变换,表示一个点在两个时刻的之间的运动,记为
H
p
o
i
n
t
H_{point}
Hpoint,用黄色线表示
有
H
p
o
i
n
t
=
L
w
o
2
∗
L
w
o
1
−
1
H_{point} = L_{wo2}*{L_{wo_1}}^{-1}
Hpoint=Lwo2∗Lwo1−1,其中
①
L
w
o
1
−
1
{L_{wo_1}}^{-1}
Lwo1−1表示黄色点在时刻1下世界坐标系
W
W
W转换到物体坐标系
O
1
O_{1}
O1;
②由于刚体假设其在物体坐标系
O
1
O_{1}
O1+时刻1的坐标 = 其在物体坐标系
O
2
O_{2}
O2+时刻2的坐标;
③此时通过
L
w
o
2
L_{wo2}
Lwo2就再次将时刻2下物体坐标系
O
2
O_{2}
O2转换到世界坐标系
W
W
W;
3.3 两个H间关系
根据两个H的表达式:
H
p
o
i
n
t
=
L
w
o
2
∗
L
w
o
1
−
1
=
(
T
w
c
2
∗
L
c
o
2
)
∗
(
T
w
c
1
∗
L
c
o
1
)
−
1
H_{point} = L_{wo2}*{L_{wo_1}}^{-1}= (T_{wc_2}*L_{co_2})*(T_{wc_1}*L_{co_1})^{-1}
Hpoint=Lwo2∗Lwo1−1=(Twc2∗Lco2)∗(Twc1∗Lco1)−1
H
m
o
t
i
o
n
=
L
w
o
1
−
1
∗
L
w
o
2
=
(
T
w
c
1
∗
L
c
o
1
)
−
1
∗
(
T
w
c
2
∗
L
c
o
2
)
H_{motion} = {L_{wo_1}}^{-1}*{L_{wo_2}}=(T_{wc_1}*L_{co_1})^{-1}*(T_{wc_2}*L_{co_2})
Hmotion=Lwo1−1∗Lwo2=(Twc1∗Lco1)−1∗(Twc2∗Lco2)
可以很容易地看出他们的关系为,两个H变换可以通过两个时刻的物体位姿进行转换:
H
m
o
t
i
o
n
=
L
w
o
1
−
1
∗
H
p
o
i
n
t
∗
L
w
o
1
=
L
w
o
2
−
1
∗
H
p
o
i
n
t
∗
L
w
o
2
H_{motion} = L_{wo1}^{-1}*H_{point}*L_{wo1} = L_{wo2}^{-1}*H_{point}*L_{wo2}
Hmotion=Lwo1−1∗Hpoint∗Lwo1=Lwo2−1∗Hpoint∗Lwo2
H
p
o
i
n
t
=
L
w
o
1
∗
H
m
o
t
i
o
n
∗
L
w
o
1
−
1
=
L
w
o
2
∗
H
m
o
t
i
o
n
∗
L
w
o
2
−
1
H_{point} = L_{wo1}*H_{motion}*L_{wo1}^{-1} = L_{wo2}*H_{motion}*L_{wo2}^{-1}
Hpoint=Lwo1∗Hmotion∗Lwo1−1=Lwo2∗Hmotion∗Lwo2−1
注意第一个式子,对于两个时刻间坐标系变换
H
m
o
t
i
o
n
H_{motion}
Hmotion,由于时刻1的物体位姿
L
w
o
1
L_{wo1}
Lwo1已经求取,可由点在世界坐标系下的坐标变换
H
p
o
i
n
t
H_{point}
Hpoint表出;
4. H矩阵求取
若不考虑目标的运动,直接利用目标上的匹配点求解位姿,可得到一个虚拟的相机位姿
C
2
′
C_2{'}
C2′,不难看出
C
2
C_2
C2与
C
2
′
C_2{'}
C2′的相对位姿和
O
2
O_2
O2与
O
1
O_1
O1的相对位姿是一样的;
根据前文已经知道
H
m
o
t
i
o
n
H_{motion}
Hmotion表示的是2时刻坐标系相对于1时刻坐标系的位姿;那么就可以通过两个相机位姿得到
H
m
o
t
i
o
n
=
T
w
c
2
′
−
1
∗
T
w
c
2
H_{motion} = T_{wc_2'}^{-1}*T_{wc2}
Hmotion=Twc2′−1∗Twc2
H
m
o
t
i
o
n
H_{motion}
Hmotion矩阵的求解如下图所示:
同理
H
p
o
i
n
t
H_{point}
Hpoint表示世界坐标系下点坐标随着时间的变换,有
H
p
o
i
n
t
=
T
w
c
2
∗
T
w
c
2
′
−
1
H_{point} = T_{wc2}*T_{wc_2'}^{-1}
Hpoint=Twc2∗Twc2′−1