《视觉SLAM进阶：从零开始手写VIO》第三讲 基于优化的IMU预积分与视觉信息融合 作业

1 绘制阻尼因子随迭代变化的曲线

1.1 编译：

mkdir build
cd build
cmake ..
make
cd app
./testCurveFitting  

运行结果

：

Test CurveFitting start...
iter: 0 , chi= 36048.3 , Lambda= 0.001
iter: 1 , chi= 30015.5 , Lambda= 699.051
iter: 2 , chi= 13421.2 , Lambda= 1864.14
iter: 3 , chi= 7273.96 , Lambda= 1242.76
iter: 4 , chi= 269.255 , Lambda= 414.252
iter: 5 , chi= 105.473 , Lambda= 138.084
iter: 6 , chi= 100.845 , Lambda= 46.028
iter: 7 , chi= 95.9439 , Lambda= 15.3427
iter: 8 , chi= 92.3017 , Lambda= 5.11423
iter: 9 , chi= 91.442 , Lambda= 1.70474
iter: 10 , chi= 91.3963 , Lambda= 0.568247
iter: 11 , chi= 91.3959 , Lambda= 0.378832
problem solve cost: 0.994615 ms
   makeHessian cost: 0.253562 ms
-------After optimization, we got these parameters :
0.941939  2.09453 0.965586
-------ground truth: 
1.0,  2.0,  1.0

方便调试，写个

build.sh

，以后只需要运行

./build.sh

就可以了。

#!/bin/bash
echo "Configuring and building ..."
if [ ! -d "build" ]; then
    mkdir build
fi
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4
cd app
./testCurveFitting

1.2 阻尼因子 µ 随着迭代变化的曲线图

参考方法1：

https://blog.csdn.net/weixin_44456692/article/details/106875299

思路是将代码中的lamada保存为lamada.txt，然后使用Python绘制出图。

参考方法2：

https://blog.csdn.net/cuifeng1993/article/details/107582589

思路是将上图中的lambda值整理后，用MATLAB生成对应的曲线图。

这里数据少，我们用方法2。

x=(0:11);%迭代次数
y=[0.001, 699.051, 1864.14, 1242.76, 414.252, 138.084, 46.028, 15.3427, 5.11423, 1.70474, 0.568247, 0.378832];%阻尼因子序列
plot (x,y,'-o','LineWidth',1.5);grid on;%画图

特别说明：图中的阻尼因子序列并不包含迭代过程中计算出的所有阻尼因子，只有满足所使用的阻尼因子更新策略中阻尼因子输出条件的才会被记录。程序中使用的是

Nielsen

策略（具体实现查看

problem.cc

中的

IsGoodStepInLM

函数）。

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

1.3 完成曲线y = ax

²

+ bx + c的参数估计

第一步：将main函数中的观测值

y = std::exp(ax^2^ + bx + c)

改为

y = ax^2^+ bx + c


// 构造 N 次观测
    for (int i = 0; i < N; ++i) {
        double x = i/100.;
        double n = noise(generator);
        // 观测 y
        // double y = std::exp( a*x*x + b*x + c ) + n;
        double y = a*x*x + b*x + c + n;

第二步：残差计算(

main函数

中)

// 计算曲线模型误差
    virtual void ComputeResidual() override
    {
        Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
        //  residual_(0) = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ) - y_;  // 构建残差 = 预测值-测量值
        residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2)  - y_;  // 构建残差
    }

第三步：计算残差对变量的雅克比(

main函数

中)

    // 计算残差对变量的雅克比
    virtual void ComputeJacobians() override
    {
        // Vec3 abc = verticies_[0]->Parameters();
        // double exp_y = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) );

        Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维，状态量 3 个，所以是 1x3 的雅克比矩阵
        // jaco_abc << x_ * x_ * exp_y, x_ * exp_y , 1 * exp_y; // 分别对a b c 求导得到jaco_abc

        jaco_abc << x_ * x_, x_, 1; // y = a*x*x + b*x + c 的雅可比矩阵
        jacobians_[0] = jaco_abc;
    }

运行

./build.sh

，结果如下：

Test CurveFitting start...
iter: 0 , chi= 719.475 , Lambda= 0.001
iter: 1 , chi= 91.395 , Lambda= 0.000333333
problem solve cost: 0.309495 ms
   makeHessian cost: 0.03713 ms
-------After optimization, we got these parameters :
 1.61039  1.61853 0.995178
-------ground truth: 
1.0,  2.0,  1.0

可以发现参数估计误差较大。由于只修改了原代码中需要估计的曲线参数，并不会引起算法失效，初步分析，误差较大的原因可能是数据点数量不足。于是将数据点

由100增加到1000


第四处修改：

int main()
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N = 1000;                          // 数据点
    double w_sigma= 1.;                 // 噪声Sigma值

运行

./build.sh

，结果如下：

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: 1.08467 ms
   makeHessian cost: 0.621147 ms
-------After optimization, we got these parameters :
0.999588   2.0063 0.968786
-------ground truth: 
1.0,  2.0,  1.0

可以看出，参数估计精度已经足够高。

2 实现其他阻尼因子更新策略

参考博客：

https://blog.csdn.net/cuifeng1993/article/details/107582589

课件中提示可以参考论文

《The Levenberg-Marquardt method for nonlinear least squares curve-fitting problems》

中的阻尼因子策略。该论文中共有三种阻尼因子策略，如下图：

第三种策略与样例代码中的策略一样，都是前面提到的Nielsen 策略。

下面在

原样例代码

problem.cc的基础上实现论文中的第一种阻尼因子策略。（恢复之前的修改）


未完.........等待更新.....


公式推导

参考

公式推导

参考

1. 
   ^
   
   SBA: A Software Package for Generic Sparse Bundle Adjustment
   
   http://users.ics.forth.gr/~lourakis/sba/sba-toms.pdf
   
2. 
   ^
   
   Zach C . Robust Bundle Adjustment Revisited[J]. 2014.
   
   http://vigir.missouri.edu/~gdesouza/Research/Conference_CDs/ECCV_2014/papers/8693/86930772.pdf
   
3. 
   ^
   
   Bill Triggs et al. “Bundle adjustment – a modern synthesis”. In: International workshop on vision algorithms. Springer. 1999. pp. 298-372.
   
   https://lear.inrialpes.fr/pubs/2000/TMHF00/Triggs-va99.pdf
   
4. 
   ^
   
   Kirk MacTavish and Timothy D Barfoot. “At all Costs: A Comparison of Robust Cost Functions for Camera Correspondence Outliers”. In: 2015 12th Conference on Computer and Robot Vision. IEEE, 2015, pp 62-69.
   
   http://ncfrn.mcgill.ca/members/pubs/AtAllCosts_mactavish_crv15.pdf
   
5. The Levenberg-Marquardt method for nonlinear least squares curve-fitting problems.
   
   https://people.duke.edu/~hpgavin/ce281/lm.pdf