淦ORB-SLAM2源码 06--手推Bundle Adjustment优化(BA优化）

最新推荐文章于 2022-09-23 18:05:05 发布

安德鲁JANKENPAN

最新推荐文章于 2022-09-23 18:05:05 发布

阅读量1k

点赞数 1

分类专栏： SLAM

本文链接：https://blog.csdn.net/qq_44047943/article/details/115189520

版权

SLAM 专栏收录该内容

20 篇文章 9 订阅

订阅专栏

手推Bundle Adjustment

BA简介
手推BA运算过程(含最小二乘法）
BA代码

本来是想写一下关于SLAM后端的三种优化方式卡线曼滤波（KF），扩展卡尔曼滤波（EKF）和基于图优化的Bundle Adjustment（后面简称BA）的，但是前面两个我确实不太懂，所以先占个位置，等我后面研究搞懂了再来填坑。

BA简介

视觉里程计可以给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，如果时间长了这个地图是不准确的。所以我们希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。实际当中考虑到精度与性能的平衡，有许多不同的做法。

视觉里程计只有短暂的记忆，在后端优化中，我们通常考虑一段更长时间内（或所有时间内）的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己，这种处理方式不妨称为“批量的”（Batch）。否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定，那不妨称为“渐进的”（Incremental）。所以这是一个假设检验的过程。先验和后验。先由以前的位姿和观测方程预测下一步，然后利用下一步的内容预测这一步的最大可能

所谓的Bundle Adjustment，是指从视觉重建中提炼出最优的3D模型和相机参数（内参数和外参数）。从每一个特征点反射出来的几束光线（bundles of light rays），在我们把相机位姿和特征点空间位置做出最优的调整（adjustment）之后，最后收束到相机光心的这个过程，简称BA。

在SFM（structure from motion）的计算中BA（Bundle Adjustment）作为最后一步优化具有很重要的作用，在近几年兴起的基于图的SLAM（simultaneous localization and mapping）算法里面使用了图优化替代了原来的滤波器，这里所谓的图优化其实也是指BA。其实很多经典的文献对于BA都有深深浅浅的介绍，如果想对BA的全过程做一个全面的更深层次的了解，推荐阅读 Bundle Adjustment —A Modern Synthesis。

Bundle Adjustment用BA代替中文译为光束法平差，大概大家看到更多的翻译可能为束调整、捆集调整或者捆绑调整等等。所谓bundle，来源于bundle of light，其本意就是指的光束，这些光束指的是三维空间中的点投影到像平面上的光束，而重投影误差，正是利用这些光束来构建的，因此称为光束法强调光束也正是描述其优化模型是如何建立的。剩下的就是平差，那什么是平差呢？

由于测量仪器的精度不完善和人为因素及外界条件的影响，测量误差总是不可避免的。为了提高成果的质量，处理好这些测量中存在的误差问题，观测值的个数往往要多于确定未知量所必须观测的个数，也就是要进行多余观测。有了多余观测，势必在观测结果之间产生矛盾，测量平差的目的就在于消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度。测量平差采用的原理就是“最小二乘法”。

手推BA运算过程(含最小二乘法）

其实第一次投影指的就是相机在拍照的时候三维空间点投影到图像上（像素）
然后我们利用这些图像对一些特征点进行三角化（triangulation）
最后利用我们计算得到的三维点的坐标（注意不是真实的，像素三角化得到）和我们计算得到的相机矩阵（当然也不是真实的，PnP得到）进行第二次投影，也就是重投影。

重投影误差是指的真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算的3D点得到的虚拟的像素点）的差值，因为种种原因计算得到的值和实际情况不会完全相符，也就是这个差值不可能恰好为0，此时也就需要将这些差值的和最小化获取最优的相机参数及三维空间点的坐标。

这里也包括了最小二乘的四种方法具体的运算（最速下降法，牛顿法，高斯牛顿法和列文伯格-马夸尔特法）

具体运算过程（手写字丑见谅）

在这里插入图片描述

BA代码

/**
 * @brief bundle adjustment 优化过程
 * 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
 *            g2o::VertexSBAPointXYZ()，MapPoint的mWorldPos
 * 2. Edge:
 *     - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：待优化当前帧的Tcw
 *         + Vertex：待优化MapPoint的mWorldPos
 *         + measurement：MapPoint在当前帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 * 
 * @param[in] vpKFs                 参与BA的所有关键帧
 * @param[in] vpMP                  参与BA的所有地图点
 * @param[in] nIterations           优化迭代次数
 * @param[in] pbStopFlag            外部控制BA结束标志
 * @param[in] nLoopKF               形成了闭环的当前关键帧的id
 * @param[in] bRobust               是否使用核函数
 */
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
                                 int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
    // 不参与优化的地图点
    vector<bool> vbNotIncludedMP;
    vbNotIncludedMP.resize(vpMP.size());

    // Step 1 初始化g2o优化器
    g2o::SparseOptimizer optimizer;
    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);

    // 如果这个时候外部请求终止，那就结束
    // 注意这句执行之后，外部再请求结束BA，就结束不了了
    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    // 记录添加到优化器中的顶点的最大关键帧id
    long unsigned int maxKFid = 0;

    // Step 2 向优化器添加顶点

    // Set KeyFrame vertices
    // Step 2.1 ：向优化器添加关键帧位姿顶点
    // 对于当前地图中的所有关键帧
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        // 去除无效的
        if(pKF->isBad())
            continue;
        
        // 对于每一个能用的关键帧构造SE3顶点,其实就是当前关键帧的位姿
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
        // 顶点的id就是关键帧在所有关键帧中的id
        vSE3->setId(pKF->mnId); 
        // 只有第0帧关键帧不优化（参考基准）
        vSE3->setFixed(pKF->mnId==0);

        // 向优化器中添加顶点，并且更新maxKFid
        optimizer.addVertex(vSE3);
        if(pKF->mnId>maxKFid)
            maxKFid=pKF->mnId;
    }

    // 卡方分布 95% 以上可信度的时候的阈值
    const float thHuber2D = sqrt(5.99);     // 自由度为2
    const float thHuber3D = sqrt(7.815);    // 自由度为3

    // Set MapPoint vertices
    // Step 2.2：向优化器添加MapPoints顶点
    // 遍历地图中的所有地图点
    for(size_t i=0; i<vpMP.size(); i++)
    {
        MapPoint* pMP = vpMP[i];
        // 跳过无效地图点
        if(pMP->isBad())
            continue;

        // 创建顶点
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        // 注意由于地图点的位置是使用cv::Mat数据类型表示的,这里需要转换成为Eigen::Vector3d类型
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        // 前面记录maxKFid 是在这里使用的
        const int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        // 注意g2o在做BA的优化时必须将其所有地图点全部schur掉，否则会出错。
        // 原因是使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>这个类型来指定linearsolver,
        // 其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。
        // Ceres库则没有这样的限制
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

        // 取出地图点和关键帧之间观测的关系
        const map<KeyFrame*,size_t> observations = pMP->GetObservations();

        // 边计数
        int nEdges = 0;
        //SET EDGES
        // Step 3：向优化器添加投影边（是在遍历地图点、添加地图点的顶点的时候顺便添加的）
        // 遍历观察到当前地图点的所有关键帧
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
        {

            KeyFrame* pKF = mit->first;
            // 跳过不合法的关键帧
            if(pKF->isBad() || pKF->mnId>maxKFid)
                continue;

            nEdges++;
            // 取出该地图点对应该关键帧的2D特征点
            const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];

            if(pKF->mvuRight[mit->second]<0)
            {
                // 如果是单目相机按照下面操作
                // 构造观测
                Eigen::Matrix<double,2,1> obs;
                obs << kpUn.pt.x, kpUn.pt.y;

                // 创建边
                g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
                // 填充数据，构造约束关系
                // 第0个顶点对应的id 是地图点的id
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                // 第1个顶点对应的id是 关键帧的id
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                // 信息矩阵，也是协方差，表明了这个约束的观测在各个维度（x,y）上的可信程度，在我们这里对于具体的一个点，两个坐标的可信程度都是相同的，
                // 其可信程度受到特征点在图像金字塔中的图层有关，图层越高，可信度越差
                // 为了避免出现信息矩阵中元素为负数的情况，这里使用的是sigma^(-2)
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

                // 如果要使用鲁棒核函数
                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    // 这里的重投影误差，自由度为2，所以这里设置为卡方分布中自由度为2的阈值，如果重投影的误差大约大于1个像素的时候，就认为不太靠谱的点了，
                    // 核函数是为了避免其误差的平方项出现数值上过大的增长
                    rk->setDelta(thHuber2D);
                }

                // 设置相机内参
                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;
                // 添加边
                optimizer.addEdge(e);
            }
            else
            {
                // 双目或RGBD相机按照下面操作
                // 双目相机的观测数据则是由三个部分组成：投影点的x坐标，投影点的y坐标，以及投影点在右目中的x坐标（默认y方向上已经对齐了）
                Eigen::Matrix<double,3,1> obs;
                const float kp_ur = pKF->mvuRight[mit->second];
                obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                // 对于双目输入，g2o也有专门的误差边
                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(pKF->mnId)));
                e->setMeasurement(obs);
                // 信息矩阵这里是相同的，考虑的是左目特征点的所在图层
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                e->setInformation(Info);

                // 如果要使用鲁棒核函数
                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    // 由于现在的观测有三个值，重投影误差会有三个平方项的和组成，因此对应的卡方分布的自由度为3，所以这里设置的也是自由度为3的时候的阈值
                    rk->setDelta(thHuber3D);
                }

                // 填充相机的基本参数
                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;
                e->bf = pKF->mbf;

                optimizer.addEdge(e);
            }  
        } // 向优化器添加投影边,也就是遍历所有观测到当前地图点的关键帧

        // 如果因为一些特殊原因,实际上并没有任何关键帧观测到当前的这个地图点,那么就删除掉这个顶点,并且这个地图点也就不参与优化
        if(nEdges==0)
        {
            optimizer.removeVertex(vPoint);
            vbNotIncludedMP[i]=true;
        }
        else
        {
            vbNotIncludedMP[i]=false;
        }
    }

    // Optimize!
    // Step 4：开始优化
    optimizer.initializeOptimization();
    optimizer.optimize(nIterations);

    // Recover optimized data
    // Step 5：得到优化的结果

    // Step 5.1 遍历所有的关键帧
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;

        // 获取到优化后的位姿
        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        if(nLoopKF==0)
        {
            // 原则上来讲不会出现"当前闭环关键帧是第0帧"的情况,如果这种情况出现,只能够说明是在创建初始地图点的时候调用的这个全局BA函数.
            // 这个时候,地图中就只有两个关键帧,其中优化后的位姿数据可以直接写入到帧的成员变量中
            pKF->SetPose(Converter::toCvMat(SE3quat));
        }
        else
        {
            // 正常的操作,先把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用
            pKF->mTcwGBA.create(4,4,CV_32F);
            Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
            pKF->mnBAGlobalForKF = nLoopKF;
        }
    }

    // Step 5.2 Points
    // 遍历所有地图点,去除其中没有参与优化过程的地图点
    for(size_t i=0; i<vpMP.size(); i++)
    {
        if(vbNotIncludedMP[i])
            continue;

        MapPoint* pMP = vpMP[i];

        if(pMP->isBad())
            continue;

        // 获取优化之后的地图点的位置
        g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));

        // 和上面对关键帧的操作一样
        if(nLoopKF==0)  
        {
            // 如果这个GBA是在创建初始地图的时候调用的话,那么地图点的位姿也可以直接写入
            pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
            pMP->UpdateNormalAndDepth();
        }
        else
        {
            // 反之,如果是正常的闭环过程调用,就先临时保存一下
            pMP->mPosGBA.create(3,1,CV_32F);
            Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
            pMP->mnBAGlobalForKF = nLoopKF;
        }// 判断是因为什么原因调用的GBA
    } // 遍历所有地图点,保存优化之后地图点的位姿
}