# Optimizer.cpp

### Optimizer::LocalBundleAdjustment()
用於LocalMapping線程的局部BA優化
在[LocalMapping::Run()](LocalMapping::Run()#)中引用

#### 定義

void Optimizer::LocalBundleAdjustment(KeyFrame *pKF,     // 關鍵幀
                                      bool* pbStopFlag,  // 是否停止優化
                                      Map* pMap)         // 優化後，更新狀態時需要用到Map的互斥量mMutexMapUpdate 

#### 函式流程
1. 將當前關鍵幀及其共視關鍵幀加入局部關鍵幀
2. 遍歷局部關鍵幀中(一級相連)關鍵幀，將它們觀測的地圖點加入到局部地圖點
3. 得到能被局部地圖點觀測到，但不屬於局部關鍵幀的關鍵幀(二級相連)，這些二級相連關鍵幀在局部BA優化時不優化
4. 構造g2o優化器
5. 添加待優化的姿態頂點：局部關鍵幀的姿態
6. 添加不優化的位姿頂點：固定關鍵幀的位姿
7. 添加待優化的局部地圖點頂點
8. 在添加完了一個地圖點之後, 對每一對關聯的地圖點和關鍵幀構建邊
9. 第一階段優化
10. 檢測outlier，並設置下次不優化
11. 排除誤差較大的outlier後再次優化 -- 第二階段優化
12. 在優化後重新計算誤差，剔除連接誤差比較大的關鍵幀和地圖點
13. 優化後更新關鍵幀位姿以及地圖點的位置、平均觀測方向等屬性

#### 函式詳解
1. 將當前關鍵幀及其共視關鍵幀加入局部關鍵幀

// 局部關鍵幀
list<KeyFrame*> lLocalKeyFrames;

lLocalKeyFrames.push_back(pKF);
pKF->mnBALocalForKF = pKF->mnId;

// 找到關鍵幀連接的共視關鍵幀（一級相連），加入局部關鍵幀中
const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
{
    KeyFrame* pKFi = vNeighKFs[i];

    // 把參與局部BA的每一個關鍵幀的 mnBALocalForKF設置為當前關鍵幀的mnId，防止重複添加
    pKFi->mnBALocalForKF = pKF->mnId;

    // 保證該關鍵幀有效才能加入
    if(!pKFi->isBad())      
        lLocalKeyFrames.push_back(pKFi);
}

2. 遍歷局部關鍵幀中(一級相連)關鍵幀，將它們觀測的地圖點加入到局部地圖點

list<MapPoint*> lLocalMapPoints;
// 遍歷局部關鍵幀中的每一個關鍵幀
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
    // 取出該關鍵幀對應的地圖點
    vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
    // 遍歷這個關鍵幀觀測到的每一個地圖點，加入到局部地圖點
    for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;
        if(pMP)
        {
            if(!pMP->isBad())   //保證地圖點有效
                // 把參與局部BA的每一個地圖點的 mnBALocalForKF設置為當前關鍵幀的mnId
                // mnBALocalForKF 是為了防止重複添加
                if(pMP->mnBALocalForKF!=pKF->mnId)
                {
                    lLocalMapPoints.push_back(pMP);
                    pMP->mnBALocalForKF=pKF->mnId;
                }
        }   // 判斷這個地圖點是否靠譜
    } // 遍歷這個關鍵幀觀測到的每一個地圖點
} // 遍歷 lLocalKeyFrames 中的每一個關鍵幀

3. 得到能被局部地圖點觀測到，但不屬於局部關鍵幀的關鍵幀(二級相連)，這些二級相連關鍵幀在局部BA優化時不優化

