[ORBSLAM2_12]之Optimizer

图优化，是把优化问题用图（Graph）描述：一个图由若干个顶点（Vertex）,以及连接着这些顶点的边（Edge）组成。顶点表示优化变量，边表示误差项。

1. 下图是14讲给的示例：对下图简单的描述：

1）三角形表示相机位姿节点，即帧（Frame、KeyFrame）的相机位姿

2）圆形表示路标点：p1和p2，即地图点，3D点，构成了图优化的顶点

3）实线表示相机的运动模型：相机的运动轨迹

4）虚线表示观测模型：p1的观察帧是x1、x2、x3，虚线构成了图优化的边

从上述的简介可以看出：构成图优化需要边和顶点，顶点也是边的构成部分。下图是优化p1和p2等3D地图点。

Optimizer类中的5种优化模型都是基于下图演变而来。

1.帧的相机位姿优化函数PoseOptimization

PoseOptimization优化的是单帧图像的位姿。相机在运动过程中，每帧图像都含2D特征点和与其对的3D地图点，通过3D-2D的投影关系估计出相机的位姿。

PoseOptimization是对某帧图像中的多个地图点建立多个一元连接边，构成图进行优化，优化某帧图像对应的相机的位姿。

2. 通过两帧之间匹配的MapPoints优化两帧间Sim3：OptimizeSim3

OptimizeSim3优化两帧的Sim3

3.局部BA优化LocalBundleAdjustment

4.本质图优化OptimizeEssentialGraph

5.全局BA优化GlobalBundleAdjustment

PoseOptimization是针对某一帧图像的相机位姿的。该帧已知的条件有3D地图点坐标，2D特征点坐标和相机内参等，构建的图优化模型：相机位姿是待优化量作为图优化的顶点，3D地图点、相机内参、相机位姿（待优化）构成图优化的边。

模型很像14讲的“6.4.3 使用g2o拟合曲线”的示例中的图优化模型。

PoseOptimization函数的步骤：

1. 初始化优化器
2. 设置输入的关键帧的相机位姿为顶点，该顶点是待优化量
3. 设置该帧的KeyPoint对应的地图点与地图点的重投影误差项为边，地图点固定，是一元边

4. 进行4次优化，每次迭代10次，剔除外点和误差过大的边

//初始值
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
optimizer.initializeOptimization(0);//对level为0的边进行优化
optimizer.optimize(its[it]);

其实在optimizer.optimize(its[it]);中调用了函数：

//来自types_six_dof_expmap.h的计算误差函数
void computeError(){
	const VertexSE3Expmap* v1=static_cast<const VertexSE3Expmap*>(_vertices[0]);
	Vector2d obs(_measurement);
	_error=obs - cam_project(v1->estimate().map(Xw));
}

计算卡方：在base_edge.h中，使用卡方校验剔除外点

    virtual double chi2() const 
{
        return _error.dot(information()*_error);
      }

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);// 设置为outlier
		nBad++;
	}
	else
	{
		pFrame->mvbOutlier[idx]=false;//原来是外点 优化过后 误差变小 变成内点了
		e->setLevel(0);// 设置为inlier
	}
	if (it==2)
		e->setRobustKernel(0);// 除了前两次优化需要RobustKernel以外, 其余的优化都不需要
}

5. 更新优化后的Frame的相机位姿

ORB-SLAM当中在LoopClosing::ComputetSim3()中调用了OptimizeSim3函数。

单目相机具有尺度不确定性，相机运动一段时间后可能会产生尺度漂移和误差，通过计算两帧图像的Sim3变换，获取尺度变换。并通过Sim3匹配两帧图像，获取更多匹配的MapPoints，因为产生了新的匹配的MapPoints，在使用这些MapPoints对Sim3进行优化，获取精度更高的Sim3。

Sim3Solver类的讲解见下面的链接：

边主要由3D点（已知）、2D点（已知）、待求值g2oS12组成。

1）构造并初始化优化器：g2o::SparseOptimizer optimizer

2）将两帧之间的Sim3（待求量）设置为顶点：optimizer.addVertex(vSim3);

3）将两针之间匹配上的地图点（世界坐标系）分别转为在各自相机坐标系下的坐标（3D）设置为顶点（已知）：optimizer.addVertex(vPoint1);和optimizer.addVertex(vPoint2);

4）分别设置两帧对应的地图点（相机坐标系下的坐标）vPoint1和vPoint2的重投影误差（封装在SparseOptimizer::computeActiveErrors()）为边

5）进行优化：optimizer.optimize(5);

6）剔除重投影误差过大的边：卡方校验if (e12->chi2() > th2 || e21->chi2() > th2)

7）再进行一次优化，剔除外点

8）更新优化后的sim3：g2oS12

局部BA优化，优化的是局部关键帧的位姿（待优化）和这些局部关键帧可以观测到的地图点的3D坐标（待优化）。局部关键帧是当前关键帧的共视帧集合，局部地图点是局部关键帧的所有地图点集合。将局部地图点与可以观测到他们的关键帧放在一起进行优化，将可以观察到局部地图点的关键帧的SE3位姿和局部地图点的3D坐标（待优化量）添加到顶点中，并作为边的顶点进行优化。

1. 初始化优化器
2. 将局部关键帧（参数关键帧及其共视关键帧）位姿（待优化）设置为顶点
3. 将局部关键帧的地图点作为局部地图点（待优化），并将他们设置为顶点
4. 将能观测到局部地图点的关键帧（不包含步骤2中的关键帧）作为补充关键帧，将他们位姿（已知）也设置为顶点
5. 对于每一个局部地图点，将它与能观测到该点的关键帧之间的重投影误差设置为边
6. 第一次优化，去除优化后误差较大的顶点，不再优化它们
7. 第二次优化，去除优化后误差较大的顶点
8. 更新优化后的局部关键帧位姿和局部地图点3D坐标

void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, const LoopClosing::KeyFrameAndPose& NonCorrectedSim3, const LoopClosing::KeyFrameAndPose& CorrectedSim3, const map<KeyFrame*, set<KeyFrame*> >& LoopConnections, const bool& bFixScale)

优化pMap中的每一个关键帧pKF的位姿和每一个地图点的坐标。

本质图EssentialGraph包含了四部分：LoopConnections、父关键帧（Spanning tree edge）、pKF->GetLoopEdges()、pKF->GetCovisiblesByWeight(minFeat)放在一起进行优化，优化的目标是关键帧pKF的sim3位姿和地图点的坐标。 将优化后的sim3位姿转换为SE3，就得到了相机的位姿，并对地图点的坐标进行投影得到更新后的坐标。

Sim3Solver类的讲解见下面的链接：

1. 构造并初始化优化器：求解器类型 g2o::BlockSolver_7_3：帧Sim3位姿 spose 维度为7[sR t]，地图点landmark 维度为3
2. 将地图中所有关键帧的位姿（待优化）设置为顶点
3. 某一帧，因为有闭环而新产生了跟他相连的关键帧LoopConnections，该帧与LoopConnections的关键帧之间建立边
4. 某一帧，若存在父关键帧，建立二者之间的边
5. 某一帧，如果有与他匹配的闭环帧pKF->GetLoopEdges()，在二者之间建立边
6. 某一帧，若存在与它连接权重大于一定值的关键帧pKF->GetCovisiblesByWeight(minFeat)，建立该帧与这些帧之间的边
7. 进行优化
8. 根据优化结果更新各帧位姿，矫正地图点的位置

整个地图中所有关键帧和所有地图点都优化，将全局地图当中所有的关键帧和地图点都放进来一起进行优化。对关键帧的位姿和地图点3D坐标进行优化。