ORBSLAM2--Optimizer

PoseOptimization

函数原型

/**
 * @brief Pose Only Optimization
 * 
 * 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) \n
 * 只优化Frame的Tcw，不优化MapPoints的坐标
 * 
 * 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
 * 2. Edge:
 *     - g2o::EdgeSE3ProjectXYZOnlyPose()，BaseUnaryEdge
 *         + Vertex：待优化当前帧的Tcw
 *         + measurement：MapPoint在当前帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *     - g2o::EdgeStereoSE3ProjectXYZOnlyPose()，BaseUnaryEdge
 *         + Vertex：待优化当前帧的Tcw
 *         + measurement：MapPoint在当前帧中的二维位置(ul,v,ur)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *
 * @param   pFrame Frame
 * @return  inliers数量
 */
int Optimizer::PoseOptimization(Frame *pFrame)

这里只对pose位姿进行优化，对路标点设置为fix固定，不参与优化。

数学理论

公式
$$Z{P}_{uv}=Z\left(\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right)=K(R{P}_w+t)=KT{P}_w^{\prime }$$
注意由于引入了变换矩阵，那么为了让计算维度一致，那么在$T{P}_w^{\prime }$中隐藏着非齐次到齐次的转换，$P_w^{\prime }=\left(\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right)$且$P_{uv}=\left(\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right)$。在式子中
$$T{P}_w^{\prime }=P_c^{\prime }=\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)$$$$P{c}^{\prime \prime }=P_{c(1:3)}^{\prime }=\left(\begin{array}{c}x\\ y\\ z\end{array}\right)$$
再者
$$P_c=\frac{1}{2}{P}_c^{\prime \prime }=\left(\begin{array}{c}x/z\\ y/z\\ 1\end{array}\right)$$
该公式进行了归一化处理，为了式它能够与K相乘，得到二维的向量，需要转化成归一化坐标，位于相机前方z=1处的平面上，最后得到二维像素坐标
于是有下面式子
$$P_{uv}=K\frac{1}{Z}{P}_c^{\prime \prime }$$

$$Z{P}_{uv}=K{P}_c^{\prime \prime }$$
Z也可以等于$s_i$尺度

构建最小二乘
$${\Im }^{\ast }=argmin\frac{1}{2}\sum _{i=1}^N||u_i-\frac{1}{s_i}Kexp({\Im }^{\wedge })P_w|{|}_2^2$$
其中$s_i$是有摄像头观测到像素信息，那么把它简化成代价函数为
$$e(x)=u_i-\frac{1}{s_i}Kexp({\Im }^{\wedge })P_w$$
其中i=0,1,2…，x是待优化变量，$exp({\Im }^{\wedge })$或者$P_i$，也或者同时是这两个，对于不方便直接求解最小二乘问题，我们可以使用迭代

1. 给定某个初始值$x_0$
2. 对于第k次迭代，寻找一个增量$\Delta x_k$，使得$||f(x_k+\Delta x_k)|{|}_2^2$达到最小值
3. 如若$\Delta x_k$足够小，则停止
4. 否则，领$x_{k+1}=x_k+\Delta x_k$，返回第2步
   那我们要求这里的梯度和下降方向把代价函数进行一阶泰勒展开
   $$e(x+\Delta x)\simeq e(x)+J\Delta x$$
   其中J是e(x)的导数，是一个雅克比矩阵，也是接下来求解的关键，可以知道迭代步长和方向
   使用LM方法进行迭代

1.给定初始值x_0，以及初始优化半径$\mu$
2. 对于第k次迭代，求解