list<KeyFrame*> lFixedCameras;
// 遍歷局部地圖中的每個地圖點
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
    // 觀測到該地圖點的KF和該地圖點在KF中的索引
    map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
    // 遍歷所有觀測到該地圖點的關鍵幀
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKFi = mit->first;

        // pKFi->mnBALocalForKF!=pKF->mnId 表示不屬於局部關鍵幀，
        // pKFi->mnBAFixedForKF!=pKF->mnId 表示還未標記為fixed（固定的）關鍵幀
        if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
        {                
            // 將局部地圖點能觀測到的、但是不屬於局部BA範圍的關鍵幀的mnBAFixedForKF標記為pKF（觸發局部BA的當前關鍵幀）的mnId
            pKFi->mnBAFixedForKF=pKF->mnId;
            if(!pKFi->isBad())
                lFixedCameras.push_back(pKFi);
        }
    }
}

4. 構造g2o優化器

// 圖優化模型
g2o::SparseOptimizer optimizer;

// 求H*Δx=−b的線性求解器
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();

// 矩陣塊求解器 求解雅克比矩陣和海塞矩陣
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

// LM優化器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

// 外界設置的停止優化標誌
// 可能在 Tracking::NeedNewKeyFrame() 裡置位
if(pbStopFlag)
    optimizer.setForceStopFlag(pbStopFlag);

// 記錄參與局部BA的最大關鍵幀mnId
unsigned long maxKFid = 0;

5. 添加待優化的姿態頂點：局部關鍵幀的姿態

for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
    KeyFrame* pKFi = *lit;
    //6自由度的頂點，可以用來當作姿態
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    // 設置初始優化位姿
    vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
    vSE3->setId(pKFi->mnId);
    // 如果是初始關鍵幀，要鎖住位姿不優化
    vSE3->setFixed(pKFi->mnId==0);
    optimizer.addVertex(vSE3);
    if(pKFi->mnId>maxKFid)
        maxKFid=pKFi->mnId;
}

6. 添加不優化的位姿頂點：固定關鍵幀的位姿

// 注意這裡調用了vSE3->setFixed(true)
// 不優化為啥也要添加？回答：為了增加約束信息
for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
{
    KeyFrame* pKFi = *lit;
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
    vSE3->setId(pKFi->mnId);
     // 所有的這些頂點的位姿都不優化，只是為了增加約束項
    vSE3->setFixed(true);  
    optimizer.addVertex(vSE3);
    if(pKFi->mnId>maxKFid)
        maxKFid=pKFi->mnId;
}

7. 添加待優化的局部地圖點頂點

// 邊的最大數目 = 位姿數目 * 地圖點數目
const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();

//3維點
vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);

vector<KeyFrame*> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);

vector<MapPoint*> vpMapPointEdgeMono;
vpMapPointEdgeMono.reserve(nExpectedSize);

vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
vpEdgesStereo.reserve(nExpectedSize);

vector<KeyFrame*> vpEdgeKFStereo;
vpEdgeKFStereo.reserve(nExpectedSize);

vector<MapPoint*> vpMapPointEdgeStereo;
vpMapPointEdgeStereo.reserve(nExpectedSize);

// 自由度為2的卡方分佈，顯著性水平為0.05，對應的臨界閾值5.991
const float thHuberMono = sqrt(5.991);

// 自由度為3的卡方分佈，顯著性水平為0.05，對應的臨界閾值7.815
const float thHuberStereo = sqrt(7.815);

// 遍歷所有的局部地圖點
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
    // 添加頂點：MapPoint
    MapPoint* pMP = *lit;
    g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
    vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
    // 前面記錄maxKFid的作用在這裡體現
    int id = pMP->mnId+maxKFid+1;
    vPoint->setId(id);
    // 因為使用了LinearSolverType，所以需要將所有的三維點邊緣化掉
    vPoint->setMarginalized(true);  
    optimizer.addVertex(vPoint);

    // 觀測到該地圖點的KF和該地圖點在KF中的索引
    const map<KeyFrame*,size_t> observations = pMP->GetObservations();

