ORB_SLAM2 Optimizer

ORB_SLAM2使用g2o库进行优化。
优化分为四种：
（1）全局BA优化（在单目初始化之后、回环检测之后进行全局BA优化。对所有关键帧的位姿、路标点进行优化）
（2）局部BA优化（在局部建图之后进行，对当前帧及其共视关键帧的位姿、路标点进行优化）
（3）位姿图优化（仅优化位姿，在恒速运动模型、参考关键字模型、重定位、局部地图跟踪的时候执行）
（4）本质图优化（在回环校正过程中进行优化，将闭环误差分配到本质图中）
（5）Sim3优化（检测到回环之后，会进行闭环帧和当前帧间的Sim3估计，随后进行Sim3优化）
需要注意的是，优化完路标点之后，要记得更改路标点的属性（如观测角度，平均深度等）。

（1）全局BA优化（GlobalBundleAdjustment）与局部BA优化（LocalBundleAdjustment）

（2）位姿优化（只优化相机位姿，不优化路标点坐标）

（3）优化本质图
优化本质图中的关键帧的位姿以及路标点坐标。

（4）筛选出的闭环帧的位姿优化

全局BA优化

优化所有关键帧的位姿和路标点。（单目初始化、闭环优化后）

void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)

加入位姿顶点

    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        // 去除无效的
        if(pKF->isBad())
            continue;
        
        // 对于每一个能用的关键帧构造SE3顶点,其实就是当前关键帧的位姿
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
        // 顶点的id就是关键帧在所有关键帧中的id
        vSE3->setId(pKF->mnId); 
        // 只有第0帧关键帧不优化（参考基准）
        vSE3->setFixed(pKF->mnId==0);

        // 向优化器中添加顶点，并且更新maxKFid
        optimizer.addVertex(vSE3);
        if(pKF->mnId>maxKFid)
            maxKFid=pKF->mnId;
    }

加入路标点顶点

        MapPoint* pMP = vpMP[i];
        // 跳过无效地图点
        if(pMP->isBad())
            continue;

        // 创建顶点
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        // 注意由于地图点的位置是使用cv::Mat数据类型表示的,这里需要转换成为Eigen::Vector3d类型
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        // 前面记录maxKFid 是在这里使用的
        const int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        // 注意g2o在做BA的优化时必须将其所有地图点全部schur掉，否则会出错。
        // 原因是使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>这个类型来指定linearsolver,
        // 其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。
        // Ceres库则没有这样的限制
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

加入边（二元边，因为优化的为位姿、路标点个两种顶点）

边中只加入了路标点与关键帧间的连接边。
在边中会设置信息矩阵。信息矩阵的作用是确定在优化过程中，每条边分担的误差有多少。

            KeyFrame* pKF = mit->first;
            // 跳过不合法的关键帧
            if(pKF->isBad() || pKF->mnId>maxKFid)
                continue;

            nEdges++;
            // 取出该地图点对应该关键帧的2D特征点
            const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];

            if(pKF->mvuRight[mit->second]<0)
            {
                // 如果是单目相机按照下面操作
                // 构造观测
                Eigen::Matrix<double,2,1> obs;
                obs << kpUn.pt.x, kpUn.pt.y;

                // 创建边
                g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
                // 填充数据，构造约束关系
                // 第0个顶点对应的id 是地图点的id
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                // 第1个顶点对应的id是 关键帧的id
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                // 信息矩阵，也是协方差，表明了这个约束的观测在各个维度（x,y）上的可信程度，在我们这里对于具体的一个点，两个坐标的可信程度都是相同的，
                // 其可信程度受到特征点在图像金字塔中的图层有关，图层越高，可信度越差
                // 为了避免出现信息矩阵中元素为负数的情况，这里使用的是sigma^(-2)
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

                // 如果要使用鲁棒核函数
                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    // 这里的重投影误差，自由度为2，所以这里设置为卡方分布中自由度为2的阈值，如果重投影的误差大约大于1个像素的时候，就认为不太靠谱的点了，
                    // 核函数是为了避免其误差的平方项出现数值上过大的增长
                    rk->setDelta(thHuber2D);
                }

                // 设置相机内参
                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;
                // 添加边
                optimizer.addEdge(e);

取出优化后的帧的位姿与路标点坐标

 
 

位姿优化

int Optimizer::PoseOptimization(Frame *pFrame)

在恒速运动模型、参考关键帧模型、重定位、局部地图跟踪（和局部地图优化不一样，局部地图追踪是在Track线程中，而局部地图优化是在LocalMapping线程中）的时候，都会用到位姿优化。（个人感觉与位姿图优化不一样。位姿图优化的话，边连接的是两个顶点，BaseBinaryEdge。而这里的边只连接一个顶点，BaseUnaryEdge。）

边为一元边，因为边只连接位姿。
 
 

局部BA优化

在局部建图线程处理完最后一个关键帧之后，调用局部BA优化。
局部地图中包括当前帧及其共视关键帧（权值大于一定的比例）。对于那些权值小于阈值的关键帧，在BA优化过程中，只提供约束，不进行优化。

void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)

过程与全局BA优化相似，设定位姿顶点、路标顶点，建立两个顶点间的边。进行优化（一开始使用鲁棒核函数进行优化，优化之后去掉外点（边的代价过大），然后对剩下的点不使用鲁棒核函数进一步优化）。
 
 

本质图优化

void Optimizer::OptimizeEssentialGraph

本质图与共视图相似，但更加严格。其主要作用是在回环检测之后，将闭环误差均摊到本质图中。
在本质图优化中，只优化关键帧的位姿。
是一个位姿图优化，边连接两个位姿顶点。
 
 

Sim3优化

nt Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)

检测到回环，计算出当前帧和回环帧间的Sim3变换后，进行优化。
在第一次优化过程中，那些误差很大的边，直接剔除掉，然后对剩下的边进一步进行优化。