(01)ORB-SLAM2源码无死角解析-(62) BA优化(g2o)→追踪线程:Optimizer::PoseOptimization→仅位姿优化

g2o理解及实践代码报错解决方法   SparseOptimizer是最终要维护的东西,优化之前需要指定求解器和迭代算法。求解器Solver包含一个SparseBlockMatrix (用于计算稀疏的雅可比和海塞矩阵)以及一个线性方程求解器BlockSolver<>   可从PCG, CSparse, Choldmod 三者选一。 在g2o中选择优化方法一共需要三个步骤: 选择一个线性方程求解器,从 PCG,... 阅读详情

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/123092196
 
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
 

一、前言

在前面的博客中,有好些重要的函数到目前为止都还没有进行讲解如:

Optimizer::PoseOptimization();   //追踪线程仅位姿优化
Optimizer::LocalBundleAdjustment() //局部建图位姿与地图点优化
Optimizer::OptimizeSim3() //闭环线程Sim3变换优化
Optimizer::OptimizeEssentialGraph() //闭环线程本质图优化
Optimizer::GlobalBundleAdjustemnt() //闭环线程全局优化

针对于这些函数,接下来都会进行具体的分析,因为这里涉及到很多图优化的知识,如果不是很了解图优化朋友请先观看一下几篇博客:

史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)
史上最简SLAM零基础解读(10.2) - g2o(图优化)→顶点 (Vertex)编程细节
史上最简SLAM零基础解读(10.3) - g2o(图优化)→边(Edge)编程细节
史上最简SLAM零基础解读(10.4) - g2o(图优化)→示例代码讲解(slam十四讲第二版为例)

在对图优化有了基本的了解之后的,下面就开始吧,首先要讲解的是 Optimizer::PoseOptimization() 函数,其主要在追踪线程中被调用,如 src/tracking.cc 文件中的 Tracking::TrackWithMotionModel()、Tracking::TrackReferenceKeyFrame()、Tracking::Relocalization()、Tracking::TrackLocalMap() 中都使用了该函数。通过上面推荐的博客我们知道 g2o 的主体流程如下:

1、顶点和边的类型定义;
2、构建图优化实例,配置求解器;
3、添加点和边,构建求解图;
4、执行优化。

那么下面就是对 src/Optimizer.cc 中的 int Optimizer::PoseOptimization(Frame *pFrame) 函数,主要核心要点如下:

/*
 * @brief Pose Only Optimization
 * 
 * 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw)
 * 只优化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数量
 */

 

一、构建图优化实例

这里的流程还是比较固定的。主要确定二个东西:①顶点与路标点(地图点)维度→6x3;②优化方式→莱文伯格-马夸特方法;就可以构建图优化实例了,如下:

    // Step 1:构造g2o优化器, BlockSolver_6_3表示:位姿 _PoseDim 为6维,路标点 _LandmarkDim 是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);

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

 

二、顶点与边定义

1、顶点

顶点还是比较好理解的,需要优化的位姿,即代码当前帧的 Tcw,就是顶点,有且仅有该位姿需要优化。

    // Set Frame vertex
    // Step 2:添加顶点:待优化当前帧的Tcw
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
     // 设置id
    vSE3->setId(0);    
    // 要优化的变量,所以不能固定
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);

因为顶点是这里需要优化的量,所以执行vSE3->setFixed(false),不能固定。宁外 注意一下,这里使用 g2o::VertexSE3Expmap 类型的顶点李代数的相机位姿。,继承自 BaseVertex:

class  VertexSE3Expmap : public BaseVertex<6, SE3Quat> {...}

参数6 :SE3Quat类型为六维,三维旋转,三维平移。
参数SE3Quat :该类型旋转在前,平移在后,注意:类型内部使用的其实是四元数,不是李代数。
 

二、边

先对来对,边要复杂一些,在源码中使用了两种边,分别为EdgeSE3ProjectXYZOnlyPose(单目),EdgeStereoSE3ProjectXYZOnlyPose(双目)。这里只对 EdgeSE3ProjectXYZOnlyPose 进行讲解,EdgeStereoSE3ProjectXYZOnlyPose进行类推即可。位于Thirdparty/g2o/g2o/types/types_six_dof_expmap.中定义,首先其继承关系如下:

class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{......}