8. 在添加完了一個地圖點之後, 對每一對關聯的地圖點和關鍵幀構建邊

    // 遍歷所有觀測到當前地圖點的關鍵幀
    for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKFi = mit->first;

        if(!pKFi->isBad())
        {                
            //當前三維點對應的特徵點
            const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
            
            // 根據單目/雙目兩種不同的輸入構造不同的誤差邊
            // 單目模式下
            if(pKFi->mvuRight[mit->second]<0)
            {
                
                Eigen::Matrix<double,2,1> obs;
                
                //三圍點的觀測，也就是二維特徵點
                obs << kpUn.pt.x, kpUn.pt.y;
                
                //定義邊
                g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
                
                // 為邊添加關聯的頂點
                // 邊的第一個頂點是地圖點
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                // 邊的第一個頂點是觀測到該地圖點的關鍵幀
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
                //添加測量值
                e->setMeasurement(obs);
                // 權重為特徵點所在圖像金字塔的層數的倒數
                const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                
                //信息矩陣
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

                // 使用魯棒核函數抑制外點
                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuberMono);

                e->fx = pKFi->fx;
                e->fy = pKFi->fy;
                e->cx = pKFi->cx;
                e->cy = pKFi->cy;
                // 將邊添加到優化器，記錄邊、邊連接的關鍵幀、邊連接的地圖點信息
                optimizer.addEdge(e);
                vpEdgesMono.push_back(e);
                vpEdgeKFMono.push_back(pKFi);
                vpMapPointEdgeMono.push_back(pMP);
            }
            else // Stereo observation
            {
                // 雙目或RGB-D模式和單目模式類似
                Eigen::Matrix<double,3,1> obs;
                const float kp_ur = pKFi->mvuRight[mit->second];
                obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
                e->setMeasurement(obs);
                const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                e->setInformation(Info);

                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuberStereo);

                e->fx = pKFi->fx;
                e->fy = pKFi->fy;
                e->cx = pKFi->cx;
                e->cy = pKFi->cy;
                e->bf = pKFi->mbf;

                optimizer.addEdge(e);
                vpEdgesStereo.push_back(e);
                vpEdgeKFStereo.push_back(pKFi);
                vpMapPointEdgeStereo.push_back(pMP);
            } 
        } 
    } // 遍歷所有觀測到當前地圖點的關鍵幀
} // 遍歷所有的局部地圖中的地圖點

// 開始BA前再次確認是否有外部請求停止優化，因為這個變量是引用傳遞，會隨外部變化
// 可能在 Tracking::NeedNewKeyFrame(), mpLocalMapper->InsertKeyFrame 裡置位
if(pbStopFlag)
    if(*pbStopFlag)
        return;

9. 第一階段優化

optimizer.initializeOptimization();
// 迭代5次
optimizer.optimize(5);  

bool bDoMore= true;

// 檢查是否外部請求停止
if(pbStopFlag)
    if(*pbStopFlag)
        bDoMore = false;

// 如果有外部請求停止,那麼就不在進行第二階段的優化
if(bDoMore)
{

10. 檢測outlier，並設置下次不優化

    // 遍歷所有的單目誤差邊
    for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
    {
        g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
        MapPoint* pMP = vpMapPointEdgeMono[i];
 
        if(pMP->isBad())
            continue;

        // 基於卡方檢驗計算出的閾值（假設測量有一個像素的偏差）
        // 自由度為2的卡方分佈，顯著性水平為0.05，對應的臨界閾值5.991
        // 如果 當前邊誤差超出閾值，或者邊鏈接的地圖點深度值為負，說明這個邊有問題，不優化了。
        if(e->chi2()>5.991 || !e->isDepthPositive())
        {
            // 不優化
            e->setLevel(1);
        }
        // 第二階段優化的時候就屬於精求解了,所以就不使用核函數
        e->setRobustKernel(0);
    }

    // 對於所有的雙目的誤差邊也都進行類似的操作
    for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
    {
        g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
        MapPoint* pMP = vpMapPointEdgeStereo[i];

        if(pMP->isBad())
            continue;
        // 自由度為3的卡方分佈，顯著性水平為0.05，對應的臨界閾值7.815
        if(e->chi2()>7.815 || !e->isDepthPositive())
        {
            e->setLevel(1);
        }

        e->setRobustKernel(0);
    }

11. 排除誤差較大的outlier後再次優化 -- 第二階段優化

    optimizer.initializeOptimization(0);
    optimizer.optimize(10);

}

