6、ORB-SLAM闭环检测之通过g2o优化求解出来的sim3

6、ORB-SLAM闭环检测之通过g2o优化求解出来的sim3

目录

• 1 OptimizeSim3（）
• 2 图解每一步
• • 2.1 初始化g2o优化器
  • 2.2 g2o添加顶点编程
  • • 2.2.1 设置Sim3 作为顶点
    • 2.2.2 设置地图点作为顶点
  • 2.3 g2o添加边编程
  • • 2.3.1 构建两元边
  • 2.4 开始优化
• 3 非常感谢您的阅读！！！

1 OptimizeSim3（）

经过前面那么多步骤，终于来到闭环检测的最后一步了，只要sim3优化通过则证明真正检测到了闭环，然后接下来的工作才是闭环矫正。
关于g2o优化的知识，如果没有学习的话，推荐看我之前的关于g2o介绍以及编程步骤的博客。
首先把整个优化代码贴在下面，然后一步步图解。这里面还包含了g2o顶点和边的编程套路，关于此可以阅读g2o顶点编程和g2o边编程。

int Optimizer::OptimizeSim3(Keyframe *pKF1, Keyframe *pKF2, vector &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
{
    // Step 1：初始化g2o优化器
    // 先构造求解器
    g2o::SparseOptimizer optimizer;
    // 构造线性方程求解器，Hx = -b的求解器
    g2o::BlockSolverX::LinearSolverType * linearSolver;
    // 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器）
    linearSolver = new g2o::LinearSolverDense();
    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
    // 使用L-M迭代
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    // 内参矩阵
    const cv::Mat &K1 = pKF1->mK; //取出当前关键帧内参
    const cv::Mat &K2 = pKF2->mK; //取保留闭环候选关键帧内参

    // Camera poses
    const cv::Mat R1w = pKF1->GetRotation();    //取出当前关键帧的旋转矩阵R
    const cv::Mat t1w = pKF1->GetTranslation(); //取出当前关键帧的平移向量t
    const cv::Mat R2w = pKF2->GetRotation();    //取出保留闭环候选关键帧的旋转矩阵R
    const cv::Mat t2w = pKF2->GetTranslation(); //取出保留闭环候选关键帧的平移向量t

    // Step 2: 设置Sim3 作为顶点         //要优化两帧之间得sim3，待优化变量sim3作为顶点
    g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap(); //new一个sim3作为顶点   
    // 根据传感器类型决定是否固定尺度
    vSim3->_fix_scale=bFixScale;//单目固定 ，双目RGBD不固定
    vSim3->setEstimate(g2oS12);//保留闭环候选关键帧到当前关键帧的粗Sim3变换放到优化变量顶点中，作为初始估计值
    vSim3->setId(0);//设置为第0个顶点
    // Sim3 需要优化
    vSim3->setFixed(false);                             // 因为要优化Sim3顶点，不固定它，所以设置为false
    vSim3->_principle_point1[0] = K1.at(0,2);    // 光心横坐标cx   //设置各种顶点参数
    vSim3->_principle_point1[1] = K1.at(1,2);    // 光心纵坐标cy
    vSim3->_focal_length1[0] = K1.at(0,0);       // 焦距 fx
    vSim3->_focal_length1[1] = K1.at(1,1);       // 焦距 fy
    vSim3->_principle_point2[0] = K2.at(0,2);
    vSim3->_principle_point2[1] = K2.at(1,2);
    vSim3->_focal_length2[0] = K2.at(0,0);
    vSim3->_focal_length2[1] = K2.at(1,1);
    optimizer.addVertex(vSim3);//sim3顶点加入到优化器里

    // Set MapPoint vertices
    // Step 3: 设置地图点作为顶点
    const int N = vpMatches1.size();//取出当前关键帧与保留闭环候选关键帧的匹配地图点（内点）大小
    // 获取pKF1的地图点
    const vector vpMapPoints1 = pKF1->GetMapPointMatches();//当前关键帧的地图点取出来

    vector vpEdges12;         //pKF2对应的地图点到pKF1的投影边   //保留闭环候选关键帧对应的地图点到当前关键帧的投影边 
    vector vpEdges21;  //pKF1对应的地图点到pKF2的投影边   //当前关键帧对应的地图点到保留闭环候选关键帧的投影边
    vector vnIndexEdge;                         //边的索引

    vnIndexEdge.reserve(2*N); //边索引的vector，预留空间
    vpEdges12.reserve(2*N);   //存储正向边的vector，预留空间
    vpEdges21.reserve(2*N);   //存储反向边的vector，预留空间

    // 核函数的阈值
    const float deltaHuber = sqrt(th2);  //设置鲁棒核函数阈值10

    int nCorrespondences = 0;//匹配对，初始为0

    // 遍历每对匹配点
    for(int i=0; iGetIndexInKeyframe(pKF2);//取出保留闭环候选关键帧对应的地图点的特征点索引

        if(pMP1 && pMP2)//如果当前关键帧的地图点和保留候选关键帧对应的地图点都存在
        {
            if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)//并且不是Bad，且保留闭环候选关键帧对应的地图点的特征点索引也存在
            {
                // 如果这对匹配点都靠谱，并且对应的2D特征点也都存在的话，添加PointXYZ顶点
                g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();//定义地图点顶点，new了一个地图点顶点
                // 地图点转换到各自相机坐标系下的三维点
                cv::Mat P3D1w = pMP1->GetWorldPos();//取出当前关键帧的地图点的世界坐标系
                cv::Mat P3D1c = R1w*P3D1w + t1w;//转换到当前关键帧的相机坐标系下
                vPoint1->setEstimate(Converter::toVector3d(P3D1c));//把当前关键帧的相机坐标系作为地图顶点的初始值（转换成EigenEigen::Vector3d类型）
                vPoint1->setId(id1);//设置地图点id
                // 地图点不优化
                vPoint1->setFixed(true);//true地图点不优化，认为地图点是准的，只优化当前关键帧到保留闭环候选关键帧的相似变换sim3
                optimizer.addVertex(vPoint1);//把地图点顶点添加到优化器里

                g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();//定义地图点顶点，new了一个地图点顶点
                cv::Mat P3D2w = pMP2->GetWorldPos();//取出保留候选关键帧的地图点的世界坐标系
                cv::Mat P3D2c = R2w*P3D2w + t2w;//转换到保留候选关键帧的相机坐标系下
                vPoint2->setEstimate(Converter::toVector3d(P3D2c));//把留候选关键帧的相机坐标系作为地图顶点的初始值（转换成EigenEigen::Vector3d类型）
                vPoint2->setId(id2);//设置地图点id
                vPoint2->setFixed(true);//true地图点不优化，认为地图点是准的，只优化保留候选关键帧到当前关键帧的相似变换sim3
                optimizer.addVertex(vPoint2);//把地图点顶点添加到优化器里
            }
            else//地图点是Bad，且保留候选关键帧对应的地图点在候选关键帧中索引也不存在
                continue;
        }
        else //如果当前关键帧的地图点和保留候选关键帧对应的地图点任一不存在，就跳过
            continue;

        // 对匹配关系进行计数v
        nCorrespondences++;

        // Step 4: 添加边（地图点投影到特征点）
        // Set edge x1 = S12*X2

        // 地图点pMP1对应的观测特征点
        Eigen::Matrix obs1;//定义观测
        const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];//取出当前循环的当前关键帧的特征点，因为要做正向投影，即保留闭环候选关键帧地图点投影到当前关键帧的边（观测） 
        obs1 << kpUn1.pt.x, kpUn1.pt.y;//把当前循环的当前关键帧的特征点放到观测里

        // Step 4.1 闭环候选帧地图点投影到当前关键帧的边 -- 正向投影
        g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();//new了一个边，边的误差 = 观测 - 投影，跳转
        // vertex(id2)对应的是pKF2 VertexSBAPointXYZ 类型的三维点
        e12->setVertex(0, dynamic_cast(optimizer.vertex(id2)));//把保留闭环候选关键帧的地图点作为边的其中一个顶点
        // ? 为什么这里添加的节点的id为0？
        // 回答：因为vertex(0)对应的是 VertexSim3Expmap 类型的待优化Sim3，其id 为 0
        e12->setVertex(1, dynamic_cast(optimizer.vertex(0)));//前面待优化粗sim3顶点设置时，id设置为0，把待优化的粗sim3作为边的另一个顶点
        e12->setMeasurement(obs1);//把观测放到边里
        // 信息矩阵和这个特征点的可靠程度（在图像金字塔中的图层）有关
        const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];//当前循环的当前关键帧的特征点的金字塔的层数的倒数 //层数越高，置信度越低
        e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);   //设置信息矩阵=单位矩阵*金字塔的层数的倒数

        // 使用鲁棒核函数
        g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;//new一个鲁棒核函数
        e12->setRobustKernel(rk1);//边设置鲁棒核函数
        rk1->setDelta(deltaHuber);//鲁棒核函数设置判断误差过大的卡方阈值
        optimizer.addEdge(e12);//把构建好的边，放到优化器里

        // Set edge x2 = S21*X1
        // Step 4.2 当前帧地图点投影到闭环候选帧的边 -- 反向投影    //同理上面

        // 地图点pMP2对应的观测特征点
        Eigen::Matrix obs2;
        const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
        obs2 << kpUn2.pt.x, kpUn2.pt.y;v

        g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();
        // vertex(id1)对应的是pKF1 VertexSBAPointXYZ 类型的三维点，内部误差公式也不同
        e21->setVertex(0, dynamic_cast(optimizer.vertex(id1)));
        e21->setVertex(1, dynamic_cast(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);//正向边插入到存储正向边的vector，后面根据优化结果进行筛选
        vpEdges21.push_back(e21);//反向边插入到存储反向边的vector，后面根据优化结果进行筛选
        vnIndexEdge.push_back(i);//边的索引插入到边索引的vector
    }

    // Optimize!
    // Step 5：g2o开始优化，先迭代5次
    optimizer.initializeOptimization();// 其实就是初始化优化器，这里的参数0就算是不填写，默认也是0，也就是只对level为0的边进行优化
    optimizer.optimize(5);//迭代5次

    // Step 6：用卡方检验剔除误差大的边
    // Check inliers
    int nBad=0;//定义需要删除的误差大的边
    for(size_t i=0; ichi2()>th2 || e21->chi2()>th2)//判断正向或反向是否大于误差阈值10
        {
            // 正向或反向投影任意一个超过误差阈值就删掉该边
            size_t idx = vnIndexEdge[i];//取出边的索引
            vpMatches1[idx]=static_cast(NULL);//当前关键帧和保留闭环候选关键帧的特征点索引对应的地图点置为NULL
            optimizer.removeEdge(e12);//优化器中删除当前循环的正向误差边
            optimizer.removeEdge(e21);//优化器中删除当前循环的反向误差边
            vpEdges12[i]=static_cast(NULL);       //上面存储正向误差边的vector中，把当前循环的正向误差边，置为NULL
            vpEdges21[i]=static_cast(NULL);//上面存储反向误差边的vector中，把当前循环的反向误差边，置为NULL
            // 累计删掉的边 数目
            nBad++;//删除的误差大的边数目++
        }
    }

    // 如果有误差较大的边被剔除，那么说明回环质量并不是非常好，还要多迭代几次；反之就少迭代几次
    int nMoreIterations;//定义更精细迭代次数
    if(nBad>0)//判断如果有误差较大的边，说明回环质量不太好
        nMoreIterations=10;//更精细迭代次数设大，多迭代几次
    else //判断如果没有误差较大的边，说明回环质量还行
        nMoreIterations=5;//更精细迭代次数设小

    // 如果经过上面的剔除后剩下的匹配关系已经非常少了,那么就放弃优化。内点数直接设置为0
    if(nCorrespondences-nBad<10)
        return 0;     

    // Optimize again only with inliers
    // Step 7：再次g2o优化 剔除后剩下的边     //对第一次迭代后，并且剔除过误差大的边后，在进行一次更精细迭代
    optimizer.initializeOptimization();// 其实就是初始化优化器，这里的参数0就算是不填写，默认也是0，也就是只对level为0的边进行优化
    optimizer.optimize(nMoreIterations);//设置迭代次数

    // 统计第二次优化之后,这些匹配点中是内点的个数
    int nIn = 0;//定义内点个数
    for(size_t i=0; ichi2()>th2 || e21->chi2()>th2)//判断正向或反向是否大于误差阈值10
        {
            size_t idx = vnIndexEdge[i];//取出边的索引
            vpMatches1[idx]=static_cast(NULL);//当前关键帧和保留闭环候选关键帧的idx索引对应的地图点置为NULL
            //这里没有remove，因为我们不优化了，所以没必要删除边了
        }
        else
            nIn++;//内点个数++
    }

    // Recover optimized Sim3
    // Step 8：最后得到优化后的结果
    g2o::VertexSim3Expmap* vSim3_recov = static_cast(optimizer.vertex(0));//最后得到的优化结果，就是优化器里顶点为0的sim3相似变换矩阵，取出来放到vSim3_recov
    g2oS12= vSim3_recov->estimate();//把优化后的sim3取出来，更新g2oS12

    return nIn;//返回内点数
}

