从零手写VIO第三讲作业（LM算法和IMU误差传递）

一、非线性最小二乘问题

定义：找到一个 n 维的变量，${\mathbf{x}}^{\ast }\in {\mathbb{R}}^n$，使得损失函数$F(\mathbf{x})$取局部最小值：
$$F(\mathbf{x})=\frac{1}{2}\sum _{i=1}^m{\left(f_i(\mathbf{x})\right)}^2$$

1.1 GN(高斯牛顿)算法

残差函数$\mathbf{f}(\mathbf{x})$为非线性函数，对其一阶泰勒近似有:
$$\mathbf{f}(\mathbf{x}+\Delta \mathbf{x})\approx \ell (\Delta \mathbf{x})\equiv \mathbf{f}(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}$$
请特别注意，这里的 J 是残差函数 f 的雅克比矩阵。代入损失函数：
$$\begin{array}{rl}F(\mathbf{x}+\Delta \mathbf{x})\approx L(\Delta \mathbf{x})& \equiv \frac{1}{2}\ell (\Delta \mathbf{x}{)}^{\top }\ell (\Delta \mathbf{x})\\ & =\frac{1}{2}{\mathbf{f}}^{\top }\mathbf{f}+\Delta {\mathbf{x}}^{\top }{\mathbf{J}}^{\top }\mathbf{f}+\frac{1}{2}\Delta {\mathbf{x}}^{\top }{\mathbf{J}}^{\top }\mathbf{J}\Delta \mathbf{x}\\ & =F(\mathbf{x})+\Delta {\mathbf{x}}^{\top }{\mathbf{J}}^{\top }\mathbf{f}+\frac{1}{2}\Delta {\mathbf{x}}^{\top }{\mathbf{J}}^{\top }\mathbf{J}\Delta \mathbf{x}\end{array}（1）$$
若直接对损失函数进行二阶泰勒展开，有
$$F(\mathbf{x}+\Delta \mathbf{x})\approx L(\Delta \mathbf{x})\equiv F(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^{\top }\mathbf{H}\Delta \mathbf{x}（2）$$
对上述两式对比，易得
$$F^{\prime }(\mathbf{x})={\left({\mathbf{J}}^{\top }\mathbf{f}\right)}^{\top },\text{ 以及 }{F}^{\prime \prime }(\mathbf{x})\approx {\mathbf{J}}^{\top }\mathbf{J}$$
令公式（1）的一阶导等于0，得到：
$$\left({\mathbf{J}}^{\top }\mathbf{J}\right)\Delta {\mathbf{x}}_{\mathrm{g}\mathrm{n}}=-{\mathbf{J}}^{\top }\mathbf{f}（3）$$
GN法由此计算步长

GN法存在的问题：

1.GN法利用了函数的二阶近似，若二次函数在某点附近不能很好拟合原函数，则步长计算方式不合理
2.${\mathbf{J}}^{\top }\mathbf{J}$为半正定矩阵，若$J$不满秩，即$|J|=0$，则${\mathbf{J}}^{\top }\mathbf{J}$不可逆，则不能求出步长。

1.2 LM算法

LM算法为在GN算法的基础上添加阻尼因子$\mu$。
$$\left({\mathbf{J}}^{\top }\mathbf{J}+\mu \mathbf{I}\right)\Delta {\mathbf{x}}_{\mathrm{lm}}=-{\mathbf{J}}^{\top }\mathbf{f}\text{ with }\mu \ge 0$$

阻尼因子作用：
$$\begin{array}{rl}& \mu >0\text{ 保证 }\left({\mathbf{J}}^{\top }\mathbf{J}+\mu \mathbf{I}\right)\text{ 正定，迭代朝着下降方向进行。 }\\ & \mu \text{ 非常大，则 }\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}=-\frac{1}{\mu }{\mathbf{J}}^{\top }\mathbf{f}=-\frac{1}{\mu }{F}^{\prime }(\mathbf{x}{)}^{\top },\text{ 接近最速下降法. }\\ & \mu \text{ 比较小，则 }\Delta {\mathbf{x}}_{\mathrm{I}\mathrm{m}}\approx \Delta {\mathbf{x}}_{\mathrm{g}\mathrm{n}},\text{ 接近高斯牛顿法。 }\end{array}$$
阻尼因子更新策略通过比例因子来确定的：
$$\rho =\frac{F(\mathbf{x})-F\left(\mathbf{x}+\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}\right)}{L(0)-L\left(\Delta {\mathbf{x}}_{\mathrm{lm}}\right)}$$
分母表示二次近似函数步进$\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}$后所下降的值，分子为原函数步进$\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}$后实际下降的值。

1.2.1阻尼因子$\mu$以及步长 $\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}$初始化

