ORB-SLAM2源码逐行解析系列（二十四）：ORB-SLAM2中的全局优化

最新推荐文章于 2024-04-06 00:45:00 发布
最新推荐文章于 2024-04-06 00:45:00 发布
阅读量178 收藏
点赞数
文章标签： 人工智能 计算机视觉 自动驾驶
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/LDST_CSDN/article/details/130672518
版权

1. BundleAdjustment

（1）顶点与边

​ 全局优化函数BundleAdjustment主要用于优化所有关键帧的位姿和地图点。因此，其顶点是待优化的所有关键帧的位姿和所有地图点。并且，以第0个关键帧位姿作为参考基准，不参与优化。顶点中关键帧位姿的类型为

g2o::VertexSE3Expmap，顶点中地图点的类型为g2o::VertexSBAPointXYZ。

​ 由于地图点和观测到它的关键帧存在投影约束关系，因此全局优化中的边是二元边。边的两个顶点分别是地图点坐标以及观测到它的关键帧的位姿。

​ 对于单目相机模式，边的类型为g2o::EdgeSE3ProjectXYZ。对于双目/RGB-D相机模式，边的类型为

g2o::EdgeStereoSE3ProjectXYZ。

（2）全局优化与局部优化的区别

  • 相同点：二者的顶点类型与边类型均相同、优化变量均为关键帧位姿与地图点、误差定义均为重投影误差。
  • 不同点：局部优化中使用的是局部关键帧以及局部地图点，全局优化中使用的是所有的关键帧和地图点。

（3）BundleAdjustment整体流程

  • 初始化g2o优化器。
  • 向优化器中添加顶点：所有的关键帧位姿和所有的地图点。
  • 向优化器中添加边：所有的地图点和观测到它的关键帧的投影关系，单目和双目模式下有所不同。
  • 开始优化，迭代10次。
  • 将优化后的结果保存起来，注意，没有直接原位替换更新。

（4）BundleAdjustment代码实现