2 图解每一步

首先得知道这个优化问题的模型，在解决什么样的问题：如下图所示，利用前面寻找到的所有地图点和估算的sim3变换，以及两关键帧的内外参数，对两关键帧的每个地图点相对另一关键帧构建一个重投影误差和函数，然后通过最小化这个目标函数优化估计的sim3变换。
注意：因为边为通过两个顶点构建的重投影误差，所以它是两元边，在优化过程中地图点设置为固定，只优化sim3顶点。

2.1 初始化g2o优化器

虽然有些出入，但总体是按照上图的步骤来的，很容易看明白。

    // Step 1：初始化g2o优化器
    // （图中的第4步）构造求解器
    g2o::SparseOptimizer optimizer;
    // （图中的第1步）构造线性方程求解器，Hx = -b的求解器，选用dense的求解器
    g2o::BlockSolverX::LinearSolverType * linearSolver;
    linearSolver = new g2o::LinearSolverDense();
    // （图中第2步）创建由线性求解器初始化的块求解器
    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
    // （图中的第3步）使用L-M迭代
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    // 也就是说，可以把上面g2o::SparseOptimizer optimizer;语句放到这个位置
    optimizer.setAlgorithm(solver);
 	// 第5步在后面（构建顶点和边）
 	
    // 准备工作（为了计算地图点的投影取出各自的内外参数）
    const cv::Mat &K1 = pKF1->mK; //取出当前关键帧内参
    const cv::Mat &K2 = pKF2->mK; //取保留闭环候选关键帧内参

    const cv::Mat R1w = pKF1->GetRotation();    //取出当前关键帧的旋转矩阵R
    const cv::Mat t1w = pKF1->GetTranslation(); //取出当前关键帧的平移向量t
    const cv::Mat R2w = pKF2->GetRotation();    //取出保留闭环候选关键帧的旋转矩阵R
    const cv::Mat t2w = pKF2->GetTranslation(); //取出保留闭环候选关键帧的平移向量t