阻尼因子$\mu$大小是相对于${\mathbf{J}}^{\top }\mathbf{J}$的元素而言的。半正定的信息矩阵${\mathbf{J}}^{\top }\mathbf{J}$特征值 { λ j } \left\{\lambda_{j}\right\} {λj​}和对应的特征向量为 { v j } \left\{\mathbf{v}_{j}\right\} {vj​}。对${\mathbf{J}}^{\top }\mathbf{J}$做特征值分解分解后有：${\mathbf{J}}^{\top }\mathbf{J}=\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^{\top }$可得：
$$\Delta {\mathbf{x}}_{\mathrm{lm}}=-\sum _{j=1}^n\frac{{\mathbf{v}}_j^{\top }{\mathbf{F}}^{\prime \top }}{{\lambda }_j+\mu }{\mathbf{v}}_j$$
第三题证明：
$$\left({\mathbf{J}}^{\top }\mathbf{J}+\mu \mathbf{I}\right)\Delta {\mathbf{x}}_{\mathrm{lm}}=-{\mathbf{J}}^{\top }\mathbf{f}$$
对${\mathbf{J}}^{\top }\mathbf{J}$特征值分解（实对称矩阵一定可以相似对角化）
$$\begin{array}{c}\left(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^{\top }+\mu \mathbf{I}\right)\Delta {\mathbf{x}}_{\mathrm{lm}}=-{\mathbf{F}}^{\prime \top }\\ \mathbf{V}(\mathbf{\Lambda }+\mu \mathbf{I}){\mathbf{V}}^{\top }\Delta {\mathbf{x}}_{\mathrm{lm}}=-{\mathbf{F}}^{\prime \top }\\ \Delta {\mathbf{x}}_{\mathrm{lm}}=-\mathbf{V}(\mathbf{\Lambda }+\mu \mathbf{I}{)}^{-1}{\mathbf{V}}^{\top }{\mathbf{F}}^{\prime \top }\end{array}$$
有
$$\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}=-\sum _{j=1}^n\frac{{\mathbf{v}}_j{\mathbf{v}}_j^{\top }}{{\lambda }_j+\mu }{\mathbf{F}}^{\prime \top }$$
因为分子后两项为标量，移到前面有
$$\Delta {\mathbf{x}}_{1\mathrm{m}}=-\sum _{j=1}^n\frac{{\mathbf{v}}_j^{\top }{\mathbf{F}}^{\prime \top }}{{\lambda }_j+\mu }{\mathbf{v}}_j$$
所以，一个简单的 ${\mu }_0$初始值的策略就是：
$${\mu }_0=\tau \cdot \max \left\{{\left({\mathbf{J}}^{\top }\mathbf{J}\right)}_{ii}\right\}$$
通常，按需设定 $\tau \sim \left[10^{-8},1\right]$
若初始值离最小值点较近，则二阶泰勒展开能够很好近似， $\tau$取小一点，否则取大一点。

1.2.2 Marquardt策略

$$\begin{array}{rl}\text{ if }\rho & <0.25\\ \mu & :=\mu \ast 2\\ \text{ elseif }\rho & >0.75\\ \mu & :=\mu /3\end{array}$$

1.2.2.1 拟合$y=\exp \left(a{x}^2+bx+c\right)$

以估计$y=\exp \left(a{x}^2+bx+c\right)$参数a,b,c为例，
核心代码：

double rho = (currentChi_ - tempChi) / scale;
if(rho > 0 && rho < 0.25){
    currentLambda_ *= 2;
    currentChi_ = tempChi;
    return true;
}
if(rho > 0.75 && isfinite(tempChi)){
    currentLambda_ /= 3;
    currentChi_ = tempChi;
    return true;
}
else if(rho >= 0.25 && rho <= 0.75 ){
    currentLambda_ = currentLambda_;
    currentChi_ = tempChi;
    return true;
}
else if(rho < 0){
    currentLambda_ *= 2;;
    return false;
}

实验结果：

$\mu$随迭代次数变化：

$F,|F^{\prime }|,\mu$随迭代次数的变化趋势（为放在同一个坐标系里，三组数据已全部做归一化处理）

1.2.3 Nielsen策略（论文中第三种方法）

   if $\rho >0$
    $\mu :=\mu \ast \max \left\{\frac{1}{3},1-(2\rho -1{)}^3\right\};\nu :=2$
   else
      $\mu :=\mu \ast \nu ;\nu :=2\ast \nu$

1.2.3.1 拟合$y=\exp \left(a{x}^2+bx+c\right)$

