一 背景概述
在SLAM和自动驾驶中经常遇到如下公式,知道特征集合P’和特征集合P,如何求解R、t。
P
′
=
R
∗
P
+
t
P' = R*P+t
P′=R∗P+t
其中R是车辆姿态,t是车辆位置。这里就是要求出特征投影误差对于姿态的雅克比矩阵。求雅克比矩阵时,姿态使用等效旋转向量V表达。
二 解决思路
2.1 左扰动模型
参见SLAM14讲4.3.4.
d
(
R
∗
P
)
d
V
=
−
(
R
∗
P
)
ˆ
\frac {d(R*P)}{d V} = -(R*P)\^\
dVd(R∗P)=−(R∗P) ˆ
2.2 右扰动模型
参见SLAM14讲4.3.4.
d
(
R
∗
P
)
d
V
=
−
R
∗
P
ˆ
\frac {d(R*P)}{d V} = -R*P\^\
dVd(R∗P)=−R∗P ˆ
2.3 直接dR_dV
将R由3行3列,变为9行1列,则dR_dV 是9行3列,每一列就是R对V的某一维度求偏导数,后续运算时将dR_dV的一列变成3行3列。
位置误差对姿态向量求雅克比矩阵时,分别单独对姿态向量的某一维度求偏导数,然后合成雅克比,那么求雅克比的关键就是求出dR_dV。
J
=
[
d
R
_
d
V
(
:
,
1
)
3
×
3
∗
P
d
R
_
d
V
(
:
,
2
)
3
×
3
∗
P
d
R
_
d
V
(
:
,
3
)
3
×
3
∗
P
]
J = \begin{bmatrix} dR\_dV(:,1)_{3 \times 3}*P \quad dR\_dV(:,2)_{3 \times 3}*P \quad dR\_dV(:,3)_{3 \times 3}*P \end{bmatrix}
J=[dR_dV(:,1)3×3∗PdR_dV(:,2)3×3∗PdR_dV(:,3)3×3∗P]
J是3行3列。那么如何求dR_dV 9行3列呢?
V
=
[
v
1
v
2
v
3
]
V = [v1 \quad v2 \quad v3]
V=[v1v2v3]
模是a,构造如下向量:
V
2
=
[
v
1
v
2
v
3
a
]
V2 = [v1 \quad v2 \quad v3 \quad a]
V2=[v1v2v3a]
单位化向量V,组成向量n2。
n
2
=
[
n
1
n
2
n
3
a
]
n
=
[
n
1
n
2
n
3
]
n 2= [n1 \quad n2 \quad n3 \quad a]\newline n = [n1 \quad n2 \quad n3]
n2=[n1n2n3a]n=[n1n2n3]
构造中间向量 nnT 和 n^成9维列向量
m
1
=
[
c
o
s
(
a
)
1
−
c
o
s
(
a
)
s
i
n
(
a
)
]
′
m
2
=
[
m
1
n
n
T
n
ˆ
]
m1 = [cos(a) \quad 1-cos(a) \quad sin(a)]' \newline m2 = [m1 \quad nn^{T} \quad n\^\ ]
m1=[cos(a)1−cos(a)sin(a)]′m2=[m1nnTn ˆ]
根据罗德里格斯公式:
R
=
c
o
s
(
a
)
∗
I
+
(
1
−
c
o
s
(
a
)
)
∗
n
n
T
+
s
i
n
(
a
)
∗
n
ˆ
R = cos(a)*I +(1-cos(a))*nn^{T}+sin(a)*n\^\
R=cos(a)∗I+(1−cos(a))∗nnT+sin(a)∗n ˆ
将上式右边三项均由33换成91,因此根据求雅克比的链式法则
d
R
d
V
=
d
R
d
m
2
∗
d
m
2
d
n
2
∗
d
n
2
d
V
2
∗
V
2
d
V
\frac{dR}{dV}=\frac{dR}{dm2}*\frac{dm2}{dn2}*\frac{dn2}{dV2}*\frac{V2}{dV}
dVdR=dm2dR∗dn2dm2∗dV2dn2∗dVV2
R 大小是 9行1列
m2 大小是 21行1列
n2 大小是 4行1列
V2 大小是 4行1列
V 大小是 3行1列
复杂雅克比求解总结:
- 矩阵求导复杂时,将矩阵转成单维列向量,然后求解求导。
- 复杂的公式求解雅克比,充分利用链式法则,生成中间向量求解雅克比。