2.2 g2o添加顶点编程 2.2.1 设置Sim3 作为顶点

ORBSLAM对sim3顶点对象进行了定义：
它维护更新的是一个7维向量。(x,y,z,qw,qx,qy,qz)。
这里面有两个比较重要的函数。主要完成两个任务：设置待优化变量；更新它。

class VertexSim3Expmap : public baseVertex<7, Sim3>
{
    //……
    virtual void setToOriginImpl() {
      _estimate = Sim3();// 设置待优化变量为sim3类型
    }

    virtual void oplusImpl(const double* update_)
    {
      Eigen::Map update(const_cast(update_));

      if (_fix_scale)
        update[6] = 0;// 如果固定尺度的话s=1，Sim3(const Vector7d & update)源码里s = std::exp(sigma);而double sigma = update[6];

      Sim3 s(update);// 把更新量转化为sim3形式
      setEstimate(s*estimate());//更新到_estimate，源码： void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}
    }

    //……
}

下面是设置sim3顶点的实现过程。

    // Step 2: 设置Sim3 作为顶点         //要优化两帧之间得sim3，待优化变量sim3作为顶点
    g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap(); //new一个sim3作为顶点   
    // 根据传感器类型决定是否固定尺度
    vSim3->_fix_scale=bFixScale;//单目固定 ，双目RGBD不固定
    vSim3->setEstimate(g2oS12);//保留闭环候选关键帧到当前关键帧的粗Sim3变换放到优化变量顶点中，作为初始估计值
    vSim3->setId(0);//设置为第0个顶点
    // Sim3 需要优化
    vSim3->setFixed(false);                             // 因为要优化Sim3顶点，不固定它，所以设置为false
    vSim3->_principle_point1[0] = K1.at(0,2);    // 光心横坐标cx   //设置各种顶点参数
    vSim3->_principle_point1[1] = K1.at(1,2);    // 光心纵坐标cy
    vSim3->_focal_length1[0] = K1.at(0,0);       // 焦距 fx
    vSim3->_focal_length1[1] = K1.at(1,1);       // 焦距 fy
    vSim3->_principle_point2[0] = K2.at(0,2);
    vSim3->_principle_point2[1] = K2.at(1,2);
    vSim3->_focal_length2[0] = K2.at(0,0);
    vSim3->_focal_length2[1] = K2.at(1,1);
    optimizer.addVertex(vSim3);//sim3顶点加入到优化器里