核心代码：

 double rho = (currentChi_ - tempChi) / scale;
 if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
 {
     double alpha = 1. - pow((2 * rho - 1), 3);
     alpha = std::min(alpha, 2. / 3.);
     double scaleFactor = (std::max)(1. / 3., alpha);
     currentLambda_ *= scaleFactor;
     ni_ = 2;
     currentChi_ = tempChi;
     return true;
 } else {
     currentLambda_ *= ni_;
     ni_ *= 2;
     return false;
 }

实验结果：

$\mu$随迭代次数变化：
$F,|F^{\prime }|,\mu$随迭代次数的变化趋势（为放在同一个坐标系里，三组数据已全部做归一化处理）

1.2.3.2 拟合$y=a{x}^2+bx+c$

核心代码：

 double rho = (currentChi_ - tempChi) / scale;
 if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
 {
     double alpha = 1. - pow((2 * rho - 1), 3);
     alpha = std::min(alpha, 2. / 3.);
     double scaleFactor = (std::max)(1. / 3., alpha);
     currentLambda_ *= scaleFactor;
     ni_ = 2;
     currentChi_ = tempChi;
     return true;
 } else {
     currentLambda_ *= ni_;
     ni_ *= 2;
     return false;
 }

实验结果：

$\mu$随迭代次数变化：
$F,|F^{\prime }|,\mu$随迭代次数的变化趋势（为放在同一个坐标系里，三组数据已全部做归一化处理）

1.2.4 论文中第二种方法拟合$y=\exp \left(a{x}^2+bx+c\right)$

${\lambda }_0={\lambda }_o\max \left[\mathrm{diag}\left[{\mathbf{J}}^{\top }\mathbf{W}\mathbf{J}\right]\right];{\lambda }_o$ is user-specified.
use eq’n (12) for ${\mathbf{h}}_{\text{Im }}$ and eq’n (15) for $\rho$ $\alpha =\left({\left({\mathbf{J}}^{\top }\mathbf{W}(\mathbf{y}-\hat{\mathbf{y}}(\mathbf{p}))\right)}^{\top }\mathbf{h}\right)/\left(\left({\chi }^2(\mathbf{p}+\mathbf{h})-{\chi }^2(\mathbf{p})\right)/2+2{\left({\mathbf{J}}^{\top }\mathbf{W}(\mathbf{y}-\hat{\mathbf{y}}(\mathbf{p}))\right)}^{\top }\mathbf{h}\right)$
if ${\rho }_i(\alpha \mathbf{h})>{ϵ}_4:\mathbf{p}\leftarrow \mathbf{p}+\alpha \mathbf{h};{\lambda }_{i+1}=\max \left[{\lambda }_i/(1+\alpha ),10^{-7}\right]$
otherwise: ${\lambda }_{i+1}={\lambda }_i+\left|{\chi }^2(\mathbf{p}+\alpha \mathbf{h})-{\chi }^2(\mathbf{p})\right|/(2\alpha )$

核心代码：
bool Problem::IsGoodStepInLM() {
    ofstream out("/home/ubuntu/slam/study/vio/HW3/course3_hw_CurveFitting_LM/exp_2/exp_2.txt",ios::app);
    double scale = 0;
    // recompute residuals after update state
    // 统计所有的残差
    double tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }
    //回退
    RollbackStates();

    //计算alpha
    VecX tempDelta = delta_x_;
    alpha_ = b_.transpose() * delta_x_;
    alpha_ /= (tempChi-currentChi_)/2 + 2*b_.transpose()*delta_x_;
    alpha_ += 1e-1;
    delta_x_ = delta_x_ * alpha_;
    UpdateStates();

    tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);
    scale += 1e-3;    // make sure it's non-zero :)
    double rho = (currentChi_ - tempChi) / scale;

    if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
    {
        currentLambda_ = max(currentLambda_/(1+alpha_),1e-7);
        currentChi_ = tempChi;
        out<<"F: "<<currentChi_<<" ";
        out<<"F': "<<sqrt(b_.transpose()*b_)<<" ";
        out<<"u: "<<currentLambda_<<" "<<endl;
        out.close();
        return true;
    } else {
        currentLambda_ += abs(tempChi-currentChi_)/(2*alpha_);
        out.close();
        return false;
    }
}

实验结果：

$\mu$随迭代次数变化：

$F,|F^{\prime }|,\mu$随迭代次数的变化趋势（为放在同一个坐标系里，三组数据已全部做归一化处理）

对比

利用三种方法对曲线$y=\exp \left(a{x}^2+bx+c\right)$拟合结果来看，三者迭代次数相近，值得注意的是论文中第二种方法在刚开始迭代时梯度下降比其他两种快，也比其他两种方式更快收敛。

二、IMU预积分误差传递推导

2.1 IMU预积分

