深蓝学院《从零开始手写VIO》作业三

深蓝学院《从零开始手写VIO》作业三

1. 代码修改

样例代码给出了使用 LM 算法来估计曲线 y = exp(ax2 + bx + c)参数 a, b, c 的完整过程。
1 请绘制样例代码中 LM 阻尼因子 µ 随着迭代变化的曲线图
2 将曲线函数改成 y = ax2 + bx + c，请修改样例代码中残差计算，雅克比计算等函数，完成曲线参数估计。
3 如果有实现其他阻尼因子更新策略可加分（选做）

（1）阻尼因子随着迭代变化的曲线图如下：

（2）代码修改如下：
残差计算：

    virtual void ComputeResidual() override
    {
        Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
        residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2) - y_;
    }

雅克比计算：

    virtual void ComputeJacobians() override
    {
        Vec3 abc = verticies_[0]->Parameters();

        Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维，状态量 3 个，所以是 1x3 的雅克比矩阵
        jaco_abc << x_ * x_, x_ , 1;
        jacobians_[0] = jaco_abc;
    }

计算结果如下（为了达到良好的迭代效果，将数据点从100个调整到1000个）：

Test CurveFitting start...
iter: 0 , chi= 3.21386e+06 , Lambda= 19.95
iter: 1 , chi= 974.658 , Lambda= 6.65001
iter: 2 , chi= 973.881 , Lambda= 2.21667
iter: 3 , chi= 973.88 , Lambda= 1.47778
problem solve cost: 35.4232 ms
   makeHessian cost: 26.0736 ms
-------After optimization, we got these parameters :
0.999588   2.0063 0.968786
-------ground truth: 
1.0,  2.0,  1.0

（3）尝试阻尼因子更新策略如下：$$\begin{array}{l}\text{ if }\rho >0\\ \mu :=\mu \ast \max \left\{\frac{1}{3},1-(2\rho -1{)}^3\right\};\nu :=2\\ \text{ else }\\ \mu :=\mu \ast \nu ;\nu :=2\ast \nu \end{array}$$
修改阻尼因子更新代码如下：

bool Problem::IsGoodStepInLM() {
    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);
    scale += 1e-3;    // make sure it's non-zero :)

    // recompute residuals after update state
    // 统计所有的残差
    double tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }

    double rho = (currentChi_ - tempChi) / scale;
    if (rho > 0.75 && isfinite(tempChi))   // last step was good, 误差在下降
    {
        currentLambda_ *= 1/3;
        currentChi_ = tempChi;
        return true;
    } else {
        currentLambda_ *= 2;
        currentChi_ = tempChi;
        return false;
    }
}

结果如下：

Test CurveFitting start...
iter: 0 , chi= 3.21386e+06 , Lambda= 19.95
iter: 1 , chi= 974.658 , Lambda= 0
iter: 2 , chi= 973.88 , Lambda= 0
problem solve cost: 27.2341 ms
   makeHessian cost: 20.6501 ms
-------After optimization, we got these parameters :
0.999589  2.00628 0.968821
-------ground truth: 
1.0,  2.0,  1.0

2. 公式推导

公式推导，根据课程知识，完成 F, G 中如下两项的推导过程：
$${\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)$$

说明：这里的公式推导约定保持和PPT中的一样的，比如求导公式：$$\frac{\partial {\mathbf{x}}_a}{\partial \delta \mathbf{\theta }}=\underset{\delta \theta \to 0}{\lim }\frac{{\mathbf{R}}_{ab}\exp (\left[[\delta \mathbf{\theta }{]}_{\mathrm{x}}\right){\mathbf{x}}_b-{\mathbf{R}}_{ab}{\mathbf{x}}_b}{\delta \mathbf{\theta }}$$后续直接简写为$$\frac{\partial {\mathbf{x}}_a}{\partial \delta \mathbf{\theta }}=\frac{\partial {\mathbf{R}}_{ab}\exp \left([\delta \mathbf{\theta }{]}_{\times }\right){\mathbf{x}}_b}{\partial \delta \mathbf{\theta }}$$

下面开始推导

（1）公式${\mathbf{f}}_{15}$推导如下：$${\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$$其中$$\begin{array}{rl}\mathbf{a}& =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right)\\ & =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right)\end{array}$$其中$\delta {\mathbf{b}}_k^g$只和$\frac{1}{2}\omega \delta t$这一项有关，因此${\mathbf{f}}_{15}$的推导可以进行如下简化：$$\begin{array}{rl}{\mathbf{f}}_{15}& =\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{b}}_k^g}\\ & =\frac{\partial \frac{1}{4}{\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\delta t\end{array}\right]\otimes \left[\begin{array}{c}1\\ -\frac{1}{2}\delta {\mathbf{b}}_k^g\delta t\end{array}\right]\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\delta t^2}{\partial \delta {\mathbf{b}}_k^g}\\ & =\frac{1}{4}\frac{\partial {\mathbf{R}}_{b_i{b}_{k+1}}\exp \left({\left[-\delta {\mathbf{b}}_k^g\delta t\right]}_{\times }\right)\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\delta t^2}{\partial \delta {\mathbf{b}}_k^g}\\ & =\frac{1}{4}\frac{\partial {\mathbf{R}}_{b_i{b}_{k+1}}\left(\mathbf{I}+{\left[-\delta {\mathbf{b}}_k^g\delta t\right]}_{\times }\right)\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\delta t^2}{\partial \delta {\mathbf{b}}_k^g}\\ & =\frac{1}{4}\frac{\partial -{\mathbf{R}}_{b_i{b}_{k+1}}\left({\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\delta t^2\right]}_{\times }\right)\left(-\delta {\mathbf{b}}_k^g\delta t\right)}{\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)\end{array}$$