2.2.2 设置地图点作为顶点

从对地图点顶点的定义可以看出，它维护的是一个三维向量。

 class VertexSBAPointXYZ : public baseVertex<3, Vector3d>
 {
       // ……
	    virtual void setToOriginImpl() {
	      _estimate.fill(0.); //地图点位置以0填充，相当于初始化
	    }
	
	    virtual void oplusImpl(const double* update)
	    {
	      Eigen::Map v(update);
	      _estimate += v; // 地图点更新（当然vPoint1->setFixed(true);那就不更新了）
	    }
 }

下面只看核心代码，剩余代码的逻辑仔细捋一捋即可。

    // Step 3: 设置地图点作为顶点
   
        //……
                g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();//定义地图点顶点，new了一个地图点顶点
                // 地图点转换到各自相机坐标系下的三维点
                cv::Mat P3D1w = pMP1->GetWorldPos();//取出当前关键帧的地图点的世界坐标系
                cv::Mat P3D1c = R1w*P3D1w + t1w;//转换到当前关键帧的相机坐标系下
                vPoint1->setEstimate(Converter::toVector3d(P3D1c));//把当前关键帧的相机坐标系作为地图顶点的初始值（转换成EigenEigen::Vector3d类型）
                vPoint1->setId(id1);//设置地图点id
                // 地图点不优化
                vPoint1->setFixed(true);//true地图点不优化，认为地图点是准的，只优化当前关键帧到保留闭环候选关键帧的相似变换sim3
                optimizer.addVertex(vPoint1);//把地图点顶点添加到优化器里

                g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();//定义地图点顶点，new了一个地图点顶点
                cv::Mat P3D2w = pMP2->GetWorldPos();//取出保留候选关键帧的地图点的世界坐标系
                cv::Mat P3D2c = R2w*P3D2w + t2w;//转换到保留候选关键帧的相机坐标系下
                vPoint2->setEstimate(Converter::toVector3d(P3D2c));//把留候选关键帧的相机坐标系作为地图顶点的初始值（转换成EigenEigen::Vector3d类型）
                vPoint2->setId(id2);//设置地图点id
                vPoint2->setFixed(true);//true地图点不优化，认为地图点是准的，只优化保留候选关键帧到当前关键帧的相似变换sim3
                optimizer.addVertex(vPoint2);//把地图点顶点添加到优化器里
     
            // ……
        }

