文章目录
系列教程来自某学院,侵权删除。
学习完这一系列课程再去看VINS才能做到不吃力,不然直接撸网上的各种VINS解析完全云里雾里-_-!
上接第五讲1/2。
单目BA程序
上一讲讲到了H矩阵的构建,之后就要进行LM算法的初始化,ComputeLambdaInitLM()函数用于计算阻尼因子的初始值,在第三讲中说到过,阻尼因子的初始值选取方式为:
其中 τ \tau τ是一个系数,按工程经验取值,程序里这个值取了10^-5double tau = 1e-5;
,最后阻尼因子的计算为currentLambda_ = tau * maxDiagonal;
maxdiagonal是H矩阵对角线上最大的块。
初始化完成之后就进入LM迭代求解的部分,迭代次数由solve函数的入口参数确定,这是最后的运行结果截图,可以看到迭代了5次,
下面我们来看一次迭代中,LM算法是怎么实现的,方便阅读再把程序放上来一次:
while (!stop && (iter < iterations)) {
std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_ << std::endl;
bool oneStepSuccess = false;
int false_cnt = 0;
while (!oneStepSuccess) // 不断尝试 Lambda, 直到成功迭代一步
{
// 解Hx=b
SolveLinearSystem();
// 优化退出条件1: delta_x_ 很小则退出
if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {
stop = true;
break;
}
// 更新状态量
UpdateStates();
// 判断当前步是否可行以及 LM 的 lambda 怎么更新
oneStepSuccess = IsGoodStepInLM();
// 后续处理,
if (oneStepSuccess) {
// 在新线性化点 构建 hessian
MakeHessian