$$\begin{array}{rl}\omega & =\frac{1}{2}\left(\left({\bar{\omega }}^{b_k}+{\mathbf{n}}_k^g-{\mathbf{b}}_k^g\right)+\left({\bar{\omega }}^{b_{k+1}}+{\mathbf{n}}_{k+1}^g-{\mathbf{b}}_k^g\right)\right)\\ {\mathbf{q}}_{b_i{b}_{k+1}}& ={\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\\ \mathbf{a}& =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}\left({\overline{\mathbf{a}}}^{b_k}+{\mathbf{n}}_k^a-{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_{k+1}}\left({\overline{\mathbf{a}}}^{b_{k+1}}+{\mathbf{n}}_{k+1}^a-{\mathbf{b}}_k^a\right)\right)\\ {\mathbf{\alpha }}_{b_i{b}_{k+1}}& ={\mathbf{\alpha }}_{b_i{b}_k}+{\mathbf{\beta }}_{b_i{b}_k}\delta t+\frac{1}{2}\mathbf{a}\delta t^2\\ {\mathbf{\beta }}_{b_i{b}_{k+1}}& ={\mathbf{\beta }}_{b_i{b}_k}+\mathbf{a}\delta t\\ {\mathbf{b}}_{k+1}^a& ={\mathbf{b}}_k^a+{\mathbf{n}}_{{\mathbf{b}}_k^a}\delta t\\ {\mathbf{b}}_{k+1}^g& ={\mathbf{b}}_k^g+{\mathbf{n}}_{{\mathbf{b}}_l^g\delta t}\end{array}$$

2.2 预积分的误差传递形式

$$\left[\begin{array}{c}\delta {\mathbf{\alpha }}_{b_{k+1}}\\ \delta {\mathbf{\theta }}_{b_{k+1}}\\ \delta {\mathbf{\beta }}_{b_{k+1}}\\ \delta {\mathbf{b}}_{k+1}^a\\ \delta {\mathbf{b}}_{k+1}^g\end{array}\right]=\mathbf{F}\left[\begin{array}{c}\delta {\mathbf{\alpha }}_{b_k}\\ \delta {\mathbf{\theta }}_{b_k}\\ \delta {\mathbf{\beta }}_{b_k}\\ \delta {\mathbf{b}}_k^a\\ \delta {\mathbf{b}}_k^g\end{array}\right]+\mathbf{G}\left[\begin{array}{c}{\mathbf{n}}_k^a\\ {\mathbf{n}}_k^g\\ {\mathbf{n}}_{k+1}^a\\ {\mathbf{n}}_{k+1}^g\\ {\mathbf{n}}_{{\mathbf{b}}_k^a}\\ {\mathbf{n}}_{{\mathbf{b}}_k^g}\end{array}\right]$$
其中
$$\mathbf{F}=\left[\begin{array}{ccccc}\mathbf{I}& {\mathbf{f}}_{12}& \mathbf{I}\delta t& -\frac{1}{4}\left({\mathbf{q}}_{b_i{b}_k}+{\mathbf{q}}_{b_i{b}_{k+1}}\right)\delta t^2& {\mathbf{f}}_{15}\\ 0& \mathbf{I}-[\mathbf{\omega }{]}_{\times }\delta t& 0& 0& -\mathbf{I}\delta t\\ 0& {\mathbf{f}}_{32}& \mathbf{I}& -\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}+{\mathbf{q}}_{b_i{b}_{k+1}}\right)\delta t& {\mathbf{f}}_{35}\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]$$
$$\mathbf{G}=\left[\begin{array}{cccccc}\frac{1}{4}{\mathbf{q}}_{b_i{b}_k}\delta t^2& {\mathbf{g}}_{12}& \frac{1}{4}{\mathbf{q}}_{b_i{b}_{k+1}}\delta t^2& {\mathbf{g}}_{14}& 0& 0\\ 0& \frac{1}{2}\mathbf{I}\delta t& 0& \frac{1}{2}\mathbf{I}\delta t& 0& 0\\ \frac{1}{2}{\mathbf{q}}_{b_i{b}_k}\delta t& {\mathbf{g}}_{32}& \frac{1}{2}{\mathbf{q}}_{b_i{b}_{k+1}}\delta t& {\mathbf{g}}_{34}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}\delta t& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\delta t\end{array}\right]$$
下推导
$${\mathbf{f}}_{15}=\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{b}}_k^g}=-\frac{1}{4}\left({\mathbf{R}}_{b_i{b}_{k+1}}{\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]}_{\times }\delta t^2\right)(-\delta t)$$

下推导
$${\mathbf{g}}_{12}=\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial {\mathbf{n}}_k^g}=-\frac{1}{4}\left({\mathbf{R}}_{b_i{b}_{k+1}}{\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]}_{\times }\delta t^2\right)\left(\frac{1}{2}\delta t\right)$$