2.3 g2o添加边编程

从这个边的定义来看，它是个二元边。第1个2是说测量值是2维的，也就是图像像素坐标x,y的差值，对应测量值的类型是Vector2d，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和sim3变换VertexSim3Expmap。它代表的是三维点的重投影误差，所以这个边的类里面提供了这个计算。

class EdgeSim3ProjectXYZ : public  baseBinaryEdge<2, Vector2d,  VertexSBAPointXYZ, VertexSim3Expmap>
{
	void computeError()//重投影误差计算
    {
      //计算重投影误差，需要被投影帧相机的内参（前面地图点已经经过相机外参转化到相机坐标系了），投影帧的地图点以及两帧之间的sim3变换。
      const VertexSim3Expmap* v1 = static_cast(_vertices[1]);//v1的类型是VertexSim3Expmap*，对应的顶点标号是1，即待优化的粗sim3
      const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);//v2的类型是VertexSBAPointXYZ*，对应的顶点标号是0，即保留候选关键帧的地图点

      Vector2d obs(_measurement);
      //这行代码信息量有点大，下面展开来看
      _error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));
    }
}

上面的计算error的代码可能不太好理解，下面展开来看：

       // 从右往左看，第一层。
       v1->estimate().map(v2->estimate())；
       // v2->estimate()保留闭环候选关键帧相机坐标系下的地图点
       // 下面是map函数源码，看出来了吧，就是用v1估计的sim12变换把v2代表的相机2坐标系下三维点变换到相机1坐标系下
       struct Sim3
  	   {
	       Vector3d map (const Vector3d& xyz) const {
	     	 return s*(r*xyz) + t;
	       }
	   }


       // 再看第二层
       project(v1->estimate().map(v2->estimate()))；
       // 看源码，真的没啥，就一个归一化 *** 作而已
       Vector2d project(const Vector3d& v)
	  {
		    Vector2d res;
		    res(0) = v(0)/v(2);
		    res(1) = v(1)/v(2);
		    return res;
	  }

      // 来看最后一层
      v1->cam_map1(project(v1->estimate().map(v2->estimate())));
      // 上源码，也没啥大不了的就是一个通过内参转化到像素平面这回事
      class VertexSim3Expmap : public baseVertex<7, Sim3>
      {
	      Vector2d cam_map1(const Vector2d & v) const
	      {
		      Vector2d res;
		      res[0] = v[0]*_focal_length1[0] + _principle_point1[0];
		      res[1] = v[1]*_focal_length1[1] + _principle_point1[1];
		      return res;
	      }
	  }
      //最后整行代码就表达了这么个意思：误差 = 观测 - 投影   