可以很明显看出继承于 BaseUnaryEdge,BaseUnaryEdge有三个模参数:
参数2:因为出来的误差是2维的,误差 = 关键点像素坐标(观测)- 地图点投影到关键点附近像素坐标(投影)。
Vector2d:表示误差值的类型。
VertexSE3Expmap:顶点类型为VertexSE3Expmap,也就是前面需要优化的顶点类型。

同时,通过前面关于 g2o(图优化) 的文章可以了解到,关于边的定义,只需要实现两个两个重要的函数即可,分别为:

virtual void computeError();
virtual void linearizeOplus();

先来看看 computeError(),其实呢,再 史上最简SLAM零基础解读(10.3) - g2o(图优化)→边(Edge)编程细节 中已经有比较详细的讲解。计算方式为: 重投影误差=图像2d观测值 - 3d点经估计的相机位姿和相机内参K投影到图像平面的2d点值。也就是对应源码如下:

  void computeError()  {
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
    Vector2d obs(_measurement);
    _error = obs-cam_project(v1->estimate().map(Xw));
  }

其上的 Xw 表示地图点对应的世界坐标,v1是前面设置的顶点(当前帧位姿), 通过v1->estimate().map(Xw)→等价于_r*Xw+ _t。_r 和 _t 分别表示当前帧的旋转与平移。也就是说把地图点 Xw 变换到当前帧的相机坐标系下。然后再使用 cam_project() 函数投影到当前帧的像素坐标。最终与obs(地图点再当前帧对应特征点的像素坐标)作差,即获得误差。

那么再来看看 linearizeOplus() 函数,其实现于 Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp 文件中,代码如下:

  void EdgeSE3ProjectXYZOnlyPose:: linearizeOplus() {
  VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
  Vector3d xyz_trans = vi->estimate().map(Xw);

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double invz = 1.0/xyz_trans[2];
  double invz_2 = invz*invz;

  _jacobianOplusXi(0,0) =  x*y*invz_2 *fx;
  _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
  _jacobianOplusXi(0,2) = y*invz *fx;
  _jacobianOplusXi(0,3) = -invz *fx;
  _jacobianOplusXi(0,4) = 0;
  _jacobianOplusXi(0,5) = x*invz_2 *fx;

  _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
  _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
  _jacobianOplusXi(1,2) = -x*invz *fy;
  _jacobianOplusXi(1,3) = 0;
  _jacobianOplusXi(1,4) = -invz *fy;
  _jacobianOplusXi(1,5) = y*invz_2 *fy;
}

这里就不进行推导了,有兴趣的朋友请,推荐大家阅读一下该博客:史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解(Bundle Adjustment)。其有具体的推导过程,另外这里需要注意一个点,根据博客得到雅克比矩阵(45式)与源码是不对应的 ∂ e ∂ Δ ξ = [ f x Z c 0 − f x X c Z c − f x X c Y c Z c 2 f x + f x X c 2 Z c 2 − f x Y c Z c 0 f y Z c − f y Y c Z c 2 − f y − f y Y c 2 Z c 2 f y X c Y c Z c 2 f y X c Z c ] (01) \color{green} \tag{01} \frac{\partial e}{\partial \Delta \xi} = \left[\begin{array}{cccccc} \frac{f_{x}}{Z_{c}} & 0 & -\frac{f_{x} X_{c}}{Z_{c}} & -\frac{f_{x} X_{c} Y_{c}}{Z_{c}^{2}} & f_{x}+\frac{f_{x} X_{c}^{2}}{Z_{c}^{2}} & -\frac{f_{x} Y_{c}}{Z_{c}} \\ \\ 0 & \frac{f_{y}}{Z_{c}} & -\frac{f_{y} Y_{c}}{Z_{c}^{2}} & -f_{y}-\frac{f_{y} Y_{c}^{2}}{Z_{c}^{2}} & \frac{f_{y} X_{c} Y_{c}}{Z_{c}^{2}} & \frac{f_{y} X_{c}}{Z_{c}} \end{array}\right] ∂Δξ∂e​=⎣ ⎡​Zc​fx​​0​0Zc​fy​​​−Zc​fx​Xc​​−Zc2​fy​Yc​​​−Zc2​fx​Xc​Yc​​−fy​−Zc2​fy​Yc2​​​fx​+Zc2​fx​Xc2​​Zc2​fy​Xc​Yc​​​−Zc​fx​Yc​​Zc​fy​Xc​​​⎦ ⎤​(01) 这是因为 se(3) 的定义方式是旋转在前,平移在后时,只要把这个矩阵的前三列与后三列对调即可。
 

三、整体逻辑讲解

