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

1. Levenberg-Marquardt 算法

LM 算法有两种推导和实现，一种是算法发明者使用的阻尼法，一种是后来学者补充的置信域法,参考知乎：https://zhuanlan.zhihu.com/p/136143299
vio课件中讲的是阻尼法，slam十四讲第二版中是置信域法

Levenberg-Marquardt Method 阻尼法推导

可见Marquardt 策略有较严重的震荡，增加了无意义的计算量，之后采用Nielsen 策略去近似该曲线的方式避免震荡：

Levenberg-Marquardt Method 置信域法推导

1.1 请绘制样例代码中 LM 阻尼因子 µ 随着迭代变化的曲线图

在problem.cc中增加以下存csv文件代码：

    ///存csv文件
    ostringstream csvfilename;
    std::ofstream Lambda_data;
    csvfilename << "Lambda.csv";
    Lambda_data.open(csvfilename.str(), ios_base::out);

    while (!stop && (iter < iterations)) {
        std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_
                  << std::endl;
        
        Lambda_data << currentChi_ << "," << currentLambda_ << endl;

画图：

import numpy as np;
import matplotlib
import matplotlib.pyplot as plt
import os
import csv

filepath = os.path.abspath('.')
Chi_ = []
Lambda_ = []

#用reder读取csv文件
with open('/home/jevin/code/vio-homework/hw3/Lambda.csv','r') as csvFile:
    reader = csv.reader(csvFile)
    for line in reader:
        print line
        # odom = line.split()        #将单个数据分隔开存好  
        numbers_float = map(float, line) #转化为浮点数  
        Chi_.append( numbers_float[0] )
        Lambda_.append( numbers_float[1] )

plt.figure(1)  
plt.plot(Chi_)

plt.figure(2)
plt.plot(Lambda_)
plt.show()

残差：

阻尼因子$\mu$：

说明：这里画出来的都是成功迭代一次后残差与阻尼因子$\mu$，不满足更新策略的中间结果省去

1.2 将曲线函数改成 $y=a{x}^2+bx+c$，请修改样例代码中残差计算，雅克比计算等函数，完成曲线参数估计。

    // 构造 N 次观测
    for (int i = 0; i < N; ++i) {

        double x = i/100.;
        double n = noise(generator);
        // 观测 y
        double y = a*x*x + b*x + c + n;

    // 计算曲线模型误差
    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
    {
        Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维，状态量 3 个，所以是 1x3 的雅克比矩阵
        jaco_abc << x_ * x_ , x_ , 1 ;
        jacobians_[0] = jaco_abc;
    }

点数为100，数据较少受噪声影响大迭代效果不好：

设置数据点数为1000：

再继续增加数据点数至3000：

发现初始参数设置不好迭代一次就停止，修改初始参数如下：

    // 设定待估计参数 a, b, c初始值
    vertex->SetParameters(Eigen::Vector3d (1.,1.,1.));

计算结果如下：

1.3 实现其他更优秀的阻尼因子策略

参考论文Henri Gavin. “The Levenberg-Marquardt method for nonlinear least squares curve-fitting problems”
中 4.1.1 节，迭代之后若误差增大(说明步长不够小近似效果不好，则不使用高斯牛顿的二阶方法)则增大 $\lambda$ (同时丢弃此次迭代)，即增大阻尼减小迭代步长，此时也更近似最速下降法。反之好的迭代效果下$\lambda$不断减小。

1.3.1 策略1

策略1中使用上图式13，16更新，不同于另两种策略，除IsGoodStepInLM()还需修改AddLambdatoHessianLM()，RemoveLambdaHessianLM()

void Problem::AddLambdatoHessianLM() {
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    for (ulong i = 0; i < size; ++i) {
        Hessian_(i, i) *= (1.+currentLambda_);
    }
}

void Problem::RemoveLambdaHessianLM() {
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    // TODO:: 这里不应该减去一个，数值的反复加减容易造成数值精度出问题？而应该保存叠加lambda前的值，在这里直接赋值
    for (ulong i = 0; i < size; ++i) {
        Hessian_(i, i) /= (1.+currentLambda_);
    }
}

bool Problem::IsGoodStepInLM() {

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

    ulong size = Hessian_.cols();
    MatXX diag_hessian(MatXX::Zero(size, size));
    for (ulong i = 0; i < size; ++i) {
        diag_hessian(i, i) = Hessian_(i, i);
    }

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

    double rho = (currentChi_ - tempChi) / scale;
//    std::cout << "rho = " << rho << std::endl;

    double L_down = 9.0;
    double L_up = 11.0;

    if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
    {
        currentLambda_ = std::max(currentLambda_ / L_down, 1e-7);
        currentChi_ = tempChi;
        return true;
    } else {
        currentLambda_ = std::min(currentLambda_ * L_up, 1e7);
        return false;
    }
}

1.3.2 策略2

bool Problem::IsGoodStepInLM() {

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

    double Numerator = b_.transpose() * delta_x_;
    double alpha = Numerator / ((currentChi_ - tempChi)/2. + 2.*Numerator);
    alpha = std::max(alpha, 1e-1);
//    cout << "Numerator:" << Numerator << endl;
//    cout << "alpha:" << alpha << endl;

    RollbackStates();
    delta_x_ *=alpha;
    UpdateStates();

    tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }

    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);
    scale += 1e-3;    // make sure it's non-zero :)
    double rho = (currentChi_ - tempChi) / scale;
//    std::cout << "rho = " << rho << std::endl;

    if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
    {
        currentLambda_ = std::max(currentLambda_ / (1.+alpha), 1e-7);
        currentChi_ = tempChi;
        return true;
    } else {
        currentLambda_ += abs(currentChi_ - tempChi)/(2.*alpha);
        return false;
    }
}