上面整行代码不就是下面那个图的整个投影过程吗！！！

2.3.1 构建两元边

下面我们也只看核心代码，看怎么构建边的。

		g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();//new了一个边
        // vertex(id2)对应的是pKF2 VertexSBAPointXYZ 类型的三维点
        e12->setVertex(0, dynamic_cast(optimizer.vertex(id2)));//把保留闭环候选关键帧的地图点作为边的其中一个顶点
        // ? 为什么这里添加的节点的id为0？因为vertex(0)对应的是 VertexSim3Expmap 类型的待优化Sim3，其id 为 0。下面再讲解一下：
        e12->setVertex(1, dynamic_cast(optimizer.vertex(0)));//前面待优化粗sim3顶点设置时，id设置为0，把待优化的粗sim3作为边的另一个顶点
        e12->setMeasurement(obs1);//把观测放到边里
        // 信息矩阵和这个特征点的可靠程度（在图像金字塔中的图层）有关
        const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];//当前循环的当前关键帧的特征点的金字塔的层数的倒数 //层数越高，置信度越低
        e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);   //设置信息矩阵=单位矩阵*金字塔的层数的倒数

        // 使用鲁棒核函数
        g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;//new一个鲁棒核函数
        e12->setRobustKernel(rk1);//边设置鲁棒核函数
        rk1->setDelta(deltaHuber);//鲁棒核函数设置判断误差过大的卡方阈值
        optimizer.addEdge(e12);//把构建好的边，放到优化器里

有关边的两个定点连接关系，下面再讲解一下：

        e12->setVertex(0, dynamic_cast(optimizer.vertex(id2)));//把保留闭环候选关键帧的地图点作为边的其中一个顶点
        e12->setVertex(1, dynamic_cast(optimizer.vertex(0)));//前面待优化粗sim3顶点设置时，id设置为0，把待优化的粗sim3作为边的另一个顶点
        
        //首先我们知道连接的两个顶点为
        dynamic_cast(optimizer.vertex(id2)) // 三维地图点
        dynamic_cast(optimizer.vertex(0)) // sim3变换
        // 再来看一下setVertex函数
        void setVertex(size_t i, Vertex* v) 
        { 
	        assert(i < _vertices.size() && "index out of bounds"); 
	        _vertices[i]=v;
        }
        // 其中有_vertices[i]=v;所以我们只要对应好这等式两边的类型即可，这得从误差计算那边源码看
        const VertexSim3Expmap* v1 = static_cast(_vertices[1]);
		const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);

        // 显然，
        // _vertices[1]对应const VertexSim3Expmap*
        // _vertices[0]对应const VertexSBAPointXYZ*
        // 所以就有 
        setVertex(0, dynamic_cast(optimizer.vertex(id2)));
        setVertex(1, dynamic_cast(optimizer.vertex(0)));