vector<pair<KeyFrame*,MapPoint*> > vToErase;
vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());

12. 在優化後重新計算誤差，剔除連接誤差比較大的關鍵幀和地圖點

// 對於單目誤差邊
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
    g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
    MapPoint* pMP = vpMapPointEdgeMono[i];

    if(pMP->isBad())
        continue;

    // 基於卡方檢驗計算出的閾值（假設測量有一個像素的偏差）
    // 自由度為2的卡方分佈，顯著性水平為0.05，對應的臨界閾值5.991
    // 如果 當前邊誤差超出閾值，或者邊鏈接的地圖點深度值為負，說明這個邊有問題，要刪掉了
    if(e->chi2()>5.991 || !e->isDepthPositive())
    {
        // outlier
        KeyFrame* pKFi = vpEdgeKFMono[i];
        vToErase.push_back(make_pair(pKFi,pMP));
    }
}

// 雙目誤差邊
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
{
    g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
    MapPoint* pMP = vpMapPointEdgeStereo[i];

    if(pMP->isBad())
        continue;

    if(e->chi2()>7.815 || !e->isDepthPositive())
    {
        KeyFrame* pKFi = vpEdgeKFStereo[i];
        vToErase.push_back(make_pair(pKFi,pMP));
    }
}

// Get Map Mutex
unique_lock<mutex> lock(pMap->mMutexMapUpdate);

// 刪除點
// 連接偏差比較大，在關鍵幀中剔除對該地圖點的觀測
// 連接偏差比較大，在地圖點中剔除對該關鍵幀的觀測
if(!vToErase.empty())
{
    for(size_t i=0;i<vToErase.size();i++)
    {
        KeyFrame* pKFi = vToErase[i].first;
        MapPoint* pMPi = vToErase[i].second;
        pKFi->EraseMapPointMatch(pMPi);
        pMPi->EraseObservation(pKFi);
    }
}

13. 優化後更新關鍵幀位姿以及地圖點的位置、平均觀測方向等屬性

//Keyframes
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
    KeyFrame* pKF = *lit;
    g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
    g2o::SE3Quat SE3quat = vSE3->estimate();
    pKF->SetPose(Converter::toCvMat(SE3quat));
}

//Points
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
    MapPoint* pMP = *lit;
    g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
    pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
    pMP->UpdateNormalAndDepth();
}