$$mi{n}_{\Delta x_k}\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k|{|}^{2}s.t||D\Delta x_k|{|}^2\le \mu$$
其中$\mu$是信赖区域半径
3.计算$\rho$
4.若$\rho >\frac{3}{4}$，则$\mu =2\mu$
5.若$\rho <\frac{1}{4}$，则$\mu =0.5\mu$
6.若果$\ell$大于某阈值，则认为近似可行。令$x_{k+1}=x_k+\Delta x_k$
7.判断算法是否收敛，如不收敛返回第k次迭代，否则结束
其中$\rho =\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}$,分子是实际函数下降的值，分母是近视模型下降的值，$\ell$接近于1，则近似是好的。
把上面第2步中的约束$||D\Delta x_k|{|}^2\le \mu$用拉格朗日乘子转化成一个无约束的问题
$$mi{n}_{\Delta x_k}\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k|{|}^2+\frac{\lambda }{2}||D\Delta x_k|{|}^2$$
这里的$\lambda$为拉格朗日乘子
把上式平方展开
$$\frac{1}{2}[(e(x)+J(x)\Delta x{)}^T(e(x)+J(x)\Delta x)+\lambda (D\Delta x{)}^T(D\Delta x)]$$
因为要寻找下降矢量$\Delta x$，使得最小二乘达到最小，所以对$\Delta x$求导得到
$$e(x{)}^{T}J(x)+J(x{)}^{T}J(x)\Delta x+\lambda D^{T}D\Delta x=0$$
$$(J(x{)}^{T}J(x)+\lambda D^{T}D)\Delta x=-e(x)J(x{)}^T$$
最后$H=J(x{)}^{T}J(x)-e(x)J(x{)}^T=g$
其中J的求解过程可以看P164

G2O的实现

想要熟悉g2o的使用必须要熟悉它的框架,可参考A和B和C，其中在了解过程中认识到边缘化的作用，其实是随着信息的越来越多，后端得到的雅克比矩阵也会越来越大，为了保证计算量的平稳，需要使用概率论上的边缘分布去忽略一些信息的影响而进行部分信息的处理。简单来时就是对后端得到的雅克比矩阵进行边缘化处理，使之更容易的进行求逆计算，可参考

求解器的创建

这是整个g2o优化框架搭建的首要任务

 // 步骤1：构造g2o优化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    //使用dense cholesky分解法。，求解Hのx=-g线性方程，由于H可能很大，直接求逆可能很困难，因此需要使用一些求解方法
    //LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
    //LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
    //LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
    //LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
    //LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，
    //因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();

    //BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA
    //BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale
    //BlockSolver_3_2：表示pose 是3维，观测点是2维
    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

顶点的创建

这里的顶点是只有一个，那就是当前帧的位姿，对于地图点不是待优化变量，边则是有很多个边，这样就构成了一元边的图。

  // 步骤2：添加顶点：待优化当前帧的Tcw
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
    vSE3->setId(0);//唯一的顶点
    vSE3->setFixed(false);//不固定该顶点，就是参与优化的意思
    optimizer.addVertex(vSE3);

边的创建

这里的双目和RGB-D创建边都是同样一个实现方案，双目的 pFrame->mvuRight[i]是有数据的，且数据可以直接从右图像中得到，但是RGB-D是需要仅通过左图像信息进行求解对这个容器进行填充 kp_ur=kpU.pt.x-mbf/d，前提是d>0的点。

vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
    vector<size_t> vnIndexEdgeMono;
    vpEdgesMono.reserve(N);
    vnIndexEdgeMono.reserve(N);

    //为什么没有for RGBD
    
    // for Stereo
    vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
    vector<size_t> vnIndexEdgeStereo;
    vpEdgesStereo.reserve(N);
    vnIndexEdgeStereo.reserve(N);

    const float deltaMono = sqrt(5.991);
    const float deltaStereo = sqrt(7.815);

    // 步骤3：添加一元边：相机投影模型
    {
    unique_lock<mutex> lock(MapPoint::mGlobalMutex);

    //顶点只有一个，边有很多条
    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = pFrame->mvpMapPoints[i];
        if(pMP)
        {
            // Monocular observation
            // 单目情况, 也有可能在双目下, 当前帧的左兴趣点找不到匹配的右兴趣点
            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;

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

		//因为整个图就只有一个顶点
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
		//Measurement类型，也就是边定义的误差值类型，Vector2d
		//对初始测量值（图像的出来的值）进行赋值
                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->fx = pFrame->fx;
                e->fy = pFrame->fy;
                e->cx = pFrame->cx;
                e->cy = pFrame->cy;
                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;

                //SET EDGE
                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;// 这里和单目不同，深度摄像机kp_ur=kpU.pt.x-mbf/d，前提是d>0的点

                //EdgeStereoSE3ProjectXYZOnlyPose这个类型的边只有一个位姿顶点，且误差是3维的
                g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();// 这里和单目不同

		//因为顶点位姿只有一个，都是第0个顶点，那么每一条边连接的都是相同的位姿顶点，那就是第0个
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
		//Measurement类型，也就是边定义的误差值类型，Vector3d
		//对初始测量值（图像的出来的值）进行赋值
                e->setMeasurement(obs);
		//该特征点被提取的金字塔层kpUn.octave
		//mvInvLevelSigma2该层的逆向尺度因子的平方
                const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
		//Eigen::Matrix3d::Identity()全1矩阵
                Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
		//typedef Matrix InformationType
		//赋值的是信息矩阵，且其类型是上一行
                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);//边的索引就是地图点的索引
            }
        }

优化迭代

// 步骤4：开始优化，总共优化四次，每次优化后，将观测分为outlier和inlier，outlier不参与下次优化
    // 由于每次优化后是对所有的观测进行outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然
    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    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};// 四次迭代，每次迭代的次数

    int nBad=0;
    for(size_t it=0; it<4; it++)
    {

        vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
        optimizer.initializeOptimization(0);// 对level为0的边进行优化，默认参数也是0
        optimizer.optimize(its[it]);//参数表填的是每次迭代的次数

        nBad=0;
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
        {
            g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

            const size_t idx = vnIndexEdgeMono[i];

	    //边的索引
            if(pFrame->mvbOutlier[idx])
            {
                e->computeError(); // NOTE g2o只会计算active edge的误差
            }

            const float chi2 = e->chi2();

            if(chi2>chi2Mono[it])
            {                
                pFrame->mvbOutlier[idx]=true;
                e->setLevel(1);                 // 设置为outlier
                nBad++;
            }
            else
            {
                pFrame->mvbOutlier[idx]=false;
                e->setLevel(0);                 // 设置为inlier
            }

            if(it==2)
                e->setRobustKernel(0); // 除了前两次优化需要RobustKernel以外, 其余的优化都不需要
        }

        for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++)
        {
            g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];

            const size_t idx = vnIndexEdgeStereo[i];

	    //地图点的索引就是边的索引
	    //如果当前边连接的地图点是外点，直接进行误差计算（有什么用）
            if(pFrame->mvbOutlier[idx])
            {
                e->computeError();
            }

            //chi2()返回的是_error.dot(information()*_error)
            const float chi2 = e->chi2();

	    //为什么通过这个判断来进行内外点的设置？
            if(chi2>chi2Stereo[it])
            {
                pFrame->mvbOutlier[idx]=true;
                e->setLevel(1);//把当前边的level设为1，那么前面声明了，只优化level=0的边，那么这里的目的是把属于外点的边不参与下次优化
                nBad++;//坏边数量加1
            }
            else
            {                
                e->setLevel(0);
                pFrame->mvbOutlier[idx]=false;
            }

            if(it==2)
                e->setRobustKernel(0);//在第四次迭代的时候，0作为鲁棒核函数？？？那第四次迭代还有什么用
        }

        if(optimizer.edges().size()<10)
            break;
    }    

    // Recover optimized pose and return number of inliers
    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);//赋值新的位姿给当前帧

    return nInitialCorrespondences-nBad;//返回经过四次优化，剩下好的边数量