2.4 开始优化

这里面涉及到卡方检验的知识，这个先按下不表。代码也整体好理解就不做赘述了。

    // Step 5：g2o开始优化，先迭代5次
    optimizer.initializeOptimization();// 其实就是初始化优化器，这里的参数0就算是不填写，默认也是0，也就是只对level为0的边进行优化
    optimizer.optimize(5);//迭代5次

    // Step 6：用卡方检验剔除误差大的边
    // Check inliers
    int nBad=0;//定义需要删除的误差大的边
    for(size_t i=0; ichi2()>th2 || e21->chi2()>th2)//判断正向或反向是否大于误差阈值10
        {
            // 正向或反向投影任意一个超过误差阈值就删掉该边
            size_t idx = vnIndexEdge[i];//取出边的索引
            vpMatches1[idx]=static_cast(NULL);//当前关键帧和保留闭环候选关键帧的特征点索引对应的地图点置为NULL
            optimizer.removeEdge(e12);//优化器中删除当前循环的正向误差边
            optimizer.removeEdge(e21);//优化器中删除当前循环的反向误差边
            vpEdges12[i]=static_cast(NULL);       //上面存储正向误差边的vector中，把当前循环的正向误差边，置为NULL
            vpEdges21[i]=static_cast(NULL);//上面存储反向误差边的vector中，把当前循环的反向误差边，置为NULL
            // 累计删掉的边 数目
            nBad++;//删除的误差大的边数目++
        }
    }

    // 如果有误差较大的边被剔除，那么说明回环质量并不是非常好，还要多迭代几次；反之就少迭代几次
    int nMoreIterations;//定义更精细迭代次数
    if(nBad>0)//判断如果有误差较大的边，说明回环质量不太好
        nMoreIterations=10;//更精细迭代次数设大，多迭代几次
    else //判断如果没有误差较大的边，说明回环质量还行
        nMoreIterations=5;//更精细迭代次数设小

    // 如果经过上面的剔除后剩下的匹配关系已经非常少了,那么就放弃优化。内点数直接设置为0
    if(nCorrespondences-nBad<10)
        return 0;     

    // Optimize again only with inliers
    // Step 7：再次g2o优化 剔除后剩下的边     //对第一次迭代后，并且剔除过误差大的边后，在进行一次更精细迭代
    optimizer.initializeOptimization();// 其实就是初始化优化器，这里的参数0就算是不填写，默认也是0，也就是只对level为0的边进行优化
    optimizer.optimize(nMoreIterations);//设置迭代次数

    // 统计第二次优化之后,这些匹配点中是内点的个数
    int nIn = 0;//定义内点个数
    for(size_t i=0; ichi2()>th2 || e21->chi2()>th2)//判断正向或反向是否大于误差阈值10
        {
            size_t idx = vnIndexEdge[i];//取出边的索引
            vpMatches1[idx]=static_cast(NULL);//当前关键帧和保留闭环候选关键帧的idx索引对应的地图点置为NULL
            //这里没有remove，因为我们不优化了，所以没必要删除边了
        }
        else
            nIn++;//内点个数++
    }

    // Recover optimized Sim3
    // Step 8：最后得到优化后的结果
    g2o::VertexSim3Expmap* vSim3_recov = static_cast(optimizer.vertex(0));//最后得到的优化结果，就是优化器里顶点为0的sim3相似变换矩阵，取出来放到vSim3_recov
    g2oS12= vSim3_recov->estimate();//把优化后的sim3取出来，更新g2oS12
    
    return nIn;//返回内点数

3 非常感谢您的阅读！！！