了解了顶点与边的定义之后,我们就来看看 Optimizer::PoseOptimization(Frame *pFrame) 的整体流程是如何的:

准备阶段

( 01 ) : \color{blue} (01): (01): 构造g2o优化器, BlockSolver_6_3表示:位姿 _PoseDim 为6维,路标点 _LandmarkDim 是3维

( 02 ) : \color{blue} (02): (02): 添加顶点:待优化当前帧的Tcw

( 03 ) : \color{blue} (03): (03): 循环添加一元边,因为所有边都只和一个顶点连接。

( 04 ) : \color{blue} (04): (04):循环遍历当前帧的所有有效地图点,然后分单目情况与双目情况进行处理,同时把地图点对应的特征点标记为内点, 对应代码 pFrame->mvbOutlier[i] = false;

( 05 ) : \color{blue} (05): (05):如果为单目(双目省略讲解),创建一元边EdgeSE3ProjectXYZOnlyPose实例。设置顶点(当前帧位姿)、设置观测点(当前帧与地图点对应的特征点)。

( 06 ) : \color{blue} (06): (06): 设置置性度(信息矩阵→与特征点所处金字塔层级相关):现在来考虑另一种情况,比方说在一次优化中,对于某一次测量,我们有十足的把握,它非常的准确,所以优化时我们希望对于这次测量给予更高的权重。对应带代码 e->setInformation()

( 07 ) : \color{blue} (07): (07): 设置鲁棒核函数,g2o中提供了鲁棒核函数来抑制某些误差特别大的点,避免拉偏整个优化结果。(鲁棒核函数不是g2o独有的,这是非线性优化方法中的一种常用手段)

( 08 ) : \color{blue} (08): (08): 设置卡法阈值(用来判断内外点)、设置相机内参、地图点的空间位置。

( 09 ) : \color{blue} (09): (09):设置好的所有一元边之后(一个特征点对应一个一元边),则添加到 vpEdgesStereo.reserve(N) 与 vnIndexEdgeStereo.reserve(N) 之中。

优化阶段

( 10 ) : \color{blue} (10): (10): 共进行四次优化,其中每次优化迭代10次(optimizer.optimize(its[it])),优化等级为0,也就是说不对外点进行优化,外点等级为1。

( 11 ) : \color{blue} (11): (11):每次优化结束之后,开始遍历每一条误差边,如果这条误差边是来自于outlier,则计算误差。如果一元边误差较大,设置为外点,误差较小设置为内点。

( 12 ) : \color{blue} (12): (12): 获取优化后当前帧的位姿,返回内点数目。

代码注释

代码位于 src/Optimizer.cc 文件中:

