用逆深度是因为这样可以在优化中从优化3个变量降低到1个，降低优化的维度加快求解速度
用逆深度是因为当距离很远的时候，
1
x
\frac{1}{x}
x1
x
x
x 就会无穷大，而3D点很近的情况也一般不会有，这也是为了数值稳定性
用逆深度的话就要和其中一帧进行绑定，这个就是和观测到该点的第一帧进行绑定，这样才能表示一个3D点信息
划窗中维护的全部都是IMU下的位姿，所以相机要通过外参变换到IMU坐标系下
这里就构成了视觉误差，需要求关于优化变量的雅可比矩阵，这里约束了第
i
i
i 帧和第
j
j
j 帧的 IMU 的姿态，同时还会优化相机和IMU的外参，这个也是紧耦合的特点之一（上一节同时优化 IMU预积分自身的零偏Ba也是紧耦合特点之一），3D点（逆深度）也是要优化，总共就是4个参数
转换公式如下：
第
i
i
i 帧归一化坐标系 -> 第
j
j
j 帧相机系，
1
λ
\frac{1}{\lambda}
λ1，就是深度，
λ
\lambda
λ 是逆深度
将旋转和平移分开后如下：
将刚刚第
i
i
i 帧相机系下的3D点进行归一化，然后和光流追踪到的匹配点进行残差计算，这就获得了视觉重投影误差
计算残差对优化量的雅可比
有带时间延时估计的雅可比计算和不带时间估计的雅可比计算
这里先讲不带时间延时的雅可比计算
这里的误差项是2维的，坐标点是3维的
要求误差对旋转的雅可比只能通过链式求导的方式
∂
r
∂
p
j
⋅
∂
p
j
∂
x
\frac{\partial r}{\partial p_{j}}·\frac{\partial p_{j}}{\partial x}
∂pj∂r⋅∂x∂pj
∂
r
∂
p
j
\frac{\partial r}{\partial p_{j}}
∂pj∂r 是2×3维的，对平移
x
,
y
,
z
x,y,z
x,y,z 进行求导
=
[
1
z
0
−
x
z
2
0
1
z
−
y
z
2
]
=\begin{bmatrix}\frac{1}{z}&0&-\frac{x}{z^{2}} \\ 0&\frac{1}{z}&-\frac{y}{z^{2}} \end{bmatrix}
=[z100z1−z2x−z2y]
这里的误差也有协方差矩阵，提点的置信度是设定为1.5个像素不变
残差也得乘上置信度
计算 p j p_{j} pj 对 T T T 的雅可比
平移
x
,
y
,
z
x,y,z
x,y,z 的公式为
T T T 包含旋转 R R R 和平移 t t t
对 i i i 时刻求导
i i i 时刻的变量也是要优化的量，所以当然也要求导
对 p w b i p_{wb_{i}} pwbi 求导
∂ p j ∂ p w b i = R b c T ⋅ R w b j T \frac{\partial p_{j}}{\partial p_{wb_{i}}}=R^{T}_{bc}·R^{T}_{wb_{j}} ∂pwbi∂pj=RbcT⋅RwbjT
对 R w b i R_{wb_{i}} Rwbi 求导
∂
p
j
∂
R
w
b
i
\frac{\partial p_{j}}{\partial R_{wb_{i}}}
∂Rwbi∂pj
先把公式中有
R
w
b
i
R_{wb_{i}}
Rwbi 的项提取出来
=
R
b
c
T
R
w
b
j
T
R
w
b
i
(
R
b
c
P
c
i
+
p
b
c
)
=R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}(R_{bc}P^{c_{i}}+p_{bc})
=RbcTRwbjTRwbi(RbcPci+pbc)
=
A
R
w
b
i
b
=AR_{wb_{i}}b
=ARwbib
后面那一串 b b b 乘完后是向量，所以可以对李代数进行扰动求导（纯旋转矩阵是不能对李代数求导的，因为矩阵无法对向量求导，这里是乘完后是个向量，所以可以用向量来表示旋转的扰动量，然后用导数的定义来进行求导）
∂
A
R
w
b
i
b
∂
ϕ
=
A
R
w
b
i
e
x
p
(
ϕ
∧
)
b
−
A
R
w
b
i
b
ϕ
\frac{\partial AR_{wb_{i}}b}{\partial \phi}=\frac{AR_{wb_{i}}exp(\phi^{\wedge})b-AR_{wb_{i}}b}{\phi}
∂ϕ∂ARwbib=ϕARwbiexp(ϕ∧)b−ARwbib
=
A
R
w
b
i
(
I
+
ϕ
∧
)
b
−
A
R
w
b
i
b
ϕ
=\frac{AR_{wb_{i}}(I+\phi^{\wedge})b-AR_{wb_{i}}b}{\phi}
=ϕARwbi(I+ϕ∧)b−ARwbib
=
A
R
w
b
i
ϕ
∧
b
ϕ
=\frac{AR_{wb_{i}}\phi^{\wedge}b}{\phi}
=ϕARwbiϕ∧b
根据反对成矩阵的性质
=
−
A
R
w
b
i
b
∧
ϕ
ϕ
=\frac{-AR_{wb_{i}}b^{\wedge}\phi}{\phi}
=ϕ−ARwbib∧ϕ
=
−
A
R
w
b
i
b
∧
=-AR_{wb_{i}}b^{\wedge}
=−ARwbib∧
那个信息矩阵乘完第一步也得乘进来这里这个第2步的雅可比矩阵
对 j j j 时刻进行求导
对 p w b j p_{wb_{j}} pwbj 求导
∂ p j ∂ p w b j = − R b c T ⋅ R w b j T \frac{\partial p_{j}}{\partial p_{wb_{j}}}=-R^{T}_{bc}·R^{T}_{wb_{j}} ∂pwbj∂pj=−RbcT⋅RwbjT
对 R w b j R_{wb_{j}} Rwbj 求导
把和
R
w
b
j
R_{wb_{j}}
Rwbj 有关的项提取出来
=
R
b
c
T
R
w
b
j
T
(
R
w
b
i
R
b
c
P
c
i
+
R
w
b
i
p
b
c
+
p
w
b
i
−
p
w
b
j
)
=R^{T}_{bc}R^{T}_{wb_{j}}(R_{wb_{i}}R_{bc}P^{c_{i}}+R_{wb_{i}}p_{bc}+p_{wb_{i}}-p_{wb_{j}})
=RbcTRwbjT(RwbiRbcPci+Rwbipbc+pwbi−pwbj)
=
A
R
w
b
j
T
b
=AR^{T}_{wb_{j}}b
=ARwbjTb
对
R
w
b
j
T
R^{T}_{wb_{j}}
RwbjT 是只能左乘的，但是我们现在要算他右乘的扰动方向，因为方向会不同，用左乘的求导结果加个负号就是右乘的结果，这里推导直接用右乘，所以要加个逆把这个转置消掉来进行推导
=
A
(
R
w
b
j
e
x
p
(
ϕ
∧
)
)
−
1
b
−
A
(
R
w
b
j
)
−
1
b
=A(R_{wb_{j}}exp(\phi^{\wedge}))^{-1}b-A(R_{wb_{j}})^{-1}b
=A(Rwbjexp(ϕ∧))−1b−A(Rwbj)−1b
=
A
(
I
−
ϕ
∧
)
R
w
b
j
T
b
−
A
(
R
w
b
j
)
−
1
b
=A(I-\phi^{\wedge})R^{T}_{wb_{j}}b-A(R_{wb_{j}})^{-1}b
=A(I−ϕ∧)RwbjTb−A(Rwbj)−1b
=
−
A
ϕ
∧
R
w
b
j
T
b
=-A\phi^{\wedge}R^{T}_{wb_{j}}b
=−Aϕ∧RwbjTb
=
A
(
R
w
b
j
T
b
)
∧
ϕ
=A(R^{T}_{wb_{j}}b)^{\wedge}\phi
=A(RwbjTb)∧ϕ
消去
ϕ
\phi
ϕ
=
A
(
R
w
b
j
T
b
)
∧
=A(R^{T}_{wb_{j}}b)^{\wedge}
=A(RwbjTb)∧
后面的 R w b j T b R^{T}_{wb_{j}}b RwbjTb 实际就是 3D 点在 第 j j j 帧 IMU系下的位姿，按照展开前的刚体变换来理解一下就好了
对 IMU-相机 的外参求导$
对 p b c p_{bc} pbc 求导
= R b c T R w b j T R w b i − R b c T =R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}-R^{T}_{bc} =RbcTRwbjTRwbi−RbcT
对 R b c R_{bc} Rbc 求导
代码中的 r i c = R b c , t i c = t b c ric=R_{bc},tic=t_{bc} ric=Rbc,tic=tbc， Q = R w b Q=R_{wb} Q=Rwb
导数是符合加法的 ( f ( x ) + g ( x ) ) ′ = f ′ ( x ) + g ′ ( x ) (f(x)+g(x))^{'}=f^{'}(x)+g^{'}(x) (f(x)+g(x))′=f′(x)+g′(x)
加法后面的求导结果 = ( R b c T b ) =(R^{T}_{bc}b) =(RbcTb) ，这个推导和上面类似，就不详细写了
加法前面的求导稍微复杂一点
=
(
R
b
c
e
x
p
(
ϕ
∧
)
)
−
1
R
w
b
j
T
R
w
b
i
R
b
c
T
e
x
p
(
ϕ
∧
)
P
c
i
−
R
b
c
T
R
w
b
j
T
R
w
b
i
R
b
c
T
P
c
i
=(R_{bc}exp(\phi^{\wedge}))^{-1}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}exp(\phi^{\wedge})P^{c_{i}}-R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}P^{c_{i}}
=(Rbcexp(ϕ∧))−1RwbjTRwbiRbcTexp(ϕ∧)Pci−RbcTRwbjTRwbiRbcTPci
下面暂时省略写后面它自身
=
(
I
−
ϕ
∧
)
R
b
c
T
R
w
b
j
T
R
w
b
i
R
b
c
T
(
I
+
ϕ
∧
)
P
c
i
=(I-\phi^{\wedge})R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}(I+\phi^{\wedge})P^{c_{i}}
=(I−ϕ∧)RbcTRwbjTRwbiRbcT(I+ϕ∧)Pci
=
(
I
−
ϕ
∧
)
A
(
I
+
ϕ
∧
)
P
c
i
=(I-\phi^{\wedge})A(I+\phi^{\wedge})P^{c_{i}}
=(I−ϕ∧)A(I+ϕ∧)Pci
=
(
A
−
ϕ
∧
A
)
(
I
+
ϕ
∧
)
P
c
i
=(A-\phi^{\wedge}A)(I+\phi^{\wedge})P^{c_{i}}
=(A−ϕ∧A)(I+ϕ∧)Pci
=
(
A
+
A
ϕ
∧
−
ϕ
∧
A
−
ϕ
∧
A
ϕ
∧
)
P
c
i
−
A
P
c
i
=(A+A\phi^{\wedge}-\phi^{\wedge}A-\phi^{\wedge}A \phi^{\wedge})P^{c_{i}}-AP^{c_{i}}
=(A+Aϕ∧−ϕ∧A−ϕ∧Aϕ∧)Pci−APci
其中 ϕ ∧ A ϕ ∧ \phi^{\wedge}A \phi^{\wedge} ϕ∧Aϕ∧ 是二阶，是相对于一阶的无穷小，这里只讨论一阶的展开且 ϕ \phi ϕ 本身就是小量，所以直接约掉
=
(
A
ϕ
∧
−
ϕ
∧
A
)
⋅
P
c
i
=(A\phi^{\wedge}-\phi^{\wedge}A)·P^{c_{i}}
=(Aϕ∧−ϕ∧A)⋅Pci
=
−
A
P
∧
ϕ
+
(
A
P
)
∧
ϕ
=-AP^{\wedge}\phi+(AP)^{\wedge}\phi
=−AP∧ϕ+(AP)∧ϕ
约掉
ϕ
\phi
ϕ
=
−
A
P
∧
+
(
A
P
)
∧
=-AP^{\wedge}+(AP)^{\wedge}
=−AP∧+(AP)∧
对逆深度 λ \lambda λ 求导
∂ p j ∂ P c i ∂ P c i ∂ λ \frac{\partial p_{j}}{\partial P^{c_{i}}}\frac{\partial P^{c_{i}}}{\partial \lambda} ∂Pci∂pj∂λ∂Pci
前面的 ∂ p j ∂ P c i = R b c T R w b j T R w b i R b c \frac{\partial p_{j}}{\partial P^{c_{i}}}=R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R_{bc} ∂Pci∂pj=RbcTRwbjTRwbiRbc
P c i = 1 λ ⋅ p P^{c_{i}}=\frac{1}{\lambda} ·p Pci=λ1⋅p， p p p 是归一化相机系下的3D点
∂ P c i ∂ λ = − 1 λ 2 ⋅ p \frac{\partial P^{c_{i}}}{\partial \lambda}=-\frac{1}{\lambda^{2}}·p ∂λ∂Pci=−λ21⋅p
这个 − 1 λ 2 -\frac{1}{\lambda^{2}} −λ21 是个系数，移到哪里都可以
零空间漂移处理
优化的时候会固定滑窗中的第一帧的xyz和yaw角，因为IMU约束的是相对位姿，且IMU的4个不可观自由度就是 y a w 、 x 、 y 、 z yaw、x、y、z yaw、x、y、z，绝对位姿是没有约束的，所以可能会产生在 4自由度的 0 空间漂移的情况，fusion中的GPS就是约束绝对位姿的。
VINS中的固定是先计算第一帧的yaw和xyz的偏移量，然后把后面的帧都减去这个偏移量，偏移回之前的位置，这样的做法就类似ORB中的固定第一帧的位姿，不过这里是减去第一帧的偏移量，其实就是滑窗中的整条轨迹调整回偏移前的位置，这样就保证不受 0 空间的影响。
和 yaw相关的量都会受影响，就是和旋转向量的量都会受影响， P , V P,V P,V 受影响，零偏 B a , B g Ba,Bg Ba,Bg ，外参 T b c Tbc Tbc 不受影响
前面会把旋转矩阵变成rpy，然后把yaw的角度差取出来构成新的偏移旋转矩阵，因为只是yaw发生漂移