1.3.1 策略3 (对应原代码)

原代码中已实现上述策略3，即Nielsen 策略：

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 && 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;
    }
}

2. 公式推导，完成 F, G 中如下两项的推导过程

预积分总结与公式推导

2.1 推导$f_{15}$

类比如下$f_{35}$推导过程

$${\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$$

只有含有$\omega$的与$\delta b_k^g$有关，前两项与$\delta b_k^g$无关直接略去，其中
$$\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+1}}\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}$$
其中
$$\mathbf{\omega }=\frac{1}{2}\left(\left({\overline{\mathbf{\omega }}}^{b_k}+{\mathbf{n}}_k^g-{\mathbf{b}}_k^g\right)+\left({\overline{\mathbf{\omega }}}^{b_{k+1}}+{\mathbf{n}}_{k+1}^g-{\mathbf{b}}_k^g\right)\right)$$
而${\mathbf{n}}_k^g$与${\mathbf{n}}_{k+1}^g$表现在矩阵$\mathbf{G}$中，在此考虑
$$\mathbf{\omega }=\frac{1}{2}\left({\overline{\mathbf{\omega }}}^{b_k}+{\overline{\mathbf{\omega }}}^{b_{k+1}}\right)-{\mathbf{b}}_k^g$$
只考虑增量$\delta {\mathbf{b}}_k^g$部分得：