/*
 * @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)
{
    // 该优化函数主要用于Tracking线程中:运动跟踪、参考帧跟踪、地图跟踪、重定位

    // Step 1:构造g2o优化器, BlockSolver_6_3表示:位姿 _PoseDim 为6维,路标点 _LandmarkDim 是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);

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

    // 输入的帧中,有效的,参与优化过程的2D-3D点对
    int nInitialCorrespondences=0;

    // Set Frame vertex
    // Step 2:添加顶点:待优化当前帧的Tcw
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
     // 设置id
    vSE3->setId(0);    
    // 要优化的变量,所以不能固定
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);

    // Set MapPoint vertices
    const int N = pFrame->N;

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

    // for Stereo
    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)
        {
            // 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)));
                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;// 这里和单目不同
                // 新建边,一元边,误差为观测特征点坐标减去投影点的坐标
                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);
            } // 根据单目和双目不同的相机输入执行不同的操作过程
        }

    }
    } // 离开临界区

    // 如果没有足够的匹配点,那么就只好放弃了
    if(nInitialCorrespondences<3)
        return 0;

    // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
    // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
    // Step 4:开始优化,总共优化四次,每次优化迭代10次,每次优化后,将观测分为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};// 四次迭代,每次迭代的次数

    // 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++)
        {
            g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

            const size_t idx = vnIndexEdgeMono[i];

            // 如果这条误差边是来自于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以外, 其余的优化都不需要 -- 因为重投影的误差已经有明显的下降了
        } // 对单目误差边的处理
        // 同样的原理遍历双目的误差边
        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();
            }

            const float chi2 = e->chi2();

            if(chi2>chi2Stereo[it])
            {
                pFrame->mvbOutlier[idx]=true;
                e->setLevel(1);
                nBad++;
            }
            else
            {                
                e->setLevel(0);
                pFrame->mvbOutlier[idx]=false;
            }

            if(it==2)
                e->setRobustKernel(0);
        } // 对双目误差边的处理

        if(optimizer.edges().size()<10)
            break;
    } // 一共要进行四次优化

    // Recover optimized pose and return number of inliers
    // 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);

    // 并且返回内点数目
    return nInitialCorrespondences-nBad;
}

 

四、前言

通过该篇博客,对Optimizer::PoseOptimization→仅位姿优化进行了详细讲解,相信对于g2o的使用大家都更加熟悉了。下面针对于其他优化,理解起来会简单很多,那么下篇博客见。

 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件

ORB-SLAM2源码逐行解析系列(二十):ORB-SLAM2跟踪线程中的优化方法 v1->estimate()表示顶点的估计值,在优化过程中不断更新,参与到重投影误差的计算当中。​ 因此,顶点类型是g2o::VertexSE3Expmap.对于单目相机,边的类型为g2o::EdgeSE3ProjectXYZOnlyPose,对于双目/RGB-D相机,边的类型为g2o::EdgeStereoSE3ProjectXYZOnlyPose。在跟踪线程中,PoseOptimization主要是获取3D-2D的最小化重投影误差,在优化过程中只对当前关键帧的位姿进行优化,不优化地图点的坐标。 阅读详情

相关推荐

【逐函数详细讲解ORB_SLAM2算法和C++代码|Optimizer|1-21】

类在ORB-SLAM2系统中负责各种优化任务,以提高位姿估计、地图点位置和地图一致性的精度。这些优化方法使用了g2o库(一个通用的优化库,用于解决图优化问题)来执行优化。优化是关键任务之一,因为它们用于提高位姿估计、地图点位置和地图一致性的精度。

小秋 AI SLAM 入门实战教程 450

ORB-SLAM2 特征点法SLAM 单目 双目 rgbd相机SLAM 单应/本质矩阵恢复运动 小图大图地图优化

ORB-SLAM2 ORB特征点法SLAM 支持单目、双目、rgbd相机 安装测试 本文github链接 orbslam2 + imu ORB-SLAM是一个基于特征点的实时单目SLAM系统,在大规模的、小规模的、室内室外的环境都可以运行。 该系统对剧烈运动也很鲁棒,支持宽基线的闭环检测和重定位,包括全自动初始化。 该系统包含了所有SLAM系统共有的模块: 跟踪(Tracking)...

万有文的博客 1万+

【ORB-SLAM2源码解读】论文公式

文章目录ORB_SLAM2论文翻译robust Huber cost function协方差矩阵Spanning Graph Covisibility Graph Essential Graph非线性优化 ORB_SLAM2论文翻译 ORB-SLAM2 论文全文翻译 robust Huber cost function 在统计学习角度,Huber损失函数是一种使用鲁棒性回归的损失函数,它相比均方误差来说,它对异常值不敏感。常常被用于分类问题上。 这个函数对于小的a值误差函数是二次的,而对大的值误差函数是线

小秋 AI SLAM 入门实战教程 1307

ORB-SLAM2系统中的优化函数

SLAM系统中,一般在特征点投影完成一次的时候就要进行一次优化,优化是为了将匹配的误差最小化,进而剔除一些outlier点。ORB-SLAM系统当中的优化分为下面5个,从优化的范围层次逐步扩大。 1.位姿优化函数PoseOptimization 一些说明:PoseOptimization优化的是单帧图像的位姿。相机在运动的时候每个图像帧可以对应三维世界中多个地图点,那么此时我们可以知道三维世界...

2934

tmux + Claude Code 远程开发指南:从 WSL 配置到断线无忧

远程开发中,SSH 连接中断常导致后台任务丢失,这是困扰开发者的经典难题。tmux 作为终端复用器,通过将进程挂在常驻会话中,使其脱离 SSH 连接独立运行,从根本上解决了断线导致的任务终止问题。Claude Code 等 AI 编程代理需要长时间运行并保持交互上下文,恰好依赖这种会话持久化能力。结合 Windows Subsystem for Linux 提供类 Unix 环境,开发者可在 Windows 上实现稳定的远程开发工作流。本文将完整介绍从 WSL 环境配置、tmux 会话管理,到与 Claud

weixin_30423065的博客 349

告别‘抓瞎’:CANoe的Trace、Graphics和Logging窗口实战技巧,高效分析总线故障

本文深入解析CANoe的Trace、Graphics和Logging窗口实战技巧,帮助工程师高效分析总线故障。通过智能过滤、可视化监控和智能日志管理,构建系统级问题定位工作流,显著提升车载网络故障排查效率。

weixin_42709044的博客 297

ORBSLAM3的g2o优化之位姿图优化(PoseOptimization)

目录

Walking_roll的博客 3915

代码实现ROS2bag录制数据和回放数据(C++)

ROS2的ROS2bag功能可以将指定Topic中的数据以.db3的格式(不同于ROS)保存到本地文件中。读取文件数据可以实现数据回放。

m0_57124501的博客 3260

认真看代码ORB_SLAM3===2 优化器

ORB-SLAM系统中的优化框架

t_home的博客 751

【智能座舱】- 智能座舱研究报告合集

智能座舱研究报告合集

零点壹客的博客 1307

【FPGA教程案例59】深度学习案例6——基于FPGA的CNN卷积神经网络之整体实现

在课程,我们对的基本原理有了初步认识。在上一个课程,我们完成了CNN的Relu激活层、pool池化层、卷积层以及图像输入层的实现,如下图所示:在本课程中,我们将上述四个模块结合起来,构建一个简单的CNN卷积神经网络。...

FPGA/MATLAB学习教程/源码/项目合作开发 2639

ORB-SLAM3 从理论到代码实现(一):Optimizer 单帧优化

本文详细分析了ORB-SLAM3中的优化器(Optimizer)模块,重点介绍了单帧位姿优化(PoseOptimization)和融合IMU信息的位姿优化(PoseInertialOptimization)两个核心函数。单帧优化通过g2o构建图优化问题,包含SE3位姿顶点和重投影误差边,采用卡方检验剔除外点。IMU融合优化则增加了速度、陀螺仪和加速度计偏置作为优化变量,并引入IMU预积分约束、随机游走约束等多种边类型。文章深入解析了优化器的初始化、顶点和边的设置、多轮优化迭代过程、外点剔除策略,以及优化结果

人无远虑,必有近忧 1801

【史上最全面esp32教程】oled显示篇

esp32-arduino Oled屏幕的使用

m0_62599305的博客 1万+

【SLAM】G2O优化库超详细解析

通用图优化。为何叫通用呢?G2O的核里带有各种各样的求解器,而它的顶点、边的类型则多种多样。通过自定义顶点和边,事实上,只要一个优化问题能够表达成图,那么就可以用G2O去求解它。常见的,比如bundle adjustment、ICP、数据拟合,都可以用G2O来做。所谓图优化,是把优化问题表现成图的一种方式,这里的图是图论意义上的图。一个图由若干个顶点,以及连着这些顶点的边组成。在这里,我们用顶点表示优化变量,而用边表示残差项。

Yngz_Miao的博客 1万+

工业预测性维护实战:振动分析+Transformer预警系统构建指南

摘要:本文聚焦工业设备预测性维护难题,提出基于振动分析与Transformer模型的全栈解决方案。通过低成本硬件选型(单传感器¥380)实现设备振动信号实时采集,利用边缘计算单元完成特征工程(RMS、峰度、峰值频率提取),并在云端部署Transformer时序模型进行异常预警。某风电龙头企业实践显示,系统将故障误报率从38%降至6%,维护成本降低58%,非计划停机时长减少87%。文中提供完整的边缘端特征提取代码、云端模型架构及实施流程,适配ISO 13373振动分析标准,为制造业设备健康管理提供可复用的工程

专注于人工智能、软件开发、工控自动化、工厂数字化及智能化等领域,希望和大家共同进步! 632

【使用DeepSeek辅助Python接口性能自动化测试教程-实战教程】

访问DeepSeek官方网站,注册账号并获取API Key。此Key是调用DeepSeek API的必要凭证,务必妥善保管。安装时,注意勾选“Add Python to PATH”选项,以便系统能识别Python命令。确保你已安装Python 3.8或更高版本。会自动发现并执行所有以。

Dreams°的博客 2471

四、跟踪线程中的当前帧位姿优化Optimizer::PoseOptimization()

待更新

YY的博客 453

IEM模型,地表粗糙度模拟

利用微波反演土壤水分时候,利用IEM模型可以实现对随机地表的模拟

SLAM--单目尺度漂移(相似变换群Sim3)

参考文献 1.Horn, Berthold, K, et al. Closed-form solution of absolute orientation using unit quaternions[J]. Journal of the Optical Society of America A, 1987. 2.Strasdat H . Local Accuracy and Global Consistency for Efficient SLAM[J]. Imperial College London,

老邹的博客 2276

数学建模模型总结

这是本人在两年内准备参加数学建模竞赛搜集到的模型资料,已经经过了本人的筛选和整理,希望能够对准备参加数学建模竞赛的学弟学妹们提供一些帮助!

深度学习工具包 DeepLearningToolbox

深度学习工具包Deprecation notice.-----This toolbox is outdated and no longer maintained.There are much better tools available for deep learning than this toolbox, e.g. [Theano](http://deeplearning.net/software/theano/), [torch](http://torch.ch/) or [tensorflow](http://www.tensorflow.org/)I would suggest you use one of the tools mentioned above rather than use this toolbox.Best, Rasmus.DeepLearnToolbox================A Matlab toolbox for Deep Learning.Deep Learning is a new subfield of machine learning that focuses on learning deep hierarchical models of data.It is inspired by the human brain's apparent deep (layered, hierarchical) architecture.A good overview of the theory of Deep Learning theory is[Learning Deep Architectures for AI](http://www.iro.umontreal.ca/~bengioy/papers/ftml_book.pdf)For a more informal introduction, see the following videos by Geoffrey Hinton and Andrew Ng.* [The Next Generation of Neural Networks](http://www.youtube.com/watch?v=AyzOUbkUf3M) (Hinton, 2007)* [Recent Developments in Deep Learning](http://www.youtube.com/watch?v=VdIURAu1-aU) (Hinton, 2010)* [Unsupervised Feature Learning and Deep Learning](http://www.youtube.com/watch?v=ZmNOAtZIgIk) (Ng, 2011)If you use this toolbox in your research please cite [Prediction as a candidate for learning deep hierarchical models of data](http://www2.imm.dtu.dk/pubdb/views/publication_details.php?id=6284)```@MASTERSTHESIS\{IMM2012-06284, author = "R. B. Palm", title = "Prediction as a candidate for learning deep hierarchical models of data", year = "2012",}```Contact: rasmusbergpalm at gmail dot comDirectories included in the toolbox-----------------------------------`NN/` - A library for Feedforward Backpropagation Neural Networks`CNN/` - A library for Convolutional Neural Networks`DBN/` - A library for Deep Belief Networks`SAE/` - A library for Stacked Auto-Encoders`CAE/` - A library for Convolutional Auto-Encoders`util/` - Utility functions used by the libraries`data/` - Data used by the examples`tests/` - unit tests to verify toolbox is workingFor references on each library check REFS.mdSetup-----1. Download.2. addpath(genpath('DeepLearnToolbox'));Example: Deep Belief Network---------------------```matlabfunction test_example_DBNload mnist_uint8;train_x = double(train_x) / 255;test_x = double(test_x) / 255;train_y = double(train_y);test_y = double(test_y);%% ex1 train a 100 hidden unit RBM and visualize its weightsrand('state',0)dbn.sizes = [100];opts.numepochs = 1;opts.batchsize = 100;opts.momentum = 0;opts.alpha = 1;dbn = dbnsetup(dbn, train_x, opts);dbn = dbntrain(dbn, train_x, opts);figure; visualize(dbn.rbm{1}.W'); % Visualize the RBM weights%% ex2 train a 100-100 hidden unit DBN and use its weights to initialize a NNrand('state',0)%train dbndbn.sizes = [100 100];opts.numepochs = 1;opts.batchsize = 100;opts.momentum = 0;opts.alpha = 1;dbn = dbnsetup(dbn, train_x, opts);dbn = dbntrain(dbn, train_x, opts);%unfold dbn to nnnn = dbnunfoldtonn(dbn, 10);nn.activation_function = 'sigm';%train nnopts.numepochs = 1;opts.batchsize = 100;nn = nntrain(nn, train_x, train_y, opts);[er, bad] = nntest(nn, test_x, test_y);assert(er < 0.10, 'Too big error');```Example: Stacked Auto-Encoders---------------------```matlabfunction test_example_SAEload mnist_uint8;train_x = double(train_x)/255;test_x = double(test_x)/255;train_y = double(train_y);test_y = double(test_y);%% ex1 train a 100 hidden unit SDAE and use it to initialize a FFNN% Setup and train a stacked denoising autoencoder (SDAE)rand('state',0)sae = saesetup([784 100]);sae.ae{1}.activation_function = 'sigm';sae.ae{1}.learningRate = 1;sae.ae{1}.inputZeroMaskedFraction = 0.5;opts.numepochs = 1;opts.batchsize = 100;sae = saetrain(sae, train_x, opts);visualize(sae.ae{1}.W{1}(:,2:end)')% Use the SDAE to initialize a FFNNnn = nnsetup([784 100 10]);nn.activation_function = 'sigm';nn.learningRate = 1;nn.W{1} = sae.ae{1}.W{1};% Train the FFNNopts.numepochs = 1;opts.batchsize = 100;nn = nntrain(nn, train_x, train_y, opts);[er, bad] = nntest(nn, test_x, test_y);assert(er < 0.16, 'Too big error');```Example: Convolutional Neural Nets---------------------```matlabfunction test_example_CNNload mnist_uint8;train_x = double(reshape(train_x',28,28,60000))/255;test_x = double(reshape(test_x',28,28,10000))/255;train_y = double(train_y');test_y = double(test_y');%% ex1 Train a 6c-2s-12c-2s Convolutional neural network %will run 1 epoch in about 200 second and get around 11% error. %With 100 epochs you'll get around 1.2% errorrand('state',0)cnn.layers = { struct('type', 'i') %input layer struct('type', 'c', 'outputmaps', 6, 'kernelsize', 5) %convolution layer struct('type', 's', 'scale', 2) %sub sampling layer struct('type', 'c', 'outputmaps', 12, 'kernelsize', 5) %convolution layer struct('type', 's', 'scale', 2) %subsampling layer};cnn = cnnsetup(cnn, train_x, train_y);opts.alpha = 1;opts.batchsize = 50;opts.numepochs = 1;cnn = cnntrain(cnn, train_x, train_y, opts);[er, bad] = cnntest(cnn, test_x, test_y);%plot mean squared errorfigure; plot(cnn.rL);assert(er<0.12, 'Too big error');```Example: Neural Networks---------------------```matlabfunction test_example_NNload mnist_uint8;train_x = double(train_x) / 255;test_x = double(test_x) / 255;train_y = double(train_y);test_y = double(test_y);% normalize[train_x, mu, sigma] = zscore(train_x);test_x = normalize(test_x, mu, sigma);%% ex1 vanilla neural netrand('state',0)nn = nnsetup([784 100 10]);opts.numepochs = 1; % Number of full sweeps through dataopts.batchsize = 100; % Take a mean gradient step over this many samples[nn, L] = nntrain(nn, train_x, train_y, opts);[er, bad] = nntest(nn, test_x, test_y);assert(er < 0.08, 'Too big error');%% ex2 neural net with L2 weight decayrand('state',0)nn = nnsetup([784 100 10]);nn.weightPenaltyL2 = 1e-4; % L2 weight decayopts.numepochs = 1; % Number of full sweeps through dataopts.batchsize = 100; % Take a mean gradient step over this many samplesnn = nntrain(nn, train_x, train_y, opts);[er, bad] = nntest(nn, test_x, test_y);assert(er < 0.1, 'Too big error');%% ex3 neural net with dropoutrand('state',0)nn = nnsetup([784 100 10]);nn.dropoutFraction = 0.5; % Dropout fraction opts.numepochs = 1; % Number of full sweeps through dataopts.batchsize = 100; % Take a mean gradient step over this many samplesnn = nntrain(nn, train_x, train_y, opts);[er, bad] = nntest(nn, test_x, test_y);assert(er < 0.1, 'Too big error');%% ex4 neural net with sigmoid activation functionrand('state',0)nn = nnsetup([784 100 10]);nn.activation_function = 'sigm'; % Sigmoid activation functionnn.learningRate = 1; % Sigm require a lower learning rateopts.numepochs = 1; % Number of full sweeps through dataopts.batchsize = 100; % Take a mean gradient step over this many samplesnn = nntrain(nn, train_x, train_y, opts);[er, bad] = nntest(nn, test_x, test_y);assert(er < 0.1, 'Too big error');%% ex5 plotting functionalityrand('state',0)nn = nnsetup([784 20 10]);opts.numepochs = 5; % Number of full sweeps through datann.output = 'softmax'; % use softmax outputopts.batchsize = 1000; % Take a mean gradient step over this many samplesopts.plot = 1; % enable plottingnn = nntrain(nn, train_x, train_y, opts);[er, bad] = nntest(nn, test_x, test_y);assert(er < 0.1, 'Too big error');%% ex6 neural net with sigmoid activation and plotting of validation and training error% split training data into training and validation datavx = train_x(1:10000,:);tx = train_x(10001:end,:);vy = train_y(1:10000,:);ty = train_y(10001:end,:);rand('state',0)nn = nnsetup([784 20 10]); nn.output = 'softmax'; % use softmax outputopts.numepochs = 5; % Number of full sweeps through dataopts.batchsize = 1000; % Take a mean gradient step over this many samplesopts.plot = 1; % enable plottingnn = nntrain(nn, tx, ty, opts, vx, vy); % nntrain takes validation set as last two arguments (optionally)[er, bad] = nntest(nn, test_x, test_y);assert(er < 0.1, 'Too big error');```[![Bitdeli Badge](https://d2weczhvl823v0.cloudfront.net/rasmusbergpalm/deeplearntoolbox/trend.png)](https://bitdeli.com/free "Bitdeli Badge")

orb-slam2 从单目开始的简单学习(7):Optimizer

orb-slam2 从单目开始的简单学习(7):Optimizer

weixin_50862344的博客 737

假人有限元模型

假人模型,车辆碰撞假人有限元模型,适合做乘员安全的研究者利用

(01)ORB-SLAM2源码无死角解析-(63) BA优化(g2o)→局部建图线程:Optimizer::LocalBundleAdjustment→位姿与地图点优化

在上一篇博客中,为大家讲解了 Optimizer::PoseOptimization→仅位姿优化,同时还分享了几篇g2o(图优化)博客,这篇博客讲 Optimizer::LocalBundleAdjustment,其在局部建图线程中被调,即 src/LocalMapping.cc 文件中 LocalMapping::Run() 函数。首先需要明白,优化的目标是→局部关键帧的位姿与地图点。

江南才尽江南山,年少无知年少狂! 1224

[学习笔记]实时SLAM的未来及深度学习与SLAM对比

实时SLAM的未来及深度学习与SLAM对比 The Future of Real-Time SLAM and Deep Learning vs SLAM学习笔记 作者:家辉, 日期:2016-07-10 CSDN博客: http://blog.csdn.net/gobitan/   本文是作者(Tombone)对2015年12月18日计算机视觉国际会议实时SLAM小组讨论要点的总结。原文发...

家辉自留地 1万+

The Future of Real-Time SLAM and "Deep Learning vs SLAM" 深度学习与slam

Tombone's Computer Vision Blog Deep Learning, Computer Vision, and the algorithms that are shaping the future of Artificial Intelligence. http://www.computervisionblog.com/2016/01/why-

klzhang的专栏 4310

传统ORB-SLam中位姿优化中雅克比矩阵讲解

由于之前的鱼眼orbslam只有单目部分,所以在优化时也只是用了单目位姿优化和三维坐标点优化,并没有将双目的优化添加进去,不知道是否对结果有影响; 这里添加双目的优化部分,主要是将添加雅克比矩阵; orbslam中的优化部分使用了g2o库,具体的详细讲解可以参考https://zhuanlan.zhihu.com/p/58521241 讲解的很详细 这里不加赘述,主要从单目的雅克比矩阵讲解,...

2764

ORB-SLAM2论文翻译

个人博客:http://www.chenjianqu.com/ 原文链接:http://www.chenjianqu.com/show-105.html 论文:Raul Mur-Artal,Juan D. Tard ´os´.ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras.2017 ...

陈建驱的博客 2474

slam优化库,优化方法,G2o Ceres的学习

优化库G2o Ceres的学习。

A172494的博客 2494

SLAM - 后端 - 1

g2o图优化法: g2o_bal_class.h #include <Eigen/Core> #include "g2o/core/base_vertex.h" #include "g2o/core/base_binary_edge.h" #include "ceres/autodiff.h" #include "tools/rotation.h" #include "common/projection.h" //g2o自身顶点类型: //VertexSE2 : public Base

Breathmetoyou的博客 448
上一篇: (01)ORB-SLAM2源码无死角解析-(61) 闭环线程→闭环矫正: CorrectLoop→全代码注释
下一篇: 史上最简SLAM零基础解读(10.2) - g2o(图优化)→顶点 (Vertex)编程细节
江南才尽,年少无知!
江南才尽,年少无知! 领域专家: 人工智能技术领域 领域专家: 人工智能技术领域
博客等级 码龄8年 4万+粉丝 519原创
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明:

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

余额充值
⌂ 换网址