### Optimizer::OptimizeSim3()
形成閉環時固定(不優化) 地圖點進行Sim3位姿優化
在[LoopClosing::ComputeSim3()](#LoopClosing::ComputeSim3())中引用

#### 定義

int Optimizer::OptimizeSim3(KeyFrame *pKF1,                 // 當前幀
                            KeyFrame *pKF2,                 // 閉環候選幀
                            vector<MapPoint *> &vpMatches1, // 兩個關鍵幀之間的匹配關係
                            g2o::Sim3 &g2oS12,              // 兩個關鍵幀間的Sim3變換，方向是從2到1 
                            const float th2,                // 卡方檢驗是否為誤差邊用到的閾值
                            const bool bFixScale)           // 是否優化尺度，單目進行尺度優化，雙目/RGB-D不進行尺度優化    

#### 函式流程
1. 初始化g2o優化器
2. 設置待優化的Sim3位姿作為頂點(用來優化兩幀之間的變換)
3. 設置匹配的地圖點作為頂點
4. 添加邊（地圖點投影到特徵點）
5. g2o開始優化，先迭代5次
6. 用卡方檢驗剔除誤差大的邊
7. 再次g2o優化剔除後剩下的邊
8. 用優化後的結果來更新Sim3位姿

#### 函式詳解
1. 初始化g2o優化器

// 先構造求解器
g2o::SparseOptimizer optimizer;
// 構造線性方程求解器，Hx = -b的求解器
g2o::BlockSolverX::LinearSolverType * linearSolver;
// 使用dense的求解器，（常見非dense求解器有cholmod線性求解器和shur補線性求解器）
linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
// 使用L-M迭代
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

// Calibration
// 內參矩陣
const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;

// Camera poses
const cv::Mat R1w = pKF1->GetRotation();
const cv::Mat t1w = pKF1->GetTranslation();
const cv::Mat R2w = pKF2->GetRotation();
const cv::Mat t2w = pKF2->GetTranslation();

2. 設置待優化的Sim3位姿作為頂點(用來優化兩幀之間的變換)

g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();    
// 根據傳感器類型決定是否固定尺度
vSim3->_fix_scale=bFixScale;
vSim3->setEstimate(g2oS12);
vSim3->setId(0);
// Sim3 需要優化
vSim3->setFixed(false);                             // 因為要優化Sim3頂點，所以設置為false
vSim3->_principle_point1[0] = K1.at<float>(0,2);    // 光心橫坐標cx
vSim3->_principle_point1[1] = K1.at<float>(1,2);    // 光心縱坐標cy
vSim3->_focal_length1[0] = K1.at<float>(0,0);       // 焦距 fx
vSim3->_focal_length1[1] = K1.at<float>(1,1);       // 焦距 fy
vSim3->_principle_point2[0] = K2.at<float>(0,2);
vSim3->_principle_point2[1] = K2.at<float>(1,2);
vSim3->_focal_length2[0] = K2.at<float>(0,0);
vSim3->_focal_length2[1] = K2.at<float>(1,1);
optimizer.addVertex(vSim3);

3. 設置匹配的地圖點作為頂點

const int N = vpMatches1.size();
// 獲取pKF1的地圖點
const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();

vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;         //pKF2對應的地圖點到pKF1的投影邊
vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;  //pKF1對應的地圖點到pKF2的投影邊
vector<size_t> vnIndexEdge;                         //邊的索引

vnIndexEdge.reserve(2*N);
vpEdges12.reserve(2*N);
vpEdges21.reserve(2*N);

// 核函數的閾值
const float deltaHuber = sqrt(th2);

int nCorrespondences = 0;

// 遍歷每對匹配點
for(int i=0; i<N; i++)
{
    if(!vpMatches1[i])
        continue;

    // pMP1和pMP2是匹配的地圖點
    MapPoint* pMP1 = vpMapPoints1[i];
    MapPoint* pMP2 = vpMatches1[i];

    // 保證頂點的id能夠錯開
    const int id1 = 2*i+1;
    const int id2 = 2*(i+1);

    // i2 是 pMP2 在pKF2中對應的索引
    const int i2 = pMP2->GetIndexInKeyFrame(pKF2);

    if(pMP1 && pMP2)
    {
        if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)
        {
            // 如果這對匹配點都靠譜，並且對應的2D特徵點也都存在的話，添加PointXYZ頂點
            g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();
            // 地圖點轉換到各自相機坐標系下的三維點
            cv::Mat P3D1w = pMP1->GetWorldPos();
            cv::Mat P3D1c = R1w*P3D1w + t1w;
            vPoint1->setEstimate(Converter::toVector3d(P3D1c));
            vPoint1->setId(id1);
            // 地圖點不優化
            vPoint1->setFixed(true);
            optimizer.addVertex(vPoint1);

            g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();
            cv::Mat P3D2w = pMP2->GetWorldPos();
            cv::Mat P3D2c = R2w*P3D2w + t2w;
            vPoint2->setEstimate(Converter::toVector3d(P3D2c));
            vPoint2->setId(id2);
            vPoint2->setFixed(true);
            optimizer.addVertex(vPoint2);
        }
        else
            continue;
    }
    else
        continue;
        
    // 對匹配關係進行計數
    nCorrespondences++;

4. 添加邊（地圖點投影到特徵點）

    // Set edge x1 = S12*X2

    // 地圖點pMP1對應的觀測特徵點
    Eigen::Matrix<double,2,1> obs1;
    const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
    obs1 << kpUn1.pt.x, kpUn1.pt.y;

// 4.1. 閉環候選幀地圖點投影到當前關鍵幀的邊 -- 正向投影
    g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();
    // vertex(id2)對應的是pKF2 VertexSBAPointXYZ 類型的三維點
    e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
    // ? 為什麼這裡添加的節點的id為0？
    // 回答：因為vertex(0)對應的是 VertexSim3Expmap 類型的待優化Sim3，其id 為 0
    e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
    e12->setMeasurement(obs1);
    // 信息矩陣和這個特徵點的可靠程度（在圖像金字塔中的圖層）有關
    const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
    e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);

    // 使用魯棒核函數
    g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;
    e12->setRobustKernel(rk1);
    rk1->setDelta(deltaHuber);
    optimizer.addEdge(e12);

    // Set edge x2 = S21*X1