（2）公式${\mathbf{g}}_{12}$推导如下：
${\mathbf{g}}_{12}$和${\mathbf{f}}_{15}$的推导是类似的
$${\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$$其中$$\begin{array}{rl}\mathbf{a}& =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right)\\ & =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right)\end{array}$$其中$$\omega =\frac{1}{2}\left(\left({\mathbf{\omega }}^{b_k}+{\mathbf{n}}_k^g-{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}+{\mathbf{n}}_{k+1}^g-{\mathbf{b}}_k^g\right)\right)$$因此${\mathbf{n}}_k^g$只和$\frac{1}{2}\omega \delta t$这一项有关，同理：$$\begin{array}{rl}{\mathbf{f}}_{15}& =\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{b}}_k^g}\\ & =\frac{\partial \frac{1}{4}{\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\delta t\end{array}\right]\otimes \left[\begin{array}{c}1\\ \frac{1}{2}(\frac{1}{2}\delta {\mathbf{n}}_k^g)\delta t\end{array}\right]\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\delta t^2}{\partial \delta {\mathbf{n}}_k^g}\\ & =\frac{1}{4}\frac{\partial {\mathbf{R}}_{b_i{b}_{k+1}}\exp \left({\left[\frac{1}{2}\delta {\mathbf{n}}_k^g\delta t\right]}_{\times }\right)\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\delta t^2}{\partial \delta {\mathbf{n}}_k^g}\\ & =\frac{1}{4}\frac{\partial {\mathbf{R}}_{b_i{b}_{k+1}}\left(\mathbf{I}+{\left[\frac{1}{2}\delta {\mathbf{n}}_k^g\delta t\right]}_{\times }\right)\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\delta t^2}{\partial \delta {\mathbf{n}}_k^g}\\ & =\frac{1}{4}\frac{\partial -{\mathbf{R}}_{b_i{b}_{k+1}}\left({\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\delta t^2\right]}_{\times }\right)\left(\frac{1}{2}\delta {\mathbf{n}}_k^g\delta t\right)}{\partial \delta {\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)(\frac{1}{2}\delta t)\end{array}$$

3. 公式证明：

证明公式：$$\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}=-\sum _{j=1}^n\frac{{\mathbf{v}}_j^{\top }{\mathbf{F}}^{\prime \top }}{{\lambda }_j+\mu }{\mathbf{v}}_j$$

证明主要是利用正规矩阵的谱分解性质，LM公式如下：$$\left({\mathbf{J}}^{\top }\mathbf{J}+\mu \mathbf{I}\right)\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}=-{\mathbf{J}}^{\top }\mathbf{f}$$对${\mathbf{J}}^{\top }\mathbf{J}$进行特征值分解，并加以变换：$$\left(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^{\top }+\mu \mathbf{I}\right)\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}=-{{\mathbf{F}}^{\prime }}^{\top }$$$$\mathbf{V}\left(\mathbf{\Lambda }+\mu \mathbf{I}\right){\mathbf{V}}^{\top }\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}=-{{\mathbf{F}}^{\prime }}^{\top }$$$$\Delta {\mathbf{x}}_{\mathrm{l}\mathrm{m}}=-\mathbf{V}{\left(\mathbf{\Lambda }+\mu \mathbf{I}\right)}^{-1}{\mathbf{V}}^{\top }{{\mathbf{F}}^{\prime }}^{\top }$$根据正规矩阵的谱分解有：$$\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$$