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

过千帆er 03-13 88

前言：

此刻各位老铁们对“orbslam特征匹配”大约比较注意，我们都需要了解一些“orbslam特征匹配”的相关知识。那么小编也在网络上搜集了一些有关“orbslam特征匹配””的相关知识，希望咱们能喜欢，咱们快快来了解一下吧！

目录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<MapPoint *> &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::PoseMatrixType>();    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<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);//sim3顶点加入到优化器里    // Set MapPoint vertices    // Step 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); //边索引的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; i<N; i++)//循环当前关键帧与保留闭环候选关键帧的匹配地图点索引    {        if(!vpMatches1[i])//无匹配，就跳出            continue;        // pMP1和pMP2是匹配的地图点        MapPoint* pMP1 = vpMapPoints1[i];//取出当前关键帧的地图点        MapPoint* pMP2 = vpMatches1[i];  //取出保留闭环候选关键帧中对应的匹配地图点，vpMatches1[i]，i表示kF1第i个地图点索引，vpMatches1[i]值存放的是kF2中i对应的地图点        // 保证顶点的id能够错开        const int id1 = 2*i+1;   //地图点也是待优化的变量，每个i对应2个地图点顶点，保证顶点id错开        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)//并且不是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<double,2,1> 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<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)));//前面待优化粗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<double,2,1> 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<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);//正向边插入到存储正向边的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; 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)//判断正向或反向是否大于误差阈值10        {            // 正向或反向投影任意一个超过误差阈值就删掉该边            size_t idx = vnIndexEdge[i];//取出边的索引            vpMatches1[idx]=static_cast<MapPoint*>(NULL);//当前关键帧和保留闭环候选关键帧的特征点索引对应的地图点置为NULL            optimizer.removeEdge(e12);//优化器中删除当前循环的正向误差边            optimizer.removeEdge(e21);//优化器中删除当前循环的反向误差边            vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);       //上面存储正向误差边的vector中，把当前循环的正向误差边，置为NULL            vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(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; 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)//判断正向或反向是否大于误差阈值10        {            size_t idx = vnIndexEdge[i];//取出边的索引            vpMatches1[idx]=static_cast<MapPoint*>(NULL);//当前关键帧和保留闭环候选关键帧的idx索引对应的地图点置为NULL            //这里没有remove，因为我们不优化了，所以没必要删除边了        }        else            nIn++;//内点个数++    }    // Recover optimized Sim3    // Step 8：最后得到优化后的结果    g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(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<g2o::BlockSolverX::PoseMatrixType>();    // （图中第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<Vector7d> update(const_cast<double*>(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<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);//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<const Vector3d> v(update);	      _estimate += v; // 地图点更新（当然vPoint1->setFixed(true);那就不更新了）	    } }12345678910111213

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

    // 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<const VertexSim3Expmap*>(_vertices[1]);//v1的类型是VertexSim3Expmap*，对应的顶点标号是1，即待优化的粗sim3      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);//v2的类型是VertexSBAPointXYZ*，对应的顶点标号是0，即保留候选关键帧的地图点      Vector2d obs(_measurement);      //这行代码信息量有点大，下面展开来看      _error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));    }}12345678910111213

上面的计算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<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)));//前面待优化粗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);//把构建好的边，放到优化器里123456789101112131415

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

        e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));//把保留闭环候选关键帧的地图点作为边的其中一个顶点        e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));//前面待优化粗sim3顶点设置时，id设置为0，把待优化的粗sim3作为边的另一个顶点                //首先我们知道连接的两个顶点为        dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)) // 三维地图点        dynamic_cast<g2o::OptimizableGraph::Vertex*>(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<const VertexSim3Expmap*>(_vertices[1]);		const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);        // 显然，        // _vertices[1]对应const VertexSim3Expmap*        // _vertices[0]对应const VertexSBAPointXYZ*        // 所以就有         setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));        setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(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; 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)//判断正向或反向是否大于误差阈值10        {            // 正向或反向投影任意一个超过误差阈值就删掉该边            size_t idx = vnIndexEdge[i];//取出边的索引            vpMatches1[idx]=static_cast<MapPoint*>(NULL);//当前关键帧和保留闭环候选关键帧的特征点索引对应的地图点置为NULL            optimizer.removeEdge(e12);//优化器中删除当前循环的正向误差边            optimizer.removeEdge(e21);//优化器中删除当前循环的反向误差边            vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);       //上面存储正向误差边的vector中，把当前循环的正向误差边，置为NULL            vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(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; 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)//判断正向或反向是否大于误差阈值10        {            size_t idx = vnIndexEdge[i];//取出边的索引            vpMatches1[idx]=static_cast<MapPoint*>(NULL);//当前关键帧和保留闭环候选关键帧的idx索引对应的地图点置为NULL            //这里没有remove，因为我们不优化了，所以没必要删除边了        }        else            nIn++;//内点个数++    }    // Recover optimized Sim3    // Step 8：最后得到优化后的结果    g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));//最后得到的优化结果，就是优化器里顶点为0的sim3相似变换矩阵，取出来放到vSim3_recov    g2oS12= vSim3_recov->estimate();//把优化后的sim3取出来，更新g2oS12        return nIn;//返回内点数

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

本文地址：http://www.longkongtuishu.com/caac6BA1sA1YHD1Q.html

标签： #orbslam特征匹配