视觉SLAM十四讲|【四】误差Jacobian推导

视觉SLAM十四讲|【四】误差Jacobian推导

预积分误差递推公式

$$\omega =\frac{1}{2}(({\omega }_b^k+n_k^g-b_k^g)+(w_b^{k+1}+n_{k+1}^g-b_{k+1}^g))$$
其中，$w_b^k$为$k$时刻下body坐标系的角速度，$n_k^g$为$k$时刻下陀螺仪白噪声，$b_k^g$为$k$时刻下陀螺仪偏置量。$n_k^a$为$k$时刻下加速度白噪声，$b_k^a$为$k$时刻下加速度偏置量。$k+1$时刻下记号同理。
$$q_{b_i{b}_{k+1}}=q_{b_i{b}_k}\otimes [1,\frac{1}{2}\omega \delta t{]}^T$$
$$a=\frac{1}{2}(q_{b_i{b}_k}(a_b^k+n_b^k-b_k^a)+q_{b_i{b}_{k+1}}(a_b^{k+1}+n_b^{k+1}-b_{k+1}^a))$$
$${\alpha }_{b_i{b}_{k+1}}={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}a\delta t^2$$
$${\beta }_{b_i{b}_{k+1}}={\beta }_{b_i{b}_k}+a\delta t$$
$$b_{k+1}^a=b_k^a+n_{b_k^a}\delta t$$
$$b_{k+1}^g=b_k^g+n_{b_k^g}\delta t$$

示例1

