【SLAM】解决ORB_SLAM2运行时出现的"maybe forgot to call initializeOptimization()"警告

本文说明了如何解决ORB_SLAM2在ubuntu18.04运行时可能出现的”forgot to call initializeOptimization()”警告，包括代码修改和修复结果验证。

为了让博客的seo更加优化，遵循bing站长工具的建议，把每篇博客的description都适当加长一些。

问题说明

在Ubuntu 18.04上运行ORB_SLAM2的时候可能遇到如下g2o警告

1	virtual int g2o::SparseOptimizer::optimize(int, bool): 0 vertices to optimize, maybe forgot to call initializeOptimization()

在github仓库中找到了相关的issue：github.com/raulmur/ORB_SLAM2/issues/463；

根据github.com/raulmur/ORB_SLAM2/issues/211里面的描述，这个报错是g2o提出的，而且是一个harmless报错，也就是它虽然爆出来了但是没关系，也不会影响进程运行。

修复代码

虽然这问题不会影响运行吧，但对于有些许强迫症的我来说，更想知道怎么操作才能修复掉这个告警。根据issue 211里面的文字描述，找到了代码修改点：

Did you find where the problem is? I met the same problem in PoseOptimization because after n iterations probably all correspondences is bad. I just add the following check.
if((nInitialCorrespondences-nBad)<5) break;
It fixes my problem. Hope it helps.
I put this in the function Optimizer::PoseOptimization. Try to find the place where it performs 4 optimizations (for loop in there). At the end of each optimization, I add this check to make sure there are still enough vertices in the optimized graph.

修改点是ORB_SLAM2代码src/Optimizer.cc中的441行之后。在函数Optimizer::PoseOptimization的for(size_t it=0; it<4; it++)的末尾添加。

if(optimizer.edges().size()<10)
	break;
// 441行之后添加下面的代码
if((nInitialCorrespondences-nBad)<5)
	break;

具体的修改位置可以参考我fork的源码提交记录：https://github.com/musnows/ORB_SLAM2/commit/4840cdccdee0842896a637e57de47acecb7d75ac，截图如下：

新增代码后完整for循环代码如下

// src/Optimizer.cc
// 函数 Optimizer::PoseOptimization
int nBad=0;
for(size_t it=0; it<4; it++)
{

	vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
	optimizer.initializeOptimization(0);
	optimizer.optimize(its[it]);

	nBad=0;
	for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
	{
		g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

		const size_t idx = vnIndexEdgeMono[i];

		if(pFrame->mvbOutlier[idx])
		{
			e->computeError();
		}

		const float chi2 = e->chi2();

		if(chi2>chi2Mono[it])
		{                
			pFrame->mvbOutlier[idx]=true;
			e->setLevel(1);
			nBad++;
		}
		else
		{
			pFrame->mvbOutlier[idx]=false;
			e->setLevel(0);
		}

		if(it==2)
			e->setRobustKernel(0);
	}

	for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++)
	{
		g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];

		const size_t idx = vnIndexEdgeStereo[i];

		if(pFrame->mvbOutlier[idx])
		{
			e->computeError();
		}

		const float chi2 = e->chi2();

		if(chi2>chi2Stereo[it])
		{
			pFrame->mvbOutlier[idx]=true;
			e->setLevel(1);
			nBad++;
		}
		else
		{                
			e->setLevel(0);
			pFrame->mvbOutlier[idx]=false;
		}

		if(it==2)
			e->setRobustKernel(0);
	}

	if(optimizer.edges().size()<10)
		break;
	// 添加如下代码
	if((nInitialCorrespondences-nBad)<5)
		break;
}