[从零写VIO|第三节]——基于优化的IMU与视觉信息融合(作业)

作业一

1. 准备工作

步骤：

1. 修改
   problem.cc文件的while循环下加入以下代码，用于存储迭代次数和lamda:

ofstream save_lamda;
save_lamda.open("../../data/Lamda.txt", ios::app);
save_lamda << iter << "\t" << currentLambda_ << endl;
save_lamda.close();

2. 编译并执行：

cd 指定文件夹下
mkdir build
cd build
cmake ..
make
./app/testCurveFitting

3. 实验结果：

2. 作业部分

2.1 绘制Lamda随迭代次数变化的曲线图

1. 编写绘图py文件

import matplotlib.pyplot as plt
filename = 'Lamda.txt'
X,Y = [],[]

for line in open(filename, 'r'):
	value = [float(s) for s in line.split()]
	X.append(value[0])
	Y.append(value[1])
	
plt.plot(X,Y)
plt.title('Lamda trends')
plt.xlabel('迭代次数')
plt.ylabel('LAMDA')

plt.show()

2. 曲线展示

2.2 修改曲线

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

1. 分析
   残差：
   【注】上标表示测量值
   对$a,b,c$依次求偏导得到的雅可比矩阵为：
   
2. 修改代码

residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2) - y_; // 计算残差
jaco_abc << x_ * x_ , x_  , 1; // 新雅可比矩阵
...
double y = a*x*x + b*x + c + n; // 观测

【注】由于原始设置的噪声过大，不好收敛；因此设置观测次数N=1500。

3. 实验结果

2.3 尝试阻尼因子更新策略

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

实验结果：

作业二

1. 公式推导$f_{15}$

2. $g_{12}$的推导，与$f_{15}$的推导极其 类似：

作业三

公式证明：

参考资料
理解IMU以及积分
LM算法