$$f_{15}=\frac{\delta {\alpha }_{b_i{b}_{k+1}}}{\delta b_k^g}$$
由上面的递推公式可知
$${\alpha }_{b_i{b}_{k+1}}={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}a\delta t^2$$
其中，${\alpha }_{b_i{b}_k}$、${\beta }_{b_i{b}_k}\delta t$都与$b_k^g$无关，可以省略，而很容易看出$a$中含有$q_{b_i{b}_{k+1}}$项，其中进一步含有对$b_k^g$相关的元素，必须保留。因此进一步推得
$$f_{15}=\frac{\delta \frac{1}{2}a\delta t^2}{\delta b_k^g}$$
其中，
$$a=\frac{1}{2}(q_{b_i{b}_k}(a_b^k+n_b^k-b_k^a)+q_{b_i{b}_{k+1}}(a_b^{k+1}+n_b^{k+1}-b_{k+1}^a))$$
$q_{b_i{b}_k}(a_b^k+n_b^k-b_k^a)$依然与$b_k^g$无关，可以省略。
$$f_{15}=\frac{\delta \frac{1}{4}{q}_{b_i{b}_{k+1}}(a_b^{k+1}+n_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
白噪声项不可知，拿掉
$$f_{15}=\frac{\delta \frac{1}{4}{q}_{b_i{b}_{k+1}}(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
$$f_{15}=\frac{\delta \frac{1}{4}{q}_{b_i{b}_{k+1}}(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
$$q_{b_i{b}_{k+1}}=q_{b_i{b}_k}\otimes [1,\frac{1}{2}\omega \delta t{]}^T$$
$$f_{15}=\frac{\delta \frac{1}{4}{q}_{b_i{b}_k}\otimes [1,\frac{1}{2}\omega \delta t{]}^T(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
其中
$$\omega =\frac{1}{2}(({\omega }_b^k+n_k^g-b_k^g)+(w_b^{k+1}+n_{k+1}^g-b_{k+1}^g))$$
去除不可知的白噪声项
$$\omega =\frac{1}{2}(({\omega }_b^k-b_k^g)+(w_b^{k+1}-b_{k+1}^g))$$
由于$k+1$时刻的信息并不知道，在此处如果不使用中值积分，直接使用初始值，有
$$\omega ={\omega }_b^k-b_k^g$$
$$f_{15}=\frac{\delta \frac{1}{4}{q}_{b_i{b}_k}\otimes [1,\frac{1}{2}({\omega }_b^k-b_k^g)\delta t{]}^T(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
此时，为了便于计算，我们需要把四元数表示旋转转换为用旋转矩阵表示矩阵的旋转，得到
$$f_{15}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}\exp (((w_b^k-b_k^g)\delta t{)}^{\wedge })(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
观察式子，我们要想办法把$b_k^g$拆出来。回顾上一章，李代数旋转有性质
$$ln(Rexp({\varphi }^{\wedge }){)}^{\vee }=ln(R{)}^{\vee }+J_r^{-1}\varphi$$
类似的，对于非对数情况，有
$$\exp ((\varphi +\delta \varphi {)}^{\wedge })=\exp ({\varphi }^{\wedge })\exp ((J_r(\varphi )\delta \varphi {)}^{\wedge })$$
$$\underset{\varphi \to 0}{\lim }{J}_r(\varphi )=I$$
$$\exp (((w_b^k-b_k^g)\delta t{)}^{\wedge }=\exp ((w_b^k\delta t{)}^{\wedge })\exp ((J_r(w_b^k\delta t)(-b_k^g\delta t){)}^{\wedge })$$
$$f_{15}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}\exp (((w_b^k-b_k^g)\delta t{)}^{\wedge })(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
$$f_{15}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}\exp ((w_b^k\delta t{)}^{\wedge })\exp ((J_r(w_b^k\delta t)(-b_k^g\delta t){)}^{\wedge })(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
$$w_b^k\delta t\to 0$$
$$f_{15}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}\exp ((J_r(w_b^k\delta t)(-b_k^g\delta t){)}^{\wedge })(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
$$f_{15}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}\exp ((-b_k^g\delta t){)}^{\wedge })(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
$$f_{15}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}(I+(-b_k^g\delta t){)}^{\wedge })(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
$$f_{15}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}(-b_k^g\delta t{)}^{\wedge }(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta b_k^g}$$
使用伴随性质，有
$$f_{15}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}(a_b^{k+1}-b_{k+1}^a{)}^{\wedge }(b_k^g\delta t)\delta t^2}{\delta b_k^g}$$
$$f_{15}=\frac{1}{4}{R}_{b_i{b}_k}(a_b^{k+1}-b_{k+1}^a{)}^{\wedge }\delta t^2\delta t$$

示例2

$$g_{12}=\frac{\delta {\alpha }_{b_i{b}_{k+1}}}{\delta n_k^g}$$
一看$n_k^g$就知道又要找和旋转有关的量了。回顾递推公式，有
$$\omega =\frac{1}{2}(({\omega }_b^k+n_k^g-b_k^g)+(w_b^{k+1}+n_{k+1}^g-b_{k+1}^g))$$
$$q_{b_i{b}_{k+1}}=q_{b_i{b}_k}\otimes [1,\frac{1}{2}\omega \delta t{]}^T$$
$$a=\frac{1}{2}(q_{b_i{b}_k}(a_b^k+n_b^k-b_k^a)+q_{b_i{b}_{k+1}}(a_b^{k+1}+n_b^{k+1}-b_{k+1}^a))$$
$${\alpha }_{b_i{b}_{k+1}}={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}a\delta t^2$$
有
$$g_{12}=\frac{\delta {\alpha }_{b_i{b}_{k+1}}}{\delta n_k^g}$$
$$g_{12}=\frac{\delta \frac{1}{2}a\delta t^2}{\delta n_k^g}$$
$$a=\frac{1}{2}(q_{b_i{b}_k}(a_b^k+n_b^k-b_k^a)+q_{b_i{b}_{k+1}}(a_b^{k+1}+n_b^{k+1}-b_{k+1}^a))$$
$$g_{12}=\frac{\delta \frac{1}{4}{q}_{b_i{b}_{k+1}}(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta n_k^g}$$
又因为
$$q_{b_i{b}_{k+1}}=q_{b_i{b}_k}\otimes [1,\frac{1}{2}\omega \delta t{]}^T$$
所以有
$$g_{12}=\frac{\delta \frac{1}{4}{q}_{b_i{b}_k}\otimes [1,\frac{1}{2}\omega \delta t{]}^T(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta n_k^g}$$
$$\omega =\frac{1}{2}(({\omega }_b^k+n_k^g-b_k^g)+(w_b^{k+1}+n_{k+1}^g-b_{k+1}^g))$$
$$g_{12}=\frac{\delta \frac{1}{4}{q}_{b_i{b}_k}\otimes [1,\frac{1}{2}({\omega }_b^k+\frac{1}{2}{n}_k^g)\delta t{]}^T(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta n_k^g}$$
$$g_{12}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}\exp ((({\omega }_b^k+\frac{1}{2}{n}_k^g)\delta t{)}^{\wedge })(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta n_k^g}$$
$$g_{12}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}(\exp (({\omega }_b^k\delta t{)}^{\wedge }))(\exp ((J_r({\omega }_b^k\delta t)\frac{1}{2}{n}_k^g\delta t{)}^{\wedge }))(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta n_k^g}$$
$$g_{12}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}(\exp ((J_r({\omega }_b^k\delta t)\frac{1}{2}{n}_k^g\delta t{)}^{\wedge }))(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta n_k^g}$$
$$g_{12}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}(\exp ((\frac{1}{2}{n}_k^g\delta t{)}^{\wedge }))(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta n_k^g}$$
$$g_{12}=\frac{1}{4}\frac{\delta R_{b_i{b}_k}((\frac{1}{2}{n}_k^g\delta t{)}^{\wedge })(a_b^{k+1}-b_{k+1}^a)\delta t^2}{\delta n_k^g}$$
$$g_{12}=-\frac{1}{4}\frac{\delta R_{b_i{b}_k}(a_b^{k+1}-b_{k+1}^a{)}^{\wedge }(\frac{1}{2}{n}_k^g\delta t)\delta t^2}{\delta n_k^g}$$
$$g_{12}=-\frac{1}{4}{R}_{b_i{b}_k}(a_b^{k+1}-b_{k+1}^a{)}^{\wedge }(\frac{1}{2}\delta t)\delta t^2$$
$$g_{12}=-\frac{1}{8}{R}_{b_i{b}_k}(a_b^{k+1}-b_{k+1}^a{)}^{\wedge }(\delta t)\delta t^2$$

Levenberg-Marquardt方法证明

Levenberg (1944) 和 Marquardt (1963) 先后对高斯牛顿法进行了改进，求解过程中引入了阻尼因子
$$(J^{T}J+\mu I)\Delta x_{lm}=-J^{T}f,\mu >0$$
$$F(x)=\frac{1}{2}\sum _i(f_i(x){)}^2$$
$$J=\frac{\delta f}{\delta x}$$
$$F^{\prime }(x)=(J^{T}f{)}^T$$
$$F^{\prime \prime }(x)=J^{T}J$$

求证

$$J^{T}J=V^T\Lambda V$$
$$\Delta x_{lm}=-\sum _{j=1}^n\frac{v_j^T{F}^{\prime T}}{{\lambda }_j+\mu }{v}_j$$
由原来的式子
$$(J^{T}J+\mu I)\Delta x_{lm}=-J^{T}f$$
进行代换有
$$(V^T\Lambda V+\mu I)\Delta x_{lm}=-J^{T}f$$
$$(V^T\Lambda V+\mu V^{T}V)\Delta x_{lm}=-J^{T}f$$
$$(V^T\Lambda V+V^T\mu V)\Delta x_{lm}=-J^{T}f$$
$$(V^T\Lambda V+V^T\mu IV)\Delta x_{lm}=-J^{T}f$$
$$(V^T(\Lambda +\mu I)V)\Delta x_{lm}=-J^{T}f$$
因为
$$F^{\prime }(x)=(J^{T}f{)}^T$$
故有
$$(V^T(\Lambda +\mu I)V)\Delta x_{lm}=-F^{\prime T}$$
$$V=[v_1,v_2,...v_n]$$
其中$v_j$为列向量
$$V^T=[v_1^T,v_2^T,...v_n^T{]}^T$$
$$\Lambda +\mu I=\left[\begin{array}{ccccc}{\lambda }_1+\mu & & & & \\ & {\lambda }_2+\mu & & & \\ & & {\lambda }_3+\mu & & \\ & & & ...& \\ & & & & {\lambda }_n+\mu \end{array}\right]$$
记为
$$Diag({\lambda }_i+\mu )$$
$$V^{T}Diag({\lambda }_i+\mu )V\Delta x_{lm}=-F^{\prime T}$$
得证
$$\Delta x_{lm}=-\sum _{j=1}^n\frac{v_j^T{F}^{\prime T}}{{\lambda }_j+\mu }{v}_j$$