Optimizer类

ORB_SLAM2优化类一共有五个优化
这里先说一下利用g2o的BundleAdjustment优化

1. 初始化优化器（稀疏，线性，LM算法）
2. 设置关键帧位姿为顶点
3. 设置地图点位置为顶点
4. 对于每一个地图点，将它与能观测到该点的关键帧之间的重投影误差设置为边
5. 设置完成，进行优化
6. 更新优化后的关键帧位姿和地图点位置

一、GlobalBundleAdjustemnt（全局优化）：对所有的地图点和关键帧进行优化
全局BA优化在本程序中有两个地方使用：

• 单目初始化：CreateInitialMapMonocular函数
• 闭环优化：RunGlobalBundleAdjustment函数
  

二、LocalBundleAdjustment（局部优化）：对某一关键帧的局部地图点集合和局部关键帧进行优化

局部优化在LocalMapping线程处理完队列中最后一个关键帧时使用

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

Pos3是新加入的关键帧，其初始估计位姿已经得到。此时，Pos2是和Pos3相连的关键帧，X2是Pos3看到的三维点，X1是Pos2看到的三维点，这些都属于局部信息，共同参与Bundle Adjustment。同时，Pos1也可以看到X1，但它和Pos3没有直接的联系，属于Pos3关联的局部信息，参与Bundle Adjustment，但取值保持不变。Pos0和X0不参与Bundle Adjustment。
因此，参与优化的是下图中红色椭圆圈出的部分，其中红色代表取值会被优化，灰色代表取值保持不变。(u,v)是X在Pos下的二维投影点，即X在Pos下的测量（measurement）。优化的目标是让投影误差最小。

三、PoseOptimization（位姿图优化）：地图点固定，只对当前帧的位姿进行优化
用于LocalTracking中运动模型跟踪，参考帧跟踪，地图跟踪TrackLocalMap，重定位

1. 初始化优化器
2. 设置输入的关键帧位姿为顶点
3. 设置关键帧对应的地图点与该关键帧的重投影误差（地图点固定，是一元边）为边
4. 进行4次优化，每次十次迭代，剔除外点和误差过大的边
5. 更新优化后的关键帧位姿

四、OptimizeEssentialGraph（优化本质图）：所有的关键帧都是顶点，但边只是进行了一部分边的优化
用于闭环检测Sim3调整后优化

1. 初始化优化器
2. 将所有关键帧的位姿设置为顶点
3. 某一帧，因为有闭环而新产生了一些跟他相连的关键帧，在该帧与这些关键帧之间建立边
4. 某一帧，找出他的父关键帧，建立二者之间的边
5. 某一帧，如果他有与他匹配的闭环帧，在二者之间建立边
6. 某一帧，找出与它连接权重大于一定值的关键帧，建立该帧与这些帧之间的边
7. 进行优化
8. 根据优化结果更新各帧位姿，矫正地图点的位置

五、OptimizeSim3（优化Sim3）：当前关键帧与闭环关键帧之间进行Sim3优化
在用RANSAC求解过Sim3，以及通过Sim3匹配更多的地图点后，对当前关键帧，闭环关键帧，以及匹配的地图点进行优化，获得更准确的Sim3位姿，再去下一步的闭环调整

1. 初始化优化器
2. 将两帧之间的sim3设置为顶点
3. 将两针之间匹配上的地图点分部在各自相机坐标系下的位置设置为顶点
4. 分别设置两帧对应的地图点的重投影误差为边
5. 进行优化
6. 剔除重投影误差过大的边
7. 再进行一次优化，剔除外点
8. 更新优化后的sim3

Pos6和Pos2为一个可能的闭环。通过之间的投影误差来优化