$$\begin{array}{rl}{\mathbf{f}}_{15}& =\frac{\partial \delta {\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}\left(\mathbf{\omega }-\delta {\mathbf{b}}_k^g\right)\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}\exp \left({\left[\left(\mathbf{\omega }-\delta {\mathbf{b}}_k^g\right)\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}\exp \left({\left[\mathbf{\omega }\delta t\right]}_{\times }\right)\exp \left({\left[-J_r(\mathbf{\omega }\delta t)\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}\\ & \approx \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(-J_r(\mathbf{\omega }\delta t)\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)\left(-J_r(\mathbf{\omega }\delta t)\delta t\right)\\ & \approx -\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(-\delta t\right)\end{array}$$

其中
$${\mathbf{R}}_{b_i{b}_k}\exp \left({\left[\mathbf{\omega }\delta t\right]}_{\times }\right)={\mathbf{R}}_{b_i{b}_{k+1}}$$

2.2 推导$g_{12}$

同理
$$\mathbf{\omega }=\frac{1}{2}\left(\left({\overline{\mathbf{\omega }}}^{b_k}+{\mathbf{n}}_k^g-{\mathbf{b}}_k^g\right)+\left({\overline{\mathbf{\omega }}}^{b_{k+1}}+{\mathbf{n}}_{k+1}^g-{\mathbf{b}}_k^g\right)\right)$$
在矩阵$\mathbf{G}$中考虑
$$\mathbf{\omega }=\frac{1}{2}\left({\overline{\mathbf{\omega }}}^{b_k}+{\overline{\mathbf{\omega }}}^{b_{k+1}}\right)+\frac{1}{2}{\mathbf{n}}_k^g+\frac{1}{2}{\mathbf{n}}_{k+1}^g$$
只考虑增量$\delta {\mathbf{n}}_k^g$部分得：

$$\begin{array}{rl}{\mathbf{g}}_{12}& =\frac{\partial \delta {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{n}}_k^g}\\ & =\frac{\partial \frac{1}{4}{\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\left(\mathbf{\omega }+\frac{1}{2}\delta {\mathbf{n}}_k^g\right)\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}\exp \left({\left[\left(\mathbf{\omega }+\frac{1}{2}\delta {\mathbf{n}}_k^g\right)\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}\exp \left({\left[\mathbf{\omega }\delta t\right]}_{\times }\right)\exp \left({\left[J_r(\mathbf{\omega }\delta t)\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}\\ & \approx \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(J_r(\mathbf{\omega }\delta t)\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)\left(\frac{1}{2}{J}_r(\mathbf{\omega }\delta t)\delta t\right)\\ & \approx -\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)\end{array}$$

3. 证明式(9)

$$\mathbf{V}{\mathbf{V}}^{\top }=\mathbf{I}$$
$$\begin{array}{c}\left({\mathbf{J}}^{\top }\mathbf{J}+\mu \mathbf{I}\right)\Delta {\mathbf{x}}_{\mathrm{lm}}=-{\mathbf{J}}^{\top }\mathbf{f}\\ \left(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^{\top }+\mu \mathbf{I}\right)\Delta {\mathbf{x}}_{\mathrm{lm}}=-{\mathbf{F}}^{\prime \top }\\ \left(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^{\top }+\mu \mathbf{V}{\mathbf{V}}^{\top }\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 }\end{array}$$

$$\begin{array}{rl}\Delta {\mathbf{x}}_{\mathrm{lm}}& =-\mathbf{V}(\mathbf{\Lambda }+\mu \mathbf{I}{)}^{-1}{\mathbf{V}}^{\top }{\mathbf{F}}^{\prime }\\ & =-\left[\begin{array}{lcc}{\mathbf{v}}_1{\mathbf{v}}_2& \cdots & {\mathbf{v}}_3\end{array}\right]\left[\begin{array}{cccc}\frac{1}{{\lambda }_1+\mu }& & & \\ & \frac{1}{{\lambda }_2+\mu }& & \\ & & \ddots & \\ & & & \frac{1}{{\lambda }_n+\mu }\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_1^T\\ {\mathbf{v}}_2^T\\ ⋮\\ {\mathbf{v}}_n^T\end{array}\right]{\mathbf{F}}^{\prime T}\\ & =-\left[\begin{array}{lcc}{\mathbf{v}}_1{\mathbf{v}}_2& \cdots & {\mathbf{v}}_3\end{array}\right]\left[\begin{array}{c}\frac{{\mathbf{v}}_1^T{\mathbf{F}}^{\prime T}}{{\lambda }_1+\mu }\\ \frac{{\mathbf{v}}_2^T{\mathbf{F}}^{\prime T}}{{\lambda }_2+\mu }\\ ⋮\\ \frac{{\mathbf{v}}_n^T{\mathbf{F}}^{\prime T}}{{\lambda }_n+\mu }\end{array}\right]\\ & =-\left(\frac{{\mathbf{v}}_1^T{\mathbf{F}}^{\prime T}}{{\lambda }_1+\mu }{\mathbf{v}}_1+\frac{{\mathbf{v}}_2^T{\mathbf{F}}^{\prime T}}{{\lambda }_2+\mu }{\mathbf{v}}_2+\cdots +\frac{{\mathbf{v}}_n^T{\mathbf{F}}^{\prime T}}{{\lambda }_n+\mu }{\mathbf{v}}_n\right)=-\sum _{j=1}^n\frac{{\mathbf{v}}_j^T{\mathbf{F}}^{\prime T}}{{\lambda }_j+\mu }{\mathbf{v}}_j\end{array}$$