之前估计的旋转等我外参，加速度计的零偏不在这里进行估计，会在后面滑窗优化的时候进行估计
q
b
k
c
0
q^{c_{0}}_{b_{k}}
qbkc0 表示枢纽帧
c
0
c_{0}
c0 到第
k
k
k 帧imu的变换
q
c
k
c
0
q^{c_{0}}_{c_{k}}
qckc0 表示枢纽帧
c
0
c_{0}
c0 到第k帧相机的变换
q
c
b
q^{b}_{c}
qcb 表示旋转外参的估计量，上一节推导的
通过公式 q b k c 0 = q c k c 0 ⊕ ( q c b ) − 1 q^{c_{0}}_{b_{k}}=q^{c_{0}}_{c_{k}}⊕(q^{b}_{c})^{-1} qbkc0=qckc0⊕(qcb)−1就能获得枢纽帧到第k帧imu的变换，这个第k是按相机的顺序来算的
然后平移公式如下：
s
P
b
k
c
0
=
s
P
c
k
c
0
−
R
b
k
c
0
P
c
b
sP^{c_{0}}_{b_{k}}=sP^{c_{0}}_{c_{k}}-R^{c_{0}}_{b_{k}}P^{b}_{c}
sPbkc0=sPckc0−Rbkc0Pcb，
s
s
s 是尺度
通过公式
T
c
0
c
k
=
T
c
0
b
k
⋅
T
b
c
T_{c_{0}c_{k}}=T_{c_{0}b_{k}}·T_{bc}
Tc0ck=Tc0bk⋅Tbc 展开推一下就出来了
上来两个公式等于是用已知量算出来了 T c 0 b k T_{c_{0}b_{k}} Tc0bk，这个是用视觉算出来旋转乘上外参算出来的，这样就和imu坐标系有关联了，后面才乘上个 T b k + 1 c 0 T_{b_{k+1}c_{0}} Tbk+1c0 ，就能获得两个imu之间的旋转 T b k + 1 b k T_{b_{k+1}b_{k}} Tbk+1bk
整体思想就是，通过相机计算出来的旋转（得通过外参转换到imu坐标系下）理论上是等于预积分出来的旋转的
求陀螺仪零偏的公式如下
m
i
n
∑
∣
∣
(
q
b
k
+
1
c
0
)
−
1
⊕
q
b
k
c
0
⊕
γ
b
k
+
1
b
k
∣
∣
2
min∑||(q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}⊕γ^{b_{k}}_{b_{k+1}}||^{2}
min∑∣∣(qbk+1c0)−1⊕qbkc0⊕γbk+1bk∣∣2
里面的
(
q
b
k
+
1
c
0
)
−
1
⊕
q
b
k
c
0
=
q
b
k
b
k
+
1
(q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}=q^{b_{k+1}}_{b_{k}}
(qbk+1c0)−1⊕qbkc0=qbkbk+1 ，这个本质上是和
γ
b
k
+
1
b
k
γ^{b_{k}}_{b_{k+1}}
γbk+1bk 的逆是一样的，他们两个相乘结果理论上是单位矩阵，
γ
b
k
+
1
b
k
γ^{b_{k}}_{b_{k+1}}
γbk+1bk是预积分量。
由于零偏的存在，所以他们的相乘不会是单位阵
更新的时候采用以下公式
γ
b
k
+
1
b
k
≈
γ
b
k
+
1
b
k
^
⊕
[
1
1
2
J
b
w
γ
∆
b
w
]
γ^{b_{k}}_{b_{k+1}}≈\hat{γ^{b_{k}}_{b_{k+1}}}⊕\begin{bmatrix}1\\\frac{1}{2}J^{γ}_{b_{w}}∆b_{w}\end{bmatrix}
γbk+1bk≈γbk+1bk^⊕[121Jbwγ∆bw]
上述的意思是当零偏经过优化发生变化是，通过获取零偏变化的微量
∆
b
w
∆b_{w}
∆bw 乘上旋转量对零偏的雅克比矩阵（这个在前面零偏建模的时候有过推导是怎么来的），然后通过这样的方式更新旋转量
然后计算公式变为如下
[
1
0
0
0
]
=
(
q
b
k
+
1
c
0
)
−
1
⊕
q
b
k
c
0
⊕
γ
b
k
+
1
b
k
^
⊕
[
1
1
2
J
b
w
γ
∆
b
w
]
\begin{bmatrix} 1\\0\\0\\0 \end{bmatrix}=(q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}⊕\hat{γ^{b_{k}}_{b_{k+1}}}⊕\begin{bmatrix}1\\\frac{1}{2}J^{γ}_{b_{w}}∆b_{w}\end{bmatrix}
1000
=(qbk+1c0)−1⊕qbkc0⊕γbk+1bk^⊕[121Jbwγ∆bw]
这样就可以更新零偏然后知道满足结果为0为止，前面计算的对零偏的雅克比也是在这里这样用的
由于
(
q
b
k
+
1
c
0
)
−
1
⊕
q
b
k
c
0
⊕
γ
b
k
+
1
b
k
^
(q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}⊕\hat{γ^{b_{k}}_{b_{k+1}}}
(qbk+1c0)−1⊕qbkc0⊕γbk+1bk^ 是已知的，都拿到左边去
则有
[
(
γ
b
k
+
1
b
k
^
)
−
1
⊕
(
q
b
k
c
0
)
−
1
⊕
q
b
k
+
1
c
0
]
v
e
c
=
1
2
J
∆
b
(\hat{γ^{b_{k}}_{b_{k+1}}})^{-1}⊕(q^{c_{0}}_{b_{k}})^{-1}⊕q^{c_{0}}_{b_{k+1}}]_{vec}=\frac{1}{2}J∆b
(γbk+1bk^)−1⊕(qbkc0)−1⊕qbk+1c0]vec=21J∆b
上面的
v
e
c
vec
vec 指的是取虚部，因为只有需要发生变化，实部一直都是1，整个公式的未知量只有
∆
b
∆b
∆b ，整个问题可以认为是 Ax=b 的模型， Ax=0的话就可以用 SVD来进行分解。
这里会有很多组数据，会把
A
,
b
A,b
A,b 矩阵累积扩大
代码中进行小矩阵累加，就是为了避免大矩阵的运算，代码中用+=来对矩阵进行累加
只要最小化
∣
∣
A
x
−
b
∣
∣
m
i
n
||Ax-b||_{min}
∣∣Ax−b∣∣min 即可
可以这样去进行记忆
A
x
=
b
Ax=b
Ax=b
=
>
A
T
A
x
=
A
T
b
=>A^{T}Ax=A^{T}b
=>ATAx=ATb
=
>
x
=
=
(
A
T
A
)
−
1
⋅
A
T
⋅
b
=>x==(A^{T}A)^{-1}·A^{T}·b
=>x==(ATA)−1⋅AT⋅b
这里的
x
x
x 结果就是我们的
∆
b
∆b
∆b ，这个结果就是我们要的零偏估计
求解出零偏后会对滑窗中的预积分根据新的零偏重新进行传播，因为第一次估计的 ∆ b ∆b ∆b 会比较大，所以才会重新传播，后面都是用雅克比矩阵进行一阶近似来传播的，算出来的是 ∆ b ∆b ∆b ，所以代码中更新零偏也是采用+=的方式
635
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