// 4.2. 當前關鍵幀地圖點投影到閉環候選幀的邊 -- 反向投影

    // 地圖點pMP2對應的觀測特徵點
    Eigen::Matrix<double,2,1> obs2;
    const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
    obs2 << kpUn2.pt.x, kpUn2.pt.y;

    g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();
    // vertex(id1)對應的是pKF1 VertexSBAPointXYZ 類型的三維點，內部誤差公式也不同
    e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
    e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
    e21->setMeasurement(obs2);
    float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
    e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);
    g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;
    e21->setRobustKernel(rk2);
    rk2->setDelta(deltaHuber);
    optimizer.addEdge(e21);

    vpEdges12.push_back(e12);
    vpEdges21.push_back(e21);
    vnIndexEdge.push_back(i);
}

5. g2o開始優化，先迭代5次

optimizer.initializeOptimization();
optimizer.optimize(5);

6. 用卡方檢驗剔除誤差大的邊

int nBad=0;
for(size_t i=0; i<vpEdges12.size();i++)
{
    g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
    g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
    if(!e12 || !e21)
        continue;

    if(e12->chi2()>th2 || e21->chi2()>th2)
    {
        // 正向或反向投影任意一個超過誤差閾值就刪掉該邊
        size_t idx = vnIndexEdge[i];
        vpMatches1[idx]=static_cast<MapPoint*>(NULL);
        optimizer.removeEdge(e12);
        optimizer.removeEdge(e21);
        vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
        vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
        // 累計刪掉的邊 數目
        nBad++;
    }
}

// 如果有誤差較大的邊被剔除那麼說明回環質量並不是非常好,還要多迭代幾次;反之就少迭代幾次
int nMoreIterations;
if(nBad>0)
    nMoreIterations=10;
else
    nMoreIterations=5;

// 如果經過上面的剔除後剩下的匹配關係已經非常少了,那麼就放棄優化。內點數直接設置為0
if(nCorrespondences-nBad<10)
    return 0;     

7. 再次g2o優化剔除後剩下的邊

optimizer.initializeOptimization();
optimizer.optimize(nMoreIterations);

// 統計第二次優化之後,這些匹配點中是內點的個數
int nIn = 0;
for(size_t i=0; i<vpEdges12.size();i++)
{
    g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
    g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
    if(!e12 || !e21)
        continue;

    if(e12->chi2()>th2 || e21->chi2()>th2)
    {
        size_t idx = vnIndexEdge[i];
        vpMatches1[idx]=static_cast<MapPoint*>(NULL);
    }
    else
        nIn++;
}

8. 用優化後的結果來更新Sim3位姿

g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
g2oS12= vSim3_recov->estimate();

return nIn;