/**
 * @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)
{
    // vbNotIncludedMP: 存储不参与优化的地图点
    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 向优化器添加顶点
    // 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
    
    // Step 2.2：向优化器添加地图点作为顶点
    // 遍历地图中的所有地图点
    for(size_t i=0; i<vpMP.size(); i++)
    {
        // 获取遍历到的当前地图点
        MapPoint* pMP = vpMP[i];
        // 跳过无效地图点
        if(pMP->isBad())
            continue;

        // 创建类型为g2o::VertexSBAPointXYZ的地图点顶点
        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;
        // 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个地图点
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>
                             									(optimizer.vertex(id)));
                // 边连接的第1号顶点对应的是第mnId个关键帧
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>		
                             							(optimizer.vertex(pKF->mnId)));
                // 设置观测
                e->setMeasurement(obs);
                
                // 信息矩阵，其可信程度与特征点在图像金字塔中的图层有关，图层越高，可信度越差
                // 为了避免出现信息矩阵中元素为负数的情况，这里使用的是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时的阈值
                    rk->setDelta(thHuber2D);
                }
                
                // 设置相机内参
                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;
                // 将边添加到优化器中
                optimizer.addEdge(e);
            }
            else  // 双目/RGB-D模式下
            {
                // 观测数据：投影点的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::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
                // 边连接的第0号顶点对应的是第id个地图点
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>
                             									(optimizer.vertex(id)));
                // 边连接的第1号顶点对应的是第mnId个关键帧
                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;
        }
    }
    
    // Step 4：开始优化
    optimizer.initializeOptimization();
    optimizer.optimize(nIterations);
    
    // Step 5：得到优化的结果
    // Step 5.1 遍历所有的关键帧
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;

        // 获取到当前关键帧pKF优化后的位姿
        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 遍历所有地图点,去除其中没有参与优化过程的地图点
    for(size_t i=0; i<vpMP.size(); i++)
    {
        // 没有参与优化的地图点就直接跳过
        if(vbNotIncludedMP[i])
            continue;
        MapPoint* pMP = vpMP[i];
        if(pMP->isBad())  // Bad的地图点也直接跳过
            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;
        }
    }
}
LDST_CSDN
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
SLAM：ORB-SLAM 位姿优化描述
alppkk4545的博客
08-05 658
只知道算法描述和代码，而不知道原理是比较扯的事情，还是把原理转载一下。 原文链接： http://www.cnblogs.com/luyb/p/5447497.html ORB-SLAM作为单目SLAM，其精度很大程度上决定于帧与帧之间的位姿优化的是否准确。因此优化（optimization）在OR...
ORB-SLAM2源码逐行解析系列（二十一）：ORB-SLAM2局部建图线程中的优化方法
LDST_CSDN的博客
10-23 190
局部建图优化中的边是二元边，它连接的需要优化的两个顶点分别是：_vertices[0]，表示待优化的局部关键帧的地图点；​ 由于局部关键帧所观测到的地图点也会被当前关键帧的二级共视关键帧观测到，即二级共视关键帧对这些地图点也会产生约束，因此顶点包括需要优化的当前帧的局部关键帧和对应的局部地图点，以及不参与优化的当前帧的二级共视关键帧（在优化时被固定住）。其中，局部关键帧是指当前关键帧的一级共视关键帧，局部地图点是指局部关键帧观测到的所有地图点。添加待优化的位姿顶点——局部关键帧的位姿。
最新方法GPO：快速、准确地进行单目SLAM初始化的全局平面优化
3D视觉工坊
08-02 567
点击上方“3D视觉工坊”，选择“星标”干货第一时间送达本文来自最新的一篇论文：GPO：Global Plane Optimization for Fast and Accurate Mo...
【SLAM学习笔记】8-ORB_SLAM3关键源码分析⑥ Optimizer（三）全局优化
qq_47891062的博客
11-14 955
2021SC@SDUSC
基于ORB-SLAM2的改进代码
weixin_39152543的博客
07-24 3837
基于ORB-SLAM2的改进代码汇总
开源SLAM框架学习——OpenVSLAM源码解析：全局优化模块（global optimization module）：回环检测、pose-graph优化、global-BA优化
一点儿也不萌的萌萌
11-04 2521
这篇博客主要介绍OpenVSLAM的全局优化模块（global_optimization_module），该模块是单独运行在一个线程中的。它主要执行的工作是：SLAM的回环检测，以及回环成功之后的回环矫正，还包括紧随着回环检测成功之后的pose graph优化和全局的BA优化。 1.全局优化模块入口 说是全局优化模块，其实也就是一个普通的类。对于类，第一步肯定就是实例化构造对象，全局优化模块的实例化过程是在system.cc代码中的system的构造函数处，对应的代码的如下： global_optimize
《ORB-SLAM2源码解析》学习手册v1.0-对外.pdf.pdf
03-08
ORB-SLAM2源码解析学习手册v1.0-对外.pdf.pdf ORB-SLAM2是基于视觉的SLAM（Simultaneous Localization and Mapping）算法的开源实现，能够实时生成高精度的地图和定位信息。在计算机视觉领域中，SLAM技术被广泛应用...
《ORB-SLAM2源码解析》学习手册v1.0-对外.pdf
07-08
《ORB-SLAM2源码解析》学习手册v1.0-对外.pdf
VI_ORB_SLAM2:基于ORB-SLAM2的MonocularStereo视觉惯性ORB-SLAM
05-14
VI_ORB_SLAM2：基于ORB-SLAM2的单目/立体视觉惯性ORB-SLAM 该存储库包括Visual-Inertial ORB-SLAM的Monocular版本和Stereo版本。 这两个是的和的。 有关详细信息，您可以参考Examples/Monocular/mono_euroc_VI.cc和...
ORB-SLAM2源码详解.zip
06-20
ORB-SLAM2是一款强大的视觉SLAM（Simultaneous Localization And Mapping，同时定位与建图）系统，由Carlos Hernández, Raúl Mur-Artal 和 José M. M. Montiel等人开发。它基于特征匹配的ORB（Oriented FAST and ...
激光SLAM后端优化方法
12-22
一种非常经典的SLAM后端优化方法，相比其他方法，该方法适合新手熟悉后端算法的流程。
ORB-SLAM2源码中文详解.pdf
08-17
东北大学吴博讲解ORB_SLAM的文档，仅有文档。
ICAR论文阅读总结-在单目视觉惯导SLAM中的重定位，全局优化以及联合地图
elecstar的博客
11-01 664
原英文名 提示：上面这个名字是我自己翻译的，原文名字我也给出来了。 原文名：Relocalization, Global Optimization and Map Merging for Monocular Visual-Inertial SLAM 文章目录原英文名前言一、Visual-inertial Odometry二、1.2.总结 前言 总结：本文提出一种单目视觉惯导SLAM系统在相机重定位、全局优化以及地图合并方面的创新： 相机与IMU的紧耦合：基于滑动窗口的非线性优化框架以紧耦合的方式处理视
SLAM编程：优化问题求解（2）_细谈求导
Ares_WangZiquan的博客
02-19 588
如何求导并形成可执行程序前言一、正常思路：目标/损失函数直接对优化变量求导1.实例：最小二乘位姿估计二、清奇思路：不直接求导1 追踪光流也能是优化问题？2.读入数据总结 前言 优化问题（可参考我的另一篇博文，《SLAM中的优化问题求解方法研讨》）中，如何求解导数是一个重要的子问题，其复杂的数学推导往往让人很难受。这里面不仅是求导本身的复杂性，更有的文章和技术对于更新量的求解并非来自于目标函数对待优化变量的直接求导。本文也会不断更新，试图以较为朴素的语言给出我对于求导的理解。 提示：以下是本篇文章正文内.
ORB_SLAM2 Optimizer
weixin_43828675的博客
03-11 1384
ORB_SLAM2使用g2o库进行优化。 （1）全局BA优化（GlobalBundleAdjustment）与局部BA优化（LocalBundleAdjustment） （2）位姿优化（只优化相机位姿，不优化路标点坐标）
BA与图优化-SLAM十四讲学习笔记
chuangkeiss的博客
10-29 1550
因此可以求误差函数关于二者的偏导数，也就是之前所述的两个偏导数，2*6的那个雅克比矩阵是误差对于扰动的小量位姿的偏导数；2*3的那个雅克比矩阵是误差对于特征点相机坐标系下坐标的偏导数，它再乘以一个3*3的R得到一个2*3的雅克比矩阵就是误差关于特征点世界坐标系下坐标的偏导数。到目前为止，还没有利用它的稀疏特性。由于我们把变量归类成了位姿和空间点两种，（注意E和F必须是整体目标函数对整体变量的导数，它是一个很大的矩阵，里面都是有每个误差项的导数。个路标点的误差，前面的是对于位姿的，后面的是对于路标点的。
2.ORB-SLAM2改进版本--稠密建图版本解析
qq_41694024的博客
02-09 1667
ORBSLAM2稠密建图版本的解释
SLAM ORB-SLAM2（25）相机位姿优化
最新发布
氢键H-H
04-06 1万+
在《SLAM ORB-SLAM2（23）运动模型跟踪》中的运动模型跟踪过程在根据匀速运动模型估计位姿后，利用用上一帧地图点进行投影匹配在《SLAM ORB-SLAM2（24）地图点投影匹配》找到了足够多的匹配点时就会调用根据匹配特征点构建优化器修正相机位姿/* 4. 利用3D-2D投影关系，优化当前帧位姿 */
ORB-SLAM（五）优化
weixin_30911809的博客
05-29 474
ORB-SLAM作为单目SLAM，其精度很大程度上决定于帧与帧之间的位姿优化的是否准确。因此优化（optimization）在ORB-SLAM里面扮演了很重要的角色。这一小节探讨一下ORB-SLAM里用到的优化。 ORB-SLAM选用g2o作为图优化的方法，关于g2o可以参考http://www.cnblogs.com/gaoxiang12/p/5304272.html。 一、为什么要优化 ...
ORB-SLAM2源码详解1
08-03
本篇文章由信息科学与工程学院人工智能与机器人研究所的吴博@泡泡机器人撰写，详细解析了ORB-SLAM2的源码结构和关键变量命名规则。 首先，我们关注变量命名规则，这是理解和阅读代码的关键。文章指出，作者遵循一定...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值