Slam学习笔记——矩阵扰动求导
参考书籍：《视觉SLAM十四讲-从理论到实践》——高翔
1. 推导结论
1.1 旋转矩阵R
旋转矩阵的逆等于转置：
R
T
=
R
−
1
R^T = R^{-1}
RT=R−1
旋转矩阵
R
R
R的左扰动：
−
(
R
p
)
∧
-(Rp)^{\wedge}
−(Rp)∧
旋转矩阵
R
R
R的右扰动：
−
R
p
∧
-Rp^{\wedge}
−Rp∧
逆旋转矩阵
R
−
1
R^{-1}
R−1的左扰动：
R
−
1
p
∧
R^{-1}p^{\wedge}
R−1p∧
逆旋转矩阵
R
−
1
R^{-1}
R−1的右扰动：
(
R
−
1
p
)
∧
(R^{-1}p)^{\wedge}
(R−1p)∧
1.2 变换矩阵T
1.2.1 结构
变换矩阵T：
[
R
t
0
T
1
]
\left[ \begin{matrix} R & t \\ 0^T & 1 \end{matrix} \right]
[R0Tt1]
逆变换矩阵
T
−
1
T^{-1}
T−1：
[
R
T
−
R
T
t
0
T
1
]
\left[ \begin{matrix} R^T & -R^Tt \\ 0^T & 1 \end{matrix} \right]
[RT0T−RTt1]
1.2.2 左扰动
与《十四讲》不同！本文涉及的扰动是对矩阵求导，而不是对李代数求导！
因此，得到的答案会有所不同！
变换矩阵T的左扰动：
[
I
−
(
R
p
)
∧
0
T
0
T
]
\left[ \begin{matrix} I & -(Rp)^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
[I0T−(Rp)∧0T]
逆变换矩阵
T
−
1
T^{-1}
T−1的左扰动：
[
−
R
T
R
T
p
∧
−
R
T
t
∧
0
T
0
T
]
\left[ \begin{matrix} -R^T & R^Tp^{\wedge}-R^Tt^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
[−RT0TRTp∧−RTt∧0T]
1.2.3 右扰动
变换矩阵T的右扰动：
[
I
−
R
p
∧
0
T
0
T
]
\left[ \begin{matrix} I & -Rp^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
[I0T−Rp∧0T]
逆变换矩阵
T
−
1
T^{-1}
T−1的右扰动：
[
−
R
T
(
R
T
p
)
∧
−
(
R
T
t
)
∧
0
T
0
T
]
\left[ \begin{matrix} -R^T & (R^Tp)^{\wedge}-(R^Tt)^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
[−RT0T(RTp)∧−(RTt)∧0T]
1.3 反对称矩阵a^
a
=
[
x
,
y
,
z
]
T
a = [x, y, z]^T
a=[x,y,z]T
则反对称矩阵
a
∧
a^{\wedge}
a∧=
[
0
−
z
y
z
0
−
x
−
y
x
0
]
\left[ \begin{matrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{matrix} \right]
⎣⎡0z−y−z0xy−x0⎦⎤
1.4 内参矩阵K
使用内参矩阵K将相机平面上的2维坐标
(
u
,
v
)
(u, v)
(u,v)与世界坐标系下的3维坐标
(
X
,
Y
,
Z
)
(X, Y, Z)
(X,Y,Z)相互转换：
[
u
v
1
]
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
=
1
Z
[
X
Y
Z
]
\left[ \begin{matrix} u \\ v \\ 1 \end{matrix} \right] \left[ \begin{matrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{matrix} \right] = \frac {1} {Z} \left[ \begin{matrix} X \\ Y \\ Z \end{matrix} \right]
⎣⎡uv1⎦⎤⎣⎡fx000fy0cxcy1⎦⎤=Z1⎣⎡XYZ⎦⎤
Z
Z
Z为深度，经常使用逆深度
λ
λ
λ代替
Z
Z
Z，即写为：
[
u
v
1
]
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
=
λ
[
X
Y
Z
]
=
[
λ
X
λ
Y
1
]
\left[ \begin{matrix} u \\ v \\ 1 \end{matrix} \right] \left[ \begin{matrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{matrix} \right] =λ \left[ \begin{matrix} X \\ Y \\ Z \end{matrix} \right]=\left[ \begin{matrix} λX \\ λY \\ 1 \end{matrix} \right]
⎣⎡uv1⎦⎤⎣⎡fx000fy0cxcy1⎦⎤=λ⎣⎡XYZ⎦⎤=⎣⎡λXλY1⎦⎤
λ
λ
λ的优点：
1、在表示较远处物体时，计算精度更好
2、假设2维点正确的情况下，只需要优化
λ
λ
λ这1个参数即可以确定3维点坐标。
2. 推导过程
T b a T^a_b Tba表示将坐标从b坐标系转换到a坐标系。
2.1 推导中用到的公式
旋转矩阵的逆等于转置： R T = R − 1 R^T = R^{-1} RT=R−1
全微分公式：
当f(x) = f(a(x), b(x))时：
∂
f
∂
x
=
∂
f
∂
a
∂
a
∂
x
+
∂
f
∂
b
∂
b
∂
x
\frac {∂f} {∂x } =\frac {∂f} {∂a }\frac {∂a} {∂x }+\frac {∂f} {∂b }\frac {∂b} {∂x }
∂x∂f=∂a∂f∂x∂a+∂b∂f∂x∂b
李代数的泰勒展开（近似）：
e
x
p
(
Δ
x
)
=
(
I
+
Δ
x
∧
)
exp(Δx) = (I + Δx^{\wedge})
exp(Δx)=(I+Δx∧)
e
x
p
(
−
Δ
x
)
=
(
I
−
Δ
x
∧
)
exp(-Δx) = (I - Δx^{\wedge})
exp(−Δx)=(I−Δx∧)
交换反对称符号（交换位置且改变正负）：
Δ
x
∧
p
=
−
p
∧
Δ
x
Δx^{\wedge}p = -p^{\wedge}Δx
Δx∧p=−p∧Δx
Δ
x
∧
R
p
=
−
(
R
p
)
∧
Δ
x
Δx^{\wedge}Rp = -(Rp)^{\wedge}Δx
Δx∧Rp=−(Rp)∧Δx
向量叉乘交换律：
a X b = - b X a
基于反对称的定义可得：
(
a
+
b
)
∧
=
a
∧
+
b
∧
(a+b)^{\wedge}=a^{\wedge}+b^{\wedge}
(a+b)∧=a∧+b∧
2.2 详细推导
2.2.1 旋转矩阵的右扰动
f ( x + Δ x ) − f ( x ) Δ x = [ e x p ( x ∧ ) e x p ( Δ x ∧ ) ] − 1 p − e x p ( x ) − 1 p Δ x (代入) \frac {f(x+Δx)-f(x)} {Δx} =\frac {[exp(x^{\wedge})exp(Δx^{\wedge})]^{-1}p-exp(x)^{-1}p} {Δx} \tag{代入} Δxf(x+Δx)−f(x)=Δx[exp(x∧)exp(Δx∧)]−1p−exp(x)−1p(代入)
f ( x + Δ x ) − f ( x ) Δ x = e x p ( Δ x ∧ ) − 1 e x p ( x ∧ ) − 1 p − e x p ( x ∧ ) − 1 p Δ x (分解后交换) \frac {f(x+Δx)-f(x)} {Δx} =\frac {exp(Δx^{\wedge})^{-1}exp(x^{\wedge})^{-1}p-exp(x^{\wedge})^{-1}p} {Δx}\tag{分解后交换} Δxf(x+Δx)−f(x)=Δxexp(Δx∧)−1exp(x∧)−1p−exp(x∧)−1p(分解后交换)
f ( x + Δ x ) − f ( x ) Δ x = ( I − Δ x ∧ ) e x p ( x ∧ ) − 1 p − e x p ( x ∧ ) − 1 p Δ x (泰勒展开) \frac {f(x+Δx)-f(x)} {Δx} =\frac {(I-Δx^{\wedge})exp(x^{\wedge})^{-1}p-exp(x^{\wedge})^{-1}p} {Δx}\tag{泰勒展开} Δxf(x+Δx)−f(x)=Δx(I−Δx∧)exp(x∧)−1p−exp(x∧)−1p(泰勒展开)
f ( x + Δ x ) − f ( x ) Δ x = − Δ x ∧ ∗ e x p ( x ∧ ) − 1 p Δ x (相减) \frac {f(x+Δx)-f(x)} {Δx} =\frac {-Δx^{\wedge}*exp(x^{\wedge})^{-1}p} {Δx}\tag{相减} Δxf(x+Δx)−f(x)=Δx−Δx∧∗exp(x∧)−1p(相减)
f ( x + Δ x ) − f ( x ) Δ x = ( e x p ( x ) − 1 p ) ∧ Δ x Δ x (反对称交换) \frac {f(x+Δx)-f(x)} {Δx} =\frac {(exp(x)^{-1}p)^{\wedge}Δx} {Δx}\tag{反对称交换} Δxf(x+Δx)−f(x)=Δx(exp(x)−1p)∧Δx(反对称交换)
f ( x + Δ x ) − f ( x ) Δ x = ( e x p ( x ) − 1 p ) ∧ = ( R − 1 p ) ∧ (消去Δx) \frac {f(x+Δx)-f(x)} {Δx} =(exp(x)^{-1}p)^{\wedge} = (R^{-1}p)^{\wedge}\tag{消去Δx} Δxf(x+Δx)−f(x)=(exp(x)−1p)∧=(R−1p)∧(消去Δx)
2.2.2 变换矩阵的右扰动
已知：
旋转矩阵
R
R
R的右扰动：
−
R
p
∧
-Rp^{\wedge}
−Rp∧
逆旋转矩阵
R
−
1
R^{-1}
R−1的右扰动：
(
R
−
1
p
)
∧
(R^{-1}p)^{\wedge}
(R−1p)∧
对于变换矩阵T：
Tp = Rp + t
因此：
1、t的偏导为I
2、R的偏导为Rp的右扰动，即
−
R
p
∧
-Rp^{\wedge}
−Rp∧
得到变换矩阵T的右扰动：
[
I
−
R
p
∧
0
T
0
T
]
\left[ \begin{matrix} I & -Rp^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
[I0T−Rp∧0T]
对于逆变换矩阵
T
−
1
T^{-1}
T−1：
T
p
=
R
−
1
(
p
−
t
)
=
R
−
1
p
−
R
−
1
t
Tp = R^{-1}(p - t) = R^{-1}p - R^{-1}t
Tp=R−1(p−t)=R−1p−R−1t
因此：
1、t的偏导为
−
R
−
1
- R^{-1}
−R−1
2、R的偏导为
R
−
1
p
R^{-1}p
R−1p的右扰动减
R
−
1
t
R^{-1}t
R−1t的右扰动，即
(
R
T
p
)
∧
−
(
R
T
t
)
∧
(R^Tp)^{\wedge}-(R^Tt)^{\wedge}
(RTp)∧−(RTt)∧
得到逆变换矩阵
T
−
1
T^{-1}
T−1的右扰动：
[
−
R
T
(
R
T
p
)
∧
−
(
R
T
t
)
∧
0
T
0
T
]
\left[ \begin{matrix} -R^T & (R^Tp)^{\wedge}-(R^Tt)^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
[−RT0T(RTp)∧−(RTt)∧0T]
2.2.3 Vins-Mono中重投影误差的雅可比矩阵求导
源代码位置：/VINS-Mono/vins_estimator/src/factor/projection_factor.cpp
函数：ProjectionFactor::Evaluate
原理：第i帧的图像通过位姿变换后得到第j帧对应点在相机坐标系下的坐标，然后与实际观测到的坐标相减，通过最小化重投影误差来优化位姿的参数。
使用右扰动的方式计算变换矩阵的偏导。
误差计算公式：
e
=
T
c
i
m
u
−
1
T
i
m
u
j
w
−
1
T
i
m
u
i
w
T
c
i
m
u
P
c
i
−
P
j
e = T^{imu -1}_c T^{w -1}_{imuj} T^w_{imui} T^{imu}_c P_{ci} - P_j
e=Tcimu−1Timujw−1TimuiwTcimuPci−Pj
1、对
T
i
m
u
i
w
T^w_{imui}
Timuiw求偏导
∂
e
∂
P
w
∂
P
w
∂
T
i
m
u
i
w
\frac {∂e} {∂P_w} \frac {∂P_w} {∂T^w_{imui}}
∂Pw∂e∂Timuiw∂Pw
= ( T c i m u − 1 T i m u j w − 1 ) [ I − R i m u i w P i m u i ^ 0 T 0 T ] =(T^{imu -1}_c T^{w -1}_{imuj}) \left[ \begin{matrix} I & -R^w_{imui}P_{imui}\hat{} \\ 0^T & 0^T \end{matrix} \right] =(Tcimu−1Timujw−1)[I0T−RimuiwPimui^0T]
= ( [ R c i m u − 1 − R c i m u − 1 t c i m u 0 T 1 ] [ R i m u j w − 1 − R i m u j w − 1 t i m u j w 0 T 1 ] ) [ I − R i m u i w P i m u i ∧ 0 T 0 T ] =( \left[ \begin{matrix} R^{imu-1}_{c} & -R^{imu-1}_{c}t^{imu}_{c} \\ 0^T & 1 \end{matrix} \right] \left[ \begin{matrix} R^{w-1}_{imuj} & -R^{w-1}_{imuj}t^w_{imuj} \\ 0^T & 1 \end{matrix} \right]) \left[ \begin{matrix} I & -R^w_{imui}P_{imui}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] =([Rcimu−10T−Rcimu−1tcimu1][Rimujw−10T−Rimujw−1timujw1])[I0T−RimuiwPimui∧0T]
= [ R c i m u − 1 R i m u j w − 1 R c i m u − 1 ( − R i m u j w − 1 t i m u j w ) − R c i m u − 1 t c i m u 0 T 1 ] [ I − R i m u i w P i m u i ∧ 0 T 0 T ] = \left[ \begin{matrix} R^{imu-1}_{c}R^{w-1}_{imuj} & R^{imu-1}_{c}(-R^{w-1}_{imuj}t^w_{imuj})-R^{imu-1}_{c}t^{imu}_{c} \\ 0^T & 1 \end{matrix} \right] \left[ \begin{matrix} I & -R^w_{imui}P_{imui}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] =[Rcimu−1Rimujw−10TRcimu−1(−Rimujw−1timujw)−Rcimu−1tcimu1][I0T−RimuiwPimui∧0T]
= [ R c i m u − 1 R i m u j w − 1 R c i m u − 1 R i m u j w − 1 ( − R i m u i w P i m u i ∧ ) 0 T 0 T ] = \left[ \begin{matrix} R^{imu-1}_{c}R^{w-1}_{imuj} & R^{imu-1}_{c}R^{w-1}_{imuj}(-R^w_{imui}P_{imui}^{\wedge})\\ 0^T & 0^T \end{matrix} \right] =[Rcimu−1Rimujw−10TRcimu−1Rimujw−1(−RimuiwPimui∧)0T]
= [ R c i m u − 1 R i m u j w − 1 R c i m u − 1 R i m u j w − 1 R i m u i w ( − P i m u i ∧ ) 0 T 0 T ] = \left[ \begin{matrix} R^{imu-1}_{c}R^{w-1}_{imuj} & R^{imu-1}_{c}R^{w-1}_{imuj}R^w_{imui}(-P_{imui}^{\wedge})\\ 0^T & 0^T \end{matrix} \right] =[Rcimu−1Rimujw−10TRcimu−1Rimujw−1Rimuiw(−Pimui∧)0T]
2、对 T i m u j w T^w_{imuj} Timujw求偏导
∂ e ∂ P i m u j ∂ P i m u j ∂ T i m u j w \frac {∂e} {∂P_{imuj}} \frac {∂P_{imuj}} {∂T^w_{imuj}} ∂Pimuj∂e∂Timujw∂Pimuj
= T c i m u − 1 [ − R i m u j w − 1 ( R i m u j w − 1 P w ) ∧ − ( R i m u j w − 1 t i m u j w ) ∧ 0 T 0 T ] =T^{imu -1}_c \left[ \begin{matrix} -R^{w -1}_{imuj} & (R^{w -1}_{imuj}P_{w})^{\wedge}-(R^{w -1}_{imuj}t^{w}_{imuj})^{\wedge} \\ 0^T & 0^T \end{matrix} \right] =Tcimu−1[−Rimujw−10T(Rimujw−1Pw)∧−(Rimujw−1timujw)∧0T]
= [ R c i m u − 1 − R c i m u − 1 t c i m u 0 T 1 ] [ − R i m u j w − 1 ( R i m u j w − 1 ( P w − t i m u j w ) ) ∧ 0 T 0 T ] =\left[ \begin{matrix} R^{imu -1}_{c} & -R^{imu -1}_{c}t^{imu}_{c} \\ 0^T & 1 \end{matrix} \right] \left[ \begin{matrix} -R^{w -1}_{imuj} & (R^{w -1}_{imuj}(P_{w}-t^{w}_{imuj}))^{\wedge} \\ 0^T & 0^T \end{matrix} \right] =[Rcimu−10T−Rcimu−1tcimu1][−Rimujw−10T(Rimujw−1(Pw−timujw))∧0T]
= [ R c i m u − 1 − R c i m u − 1 t c i m u 0 T 1 ] [ − R i m u j w − 1 P i m u j ∧ 0 T 0 T ] =\left[ \begin{matrix} R^{imu -1}_{c} & -R^{imu -1}_{c}t^{imu}_{c} \\ 0^T & 1 \end{matrix} \right] \left[ \begin{matrix} -R^{w -1}_{imuj} & P_{imuj}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] =[Rcimu−10T−Rcimu−1tcimu1][−Rimujw−10TPimuj∧0T]
= [ R c i m u − 1 ( − R i m u j w − 1 ) R c i m u − 1 P i m u j ∧ 0 T 0 T ] =\left[ \begin{matrix} R^{imu -1}_{c}(-R^{w -1}_{imuj}) & R^{imu -1}_{c}P_{imuj}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] =[Rcimu−1(−Rimujw−1)0TRcimu−1Pimuj∧0T]
3、对 T c i m u T^{imu}_c Tcimu求偏导
已知：
e
=
T
c
i
m
u
−
1
T
i
m
u
j
w
−
1
T
i
m
u
i
w
T
c
i
m
u
P
c
i
−
P
j
e = T^{imu -1}_c T^{w -1}_{imuj} T^w_{imui} T^{imu}_c P_{ci} - P_j
e=Tcimu−1Timujw−1TimuiwTcimuPci−Pj
P c j = T c i m u − 1 T i m u j w − 1 T i m u i w T c i m u P c i P_{cj} = T^{imu -1}_c T^{w -1}_{imuj} T^w_{imui} T^{imu}_c P_{ci} Pcj=Tcimu−1Timujw−1TimuiwTcimuPci
P i m u j = T i m u j w − 1 T i m u i w T c i m u P c i P_{imuj} = T^{w -1}_{imuj} T^w_{imui} T^{imu}_c P_{ci} Pimuj=Timujw−1TimuiwTcimuPci
P i m u i = T c i m u P c i P_{imui} = T^{imu}_c P_{ci} Pimui=TcimuPci
由全微分公式可得：
∂
e
∂
T
c
i
m
u
=
∂
P
c
j
∂
T
c
i
m
u
=
∂
P
c
j
∂
T
c
i
m
u
−
1
∂
T
c
i
m
u
−
1
∂
T
c
i
m
u
+
∂
P
c
j
∂
P
i
m
u
j
∂
P
i
m
u
j
∂
T
c
i
m
u
\frac {∂e} {∂T^{imu}_c} = \frac {∂P_{cj}} {∂T^{imu}_c} = \frac {∂P_{cj}} {∂T^{imu -1}_c}\frac {∂T^{imu -1}_c} {∂T^{imu}_c} +\frac {∂P_{cj}} {∂P_{imuj}}\frac {∂P_{imuj}} {∂T^{imu}_c}
∂Tcimu∂e=∂Tcimu∂Pcj=∂Tcimu−1∂Pcj∂Tcimu∂Tcimu−1+∂Pimuj∂Pcj∂Tcimu∂Pimuj
= ( ∂ P c j ∂ T c i m u − 1 ∂ T c i m u − 1 ∂ T c i m u ) + ∂ P c j ∂ P i m u j ∂ P i m u j ∂ P i m u i ( ∂ P i m u i ∂ T c i m u ) = (\frac {∂P_{cj}} {∂T^{imu -1}_c}\frac {∂T^{imu -1}_c} {∂T^{imu}_c} )+\frac {∂P_{cj}} {∂P_{imuj}}\frac {∂P_{imuj}} {∂P_{imui}}(\frac {∂P_{imui}} {∂T^{imu}_c}) =(∂Tcimu−1∂Pcj∂Tcimu∂Tcimu−1)+∂Pimuj∂Pcj∂Pimui∂Pimuj(∂Tcimu∂Pimui)
(对括号内的部分进行右扰动)
=
[
−
R
c
i
m
u
−
1
(
R
c
i
m
u
−
1
P
i
m
u
j
)
∧
−
(
R
c
i
m
u
−
1
t
c
i
m
u
)
∧
0
T
0
T
]
+
T
c
i
m
u
−
1
(
T
i
m
u
j
w
−
1
T
i
m
u
i
w
)
[
I
−
R
c
i
m
u
P
c
i
∧
0
T
0
T
]
= \left[ \begin{matrix} -R^{imu -1}_c & (R^{imu -1}_cP_{imuj})^{\wedge}-(R^{imu -1}_ct^{imu}_{c})^{\wedge} \\ 0^T & 0^T \end{matrix} \right] +T^{imu-1}_c(T^{w -1}_{imuj}T^w_{imui}) \left[ \begin{matrix} I & -R^{imu}_c P_{ci}^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
=[−Rcimu−10T(Rcimu−1Pimuj)∧−(Rcimu−1tcimu)∧0T]+Tcimu−1(Timujw−1Timuiw)[I0T−RcimuPci∧0T]
= [ − R c i m u − 1 P c j ∧ 0 T 0 T ] + [ R c i m u − 1 − R c i m u − 1 t c i m u 0 T 1 ] ( [ R i m u j w − 1 − R i m u j w − 1 t i m u j w 0 T 1 ] [ R i m u i w t i m u i w 0 T 1 ] ) [ I − R c i m u P c i ∧ 0 T 0 T ] = \left[ \begin{matrix} -R^{imu -1}_c & P_{cj}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] +\left[ \begin{matrix} R^{imu -1}_c & -R^{imu -1}_ct^{imu}_c \\ 0^T & 1 \end{matrix} \right] (\left[ \begin{matrix} R^{w -1}_{imuj} & -R^{w -1}_{imuj}t^{w}_{imuj} \\ 0^T & 1 \end{matrix} \right] \left[ \begin{matrix} R^w_{imui} & t^w_{imui} \\ 0^T & 1 \end{matrix} \right]) \left[ \begin{matrix} I & -R^{imu}_c P_{ci}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] =[−Rcimu−10TPcj∧0T]+[Rcimu−10T−Rcimu−1tcimu1]([Rimujw−10T−Rimujw−1timujw1][Rimuiw0Ttimuiw1])[I0T−RcimuPci∧0T]
= [ − R c i m u − 1 P c j ∧ 0 T 0 T ] + [ R c i m u − 1 − R c i m u − 1 t c i m u 0 T 1 ] [ R i m u j w − 1 R i m u i w R i m u j w − 1 t i m u i w − R i m u j w − 1 t i m u j w 0 T 1 ] [ I − R c i m u P c i ∧ 0 T 0 T ] = \left[ \begin{matrix} -R^{imu -1}_c & P_{cj}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] +\left[ \begin{matrix} R^{imu -1}_c & -R^{imu -1}_ct^{imu}_c \\ 0^T & 1 \end{matrix} \right] \left[ \begin{matrix} R^{w -1}_{imuj}R^w_{imui} & R^{w -1}_{imuj} t^w_{imui} -R^{w -1}_{imuj}t^{w}_{imuj}\\ 0^T & 1 \end{matrix} \right] \left[ \begin{matrix} I & -R^{imu}_c P_{ci}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] =[−Rcimu−10TPcj∧0T]+[Rcimu−10T−Rcimu−1tcimu1][Rimujw−1Rimuiw0TRimujw−1timuiw−Rimujw−1timujw1][I0T−RcimuPci∧0T]
= [ − R c i m u − 1 P c j ∧ 0 T 0 T ] + [ R c i m u − 1 − R c i m u − 1 t c i m u 0 T 1 ] [ R i m u j w − 1 R i m u i w R i m u j w − 1 R i m u i w ( − R c i m u P c i ∧ ) 0 T 0 T ] = \left[ \begin{matrix} -R^{imu -1}_c & P_{cj}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] +\left[ \begin{matrix} R^{imu -1}_c & -R^{imu -1}_ct^{imu}_c \\ 0^T & 1 \end{matrix} \right] \left[ \begin{matrix} R^{w -1}_{imuj}R^w_{imui} & R^{w -1}_{imuj}R^w_{imui}(-R^{imu}_c P_{ci}^{\wedge})\\ 0^T & 0^T \end{matrix} \right] =[−Rcimu−10TPcj∧0T]+[Rcimu−10T−Rcimu−1tcimu1][Rimujw−1Rimuiw0TRimujw−1Rimuiw(−RcimuPci∧)0T]
=
[
−
R
c
i
m
u
−
1
P
c
j
∧
0
T
0
T
]
+
[
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
(
−
R
c
i
m
u
P
c
i
∧
)
0
T
0
T
]
= \left[ \begin{matrix} -R^{imu -1}_c & P_{cj}^{\wedge} \\ 0^T & 0^T \end{matrix} \right] +\left[ \begin{matrix} R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui} & R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}(-R^{imu}_c P_{ci}^{\wedge}) \\ 0^T & 0^T \end{matrix} \right]
=[−Rcimu−10TPcj∧0T]+[Rcimu−1Rimujw−1Rimuiw0TRcimu−1Rimujw−1Rimuiw(−RcimuPci∧)0T]
=
[
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
−
R
c
i
m
u
−
1
P
c
j
∧
−
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
R
c
i
m
u
P
c
i
∧
0
T
0
T
]
= \left[ \begin{matrix} R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}-R^{imu -1}_c & P_{cj}^{\wedge}-R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}R^{imu}_c P_{ci}^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
=[Rcimu−1Rimujw−1Rimuiw−Rcimu−10TPcj∧−Rcimu−1Rimujw−1RimuiwRcimuPci∧0T]
因为：
P
c
j
∧
=
(
R
c
i
m
u
−
1
(
R
i
m
u
j
w
−
1
(
R
i
m
u
i
w
(
R
c
i
m
u
P
c
i
+
t
c
i
m
u
)
+
t
i
m
u
i
w
−
t
i
m
u
j
w
)
−
t
c
i
m
u
−
1
)
)
∧
P_{cj}^{\wedge} =(R^{imu -1}_c(R^{w -1}_{imuj}(R^w_{imui}(R^{imu}_cP_{ci}+t^{imu}_c)+t^w_{imui}-t^w_{imuj})-t^{imu -1}_c))^{\wedge}
Pcj∧=(Rcimu−1(Rimujw−1(Rimuiw(RcimuPci+tcimu)+timuiw−timujw)−tcimu−1))∧
= ( R c i m u − 1 ( R i m u j w − 1 ( R i m u i w R c i m u P c i + R i m u i w t c i m u + t i m u i w − t i m u j w ) − t c i m u − 1 ) ) ∧ =(R^{imu -1}_c(R^{w -1}_{imuj}(R^w_{imui}R^{imu}_cP_{ci}+R^w_{imui}t^{imu}_c+t^w_{imui}-t^w_{imuj})-t^{imu -1}_c))^{\wedge} =(Rcimu−1(Rimujw−1(RimuiwRcimuPci+Rimuiwtcimu+timuiw−timujw)−tcimu−1))∧
= ( R c i m u − 1 ( R i m u j w − 1 R i m u i w R c i m u P c i + R i m u j w − 1 R i m u i w t c i m u + R i m u j w − 1 t i m u i w − R i m u j w − 1 t i m u j w ) − t c i m u − 1 ) ∧ =(R^{imu -1}_c(R^{w -1}_{imuj}R^w_{imui}R^{imu}_cP_{ci}+R^{w -1}_{imuj}R^w_{imui}t^{imu}_c+R^{w -1}_{imuj}t^w_{imui}-R^{w -1}_{imuj}t^w_{imuj})-t^{imu -1}_c)^{\wedge} =(Rcimu−1(Rimujw−1RimuiwRcimuPci+Rimujw−1Rimuiwtcimu+Rimujw−1timuiw−Rimujw−1timujw)−tcimu−1)∧
= ( R c i m u − 1 R i m u j w − 1 R i m u i w R c i m u P c i + R c i m u − 1 R i m u j w − 1 R i m u i w t c i m u + R c i m u − 1 R i m u j w − 1 t i m u i w − R c i m u − 1 R i m u j w − 1 t i m u j w − R c i m u − 1 t c i m u − 1 ) ∧ =(R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}R^{imu}_cP_{ci}+R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}t^{imu}_c+R^{imu -1}_cR^{w -1}_{imuj}t^w_{imui}-R^{imu -1}_cR^{w -1}_{imuj}t^w_{imuj}-R^{imu -1}_ct^{imu -1}_c)^{\wedge} =(Rcimu−1Rimujw−1RimuiwRcimuPci+Rcimu−1Rimujw−1Rimuiwtcimu+Rcimu−1Rimujw−1timuiw−Rcimu−1Rimujw−1timujw−Rcimu−1tcimu−1)∧
= ( R c i m u − 1 R i m u j w − 1 R i m u i w R c i m u P c i ) ∧ + ( R c i m u − 1 R i m u j w − 1 R i m u i w t c i m u + R c i m u − 1 R i m u j w − 1 t i m u i w − R c i m u − 1 R i m u j w − 1 t i m u j w − R c i m u − 1 t c i m u − 1 ) ∧ =(R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}R^{imu}_cP_{ci})^{\wedge}+(R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}t^{imu}_c+R^{imu -1}_cR^{w -1}_{imuj}t^w_{imui}-R^{imu -1}_cR^{w -1}_{imuj}t^w_{imuj}-R^{imu -1}_ct^{imu -1}_c)^{\wedge} =(Rcimu−1Rimujw−1RimuiwRcimuPci)∧+(Rcimu−1Rimujw−1Rimuiwtcimu+Rcimu−1Rimujw−1timuiw−Rcimu−1Rimujw−1timujw−Rcimu−1tcimu−1)∧
= ( R c i m u − 1 R i m u j w − 1 R i m u i w R c i m u P c i ) ∧ + ( R c i m u − 1 ( R i m u j w − 1 ( R i m u i w t c i m u + t i m u i w − t i m u j w ) − t c i m u − 1 ) ) ∧ =(R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}R^{imu}_cP_{ci})^{\wedge}+(R^{imu -1}_c(R^{w -1}_{imuj}(R^w_{imui}t^{imu}_c+t^w_{imui}-t^w_{imuj})-t^{imu -1}_c))^{\wedge} =(Rcimu−1Rimujw−1RimuiwRcimuPci)∧+(Rcimu−1(Rimujw−1(Rimuiwtcimu+timuiw−timujw)−tcimu−1))∧
因此，答案为：
[
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
−
R
c
i
m
u
−
1
(
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
R
c
i
m
u
P
c
i
)
∧
+
(
R
c
i
m
u
−
1
(
R
i
m
u
j
w
−
1
(
R
i
m
u
i
w
t
c
i
m
u
+
t
i
m
u
i
w
−
t
i
m
u
j
w
)
−
t
c
i
m
u
−
1
)
)
∧
−
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
R
c
i
m
u
P
c
i
∧
0
T
0
T
]
\left[ \begin{matrix} R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}-R^{imu -1}_c & (R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}R^{imu}_cP_{ci})^{\wedge}+(R^{imu -1}_c(R^{w -1}_{imuj}(R^w_{imui}t^{imu}_c+t^w_{imui}-t^w_{imuj})-t^{imu -1}_c))^{\wedge}-R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}R^{imu}_c P_{ci}^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
[Rcimu−1Rimujw−1Rimuiw−Rcimu−10T(Rcimu−1Rimujw−1RimuiwRcimuPci)∧+(Rcimu−1(Rimujw−1(Rimuiwtcimu+timuiw−timujw)−tcimu−1))∧−Rcimu−1Rimujw−1RimuiwRcimuPci∧0T]
=
[
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
−
R
c
i
m
u
−
1
−
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
R
c
i
m
u
P
c
i
∧
+
(
R
c
i
m
u
−
1
R
i
m
u
j
w
−
1
R
i
m
u
i
w
R
c
i
m
u
P
c
i
)
∧
+
(
R
c
i
m
u
−
1
(
R
i
m
u
j
w
−
1
(
R
i
m
u
i
w
t
c
i
m
u
+
t
i
m
u
i
w
−
t
i
m
u
j
w
)
−
t
c
i
m
u
−
1
)
)
∧
0
T
0
T
]
=\left[ \begin{matrix} R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}-R^{imu -1}_c & -R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}R^{imu}_c P_{ci}^{\wedge}+(R^{imu -1}_cR^{w -1}_{imuj}R^w_{imui}R^{imu}_cP_{ci})^{\wedge}+(R^{imu -1}_c(R^{w -1}_{imuj}(R^w_{imui}t^{imu}_c+t^w_{imui}-t^w_{imuj})-t^{imu -1}_c))^{\wedge} \\ 0^T & 0^T \end{matrix} \right]
=[Rcimu−1Rimujw−1Rimuiw−Rcimu−10T−Rcimu−1Rimujw−1RimuiwRcimuPci∧+(Rcimu−1Rimujw−1RimuiwRcimuPci)∧+(Rcimu−1(Rimujw−1(Rimuiwtcimu+timuiw−timujw)−tcimu−1))∧0T]
4、对逆深度 λ i λ_i λi求偏导
误差计算公式：
e
=
T
c
i
m
u
−
1
T
i
m
u
j
w
−
1
T
i
m
u
i
w
T
c
i
m
u
P
c
i
−
P
j
e = T^{imu -1}_c T^{w -1}_{imuj} T^w_{imui} T^{imu}_c P_{ci} - P_j
e=Tcimu−1Timujw−1TimuiwTcimuPci−Pj
即：
e
=
T
c
i
m
u
−
1
T
i
m
u
j
w
−
1
T
i
m
u
i
w
T
c
i
m
u
1
λ
i
(
u
i
,
v
i
)
−
P
j
e = T^{imu -1}_c T^{w -1}_{imuj} T^w_{imui} T^{imu}_c \frac {1} {λ_i}(ui, vi) - P_j
e=Tcimu−1Timujw−1TimuiwTcimuλi1(ui,vi)−Pj
= T c i m u − 1 T i m u j w − 1 T i m u i w ( R c i m u ( − 1 λ 2 ( u i , v i ) ) + t c i m u ) =T^{imu -1}_c T^{w -1}_{imuj} T^w_{imui} (R^{imu}_c(- \frac {1} {λ^2}(ui, vi))+t^{imu}_c) =Tcimu−1Timujw−1Timuiw(Rcimu(−λ21(ui,vi))+tcimu)
= T c i m u − 1 T i m u j w − 1 ( R i m u i w ( R c i m u ( − 1 λ 2 ( u i , v i ) ) + t c i m u ) + t i m u i w ) =T^{imu -1}_c T^{w -1}_{imuj} (R^w_{imui} (R^{imu}_c(- \frac {1} {λ^2}(ui, vi))+t^{imu}_c) + t^w_{imui}) =Tcimu−1Timujw−1(Rimuiw(Rcimu(−λ21(ui,vi))+tcimu)+timuiw)
= T c i m u − 1 R i m u j w − 1 ( R i m u i w ( R c i m u ( − 1 λ 2 ( u i , v i ) ) + t c i m u ) + t i m u i w − t i m u j w ) =T^{imu -1}_c R^{w -1}_{imuj} (R^w_{imui} (R^{imu}_c(- \frac {1} {λ^2}(ui, vi))+t^{imu}_c) + t^w_{imui}-t^w_{imuj}) =Tcimu−1Rimujw−1(Rimuiw(Rcimu(−λ21(ui,vi))+tcimu)+timuiw−timujw)
= R c i m u − 1 ( R i m u j w − 1 ( R i m u i w ( R c i m u ( − 1 λ 2 ( u i , v i ) ) + t c i m u ) + t i m u i w − t i m u j w ) − t c i m u t − 1 ) =R^{imu -1}_c (R^{w -1}_{imuj} (R^w_{imui} (R^{imu}_c(- \frac {1} {λ^2}(ui, vi))+t^{imu}_c) + t^w_{imui}-t^w_{imuj})-t^{imut -1}_c) =Rcimu−1(Rimujw−1(Rimuiw(Rcimu(−λ21(ui,vi))+tcimu)+timuiw−timujw)−tcimut−1)
可得：
∂
e
∂
P
c
i
∂
P
c
i
∂
λ
i
\frac {∂e} {∂P_{ci}} \frac {∂P_{ci}} {∂λ_i}
∂Pci∂e∂λi∂Pci
= R c i m u − 1 R i m u j w − 1 R i m u i w R c i m u ( − 1 λ 2 ( u i , v i ) ) =R^{imu -1}_c R^{w -1}_{imuj} R^w_{imui} R^{imu}_c(- \frac {1} {λ^2}(ui, vi)) =Rcimu−1Rimujw−1RimuiwRcimu(−λ21(ui,vi))
2.2.4 Vins-Mono中回环检测的雅可比矩阵求导
源代码位置：/VINS-Mono/pose_graph/src/pose_graph.h
函数：struct FourDOFError
原理：计算回环两侧在世界坐标系下的平移向量与偏航角 ψ ψ ψ(yaw)，然后与实际观测到的从i到j的坐标差相减，优化位姿的参数。
输入参数 ( t i w , ψ i , t j w , ψ j ) (t^w_i, ψ_i, t^w_j, ψ_j) (tiw,ψi,tjw,ψj)，即i帧与j帧对应的在世界坐标系下的平移向量与偏航角 ψ ψ ψ(yaw)
俯仰角 θ θ θ(pitch)与滚转角 Φ Φ Φ(roll)为固定观测值不进行优化。
残差计算公式为：
[
R
i
w
−
1
(
t
j
w
−
t
i
w
)
−
t
i
j
i
ψ
j
−
ψ
i
−
ψ
i
j
]
=
[
R
i
w
−
1
t
j
w
−
R
i
w
−
1
t
i
w
−
t
i
j
i
ψ
j
−
ψ
i
−
ψ
i
j
]
\left[ \begin{matrix} R^{w-1}_i(t^w_j-t^w_i)-t^i_{ij} \\ ψ_j-ψ_i-ψ_{ij} \end{matrix} \right] =\left[ \begin{matrix} R^{w-1}_it^w_j-R^{w-1}_it^w_i-t^i_{ij} \\ ψ_j-ψ_i-ψ_{ij} \end{matrix} \right]
[Riw−1(tjw−tiw)−tijiψj−ψi−ψij]=[Riw−1tjw−Riw−1tiw−tijiψj−ψi−ψij]
t
i
j
i
t^i_{ij}
tiji为
t
j
i
−
t
i
i
t^i_{j}-t^i_{i}
tji−tii
ψ
i
j
ψ_{ij}
ψij为
ψ
j
−
ψ
i
ψ_{j}-ψ_{i}
ψj−ψi
这两个值和左侧的数据来源不太一样，由外部计算完成后直接导入。
对
t
i
w
t^w_i
tiw求偏导，雅可比矩阵为：
[
−
(
R
i
w
−
1
t
i
w
)
∧
0
]
\left[ \begin{matrix} -(R^{w-1}_it^w_i)^{\wedge} \\ 0 \end{matrix} \right]
[−(Riw−1tiw)∧0]
对
ψ
i
ψ_i
ψi求偏导，雅可比矩阵为：
[
0
−
1
]
\left[ \begin{matrix} 0 \\ -1 \end{matrix} \right]
[0−1]
对
t
j
w
t^w_j
tjw求偏导，雅可比矩阵为：
[
(
R
i
w
−
1
t
j
w
)
∧
0
]
\left[ \begin{matrix} (R^{w-1}_it^w_j)^{\wedge} \\ 0 \end{matrix} \right]
[(Riw−1tjw)∧0]
对
ψ
j
ψ_j
ψj求偏导，雅可比矩阵为：
[
0
1
]
\left[ \begin{matrix} 0 \\ 1 \end{matrix} \right]
[01]
84
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
