ORBSLAM3的g2o优化之位姿图优化（PoseOptimization）

PoseOptimization(Frame *pFrame)函数注释

该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位。

前面捋清了g2o编程套路，下面对ORB-SLAM3中的PoseOptimization(Frame *pFrame)函数进行注释理解。

算法流程：1、构造g2o优化问题并执行优化；2、每次优化完成后，对地图点是否为外点进行卡方检验，检验到为内点的加入下次优化，否则下次不优化；3、得到优化后的当前帧的位姿（此函数只优化当前帧位姿）

int Optimizer::PoseOptimization(Frame *pFrame)
{
    // Step 1：这里先构造了大boss--g2o稀疏优化器, BlockSolver_6_3表示：位姿为6维，路标点是3维
    g2o::SparseOptimizer optimizer;//图模型
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;//线性求解器声明

	// 创建一个线性求解器
    linearSolver = new g2o::LinearSolverDense<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);

    // 输入的帧中,有效的,参与优化过程的2D-3D点对
    int nInitialCorrespondences=0;
    
    // Step 2：添加顶点：待优化当前帧的位姿
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();//创建一个顶点
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));//转化成四元数+平移向量得形式
     // 设置id
    vSE3->setId(0);    
    // 要优化的变量，所以不能固定
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);//添加顶点到优化器

    // 地图点得个数，也就是要往优化器里添加得地图点个数。用于计算误差边（重投影误差）
    const int N = pFrame->N;

    vector<ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
    vector<ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *> vpEdgesMono_FHR;
    vector<size_t> vnIndexEdgeMono, vnIndexEdgeRight;
    vpEdgesMono.reserve(N);
    vpEdgesMono_FHR.reserve(N);
    vnIndexEdgeMono.reserve(N);
    vnIndexEdgeRight.reserve(N);

    vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
    vector<size_t> vnIndexEdgeStereo;
    vpEdgesStereo.reserve(N);
    vnIndexEdgeStereo.reserve(N);

	// 下面涉及卡方分布去除外点的知识，这里不做讲解
    // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
    const float deltaMono = sqrt(5.991);  
    // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815   
    const float deltaStereo = sqrt(7.815);     

    // Step 3：添加一元边（因为此函数只优化当前位姿）
    {
    // 锁定地图点。因为系统是多线程，所以在取数据时要加锁才能保证线程安全。
    // 另一方面，需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误
    unique_lock<mutex> lock(MapPoint::mGlobalMutex);

    // 遍历当前地图中的所有地图点
    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = pFrame->mvpMapPoints[i];
        // 如果这个地图点存在
        if(pMP)
        {
            if(!pFrame->mpCamera2)
            {
            	// 单目情况
                if(pFrame->mvuRight[i]<0)
                {
                    nInitialCorrespondences++;
                    pFrame->mvbOutlier[i] = false;// 先默认此地图点不是外点

                	// 对这个地图点的观测
                    Eigen::Matrix<double,2,1> obs;
                    const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
                    obs << kpUn.pt.x, kpUn.pt.y;
                	// 新建边,这个边只优化位姿Pose
                    ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();

                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
                    e->setMeasurement(obs);//设置测量值
                	// 这个点的置信度，其与特征点所在的图层有关。用信息矩阵（协方差矩阵得逆）来表示。
                    const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
                    e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
                	// 在这里使用了鲁棒核函数
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(deltaMono);

                	// 设置相机内参
                    e->pCamera = pFrame->mpCamera;
                	// 地图点的空间位置,作为迭代的初始值
                    cv::Mat Xw = pMP->GetWorldPos();
                    e->Xw[0] = Xw.at<float>(0);
                    e->Xw[1] = Xw.at<float>(1);
                    e->Xw[2] = Xw.at<float>(2);

                    optimizer.addEdge(e);//将此边加入优化器

                    vpEdgesMono.push_back(e);//记录边属于单目情况
                    vnIndexEdgeMono.push_back(i);//记录索引
                }
                else  // Stereo observation
                {
                    nInitialCorrespondences++;
                    pFrame->mvbOutlier[i] = false;

                	// 观测多了一项右目的坐标
                    Eigen::Matrix<double,3,1> obs;
                    const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
                    const float &kp_ur = pFrame->mvuRight[i];
                    obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
                	// 新建节点,注意这里也是只优化位姿
                    g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();

                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
                    e->setMeasurement(obs);
                	// 置信程度主要是看左目特征点所在的图层
                    const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
                    Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                    e->setInformation(Info);

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

					//获得内参和地图点坐标作为初始值
                    e->fx = pFrame->fx;
                    e->fy = pFrame->fy;
                    e->cx = pFrame->cx;
                    e->cy = pFrame->cy;
                    e->bf = pFrame->mbf;
                    cv::Mat Xw = pMP->GetWorldPos();
                    e->Xw[0] = Xw.at<float>(0);
                    e->Xw[1] = Xw.at<float>(1);
                    e->Xw[2] = Xw.at<float>(2);

                    optimizer.addEdge(e);

                    vpEdgesStereo.push_back(e);
                    vnIndexEdgeStereo.push_back(i);
                }
            }
        }
    }
    }
    // 如果没有足够的匹配点,那么就只好放弃了
    //cout << "PO: vnIndexEdgeMono.size() = " << vnIndexEdgeMono.size() << "   vnIndexEdgeRight.size() = " << vnIndexEdgeRight.size() << endl;
    if(nInitialCorrespondences<3)
        return 0;

    // Step 4：开始优化，总共优化四次，每次优化迭代10次,每次优化后，将观测分为outlier和inlier，outlier不参与下次优化
    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    const float chi2Mono[4]={5.991,5.991,5.991,5.991};          // 单目
    const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};       // 双目
    const int its[4]={10,10,10,10};// 四次迭代，每次迭代的次数

    // bad 的地图点个数
    int nBad=0;
    // 一共进行四次优化
    for(size_t it=0; it<4; it++)
    {
		//这里面计算了重投影误差。
        vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
        // 其实就是初始化优化器,这里的参数0就算是不填写,默认也是0,也就是只对level为0的边进行优化
        optimizer.initializeOptimization(0);
        // 开始优化，优化10次
        optimizer.optimize(its[it]);

        nBad=0;
        // 优化结束,开始遍历参与优化的每一条误差边(单目)，其实就是重投影误差
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
        {
            ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

            const size_t idx = vnIndexEdgeMono[i];

			// 下面是卡方检验
			// 由于每次优化后是对所有的观测进行卡方检验outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然
            // 如果这条误差边是来自于outlier
            if(pFrame->mvbOutlier[idx])
            {
                e->computeError();
            }

            // 就是error*\Omega*error,表征了这个点的误差大小(考虑置信度以后)
            const float chi2 = e->chi2();
			//检验不通过
            if(chi2>chi2Mono[it])
            {                
                pFrame->mvbOutlier[idx]=true;
                e->setLevel(1);   // 设置为outlier , level 1 对应为外点,上面的过程中我们设置其为不优化
                nBad++;
            }
            else
            {
                pFrame->mvbOutlier[idx]=false;
                e->setLevel(0);                 // 设置为inlier, level 0 对应为内点,上面的过程中我们就是要优化这些关系
            }

            if(it==2)
                e->setRobustKernel(0); // 除了前两次优化需要RobustKernel以外, 其余的优化都不需要 -- 因为重投影的误差已经有明显的下降了
        }
		// ……
        
    // Step 5 得到优化后的当前帧的位姿
    g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
    g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
    cv::Mat pose = Converter::toCvMat(SE3quat_recov);
    pFrame->SetPose(pose);// 设置优化后得位姿

    //cout << "[PoseOptimization]: initial correspondences-> " << nInitialCorrespondences << " --- outliers-> " << nBad << endl;
    // 并且返回内点数目
    return nInitialCorrespondences-nBad;
}