inertial部分的对比

ORB_SLAM3 VINS
IMU的偏差 同时考虑了加速度计和陀螺仪的偏差，并且将其放在目标函数中与重力方向，尺度因子等待优化变量共同优化，在inertialoptimization函数 只估计了陀螺仪的偏差，并且是将其考虑成非线性的，将其独立出来单独优化，没有采用g2o求解器，而是直接一次高斯牛顿迭代解决，在initial_aligment.cpp中的solveGyroscopeBias中。
速度，重力，尺度因子 与IMU偏差共同估计，通过最大后验估计并取负对数将其转换成包含IMU先验残差的非线性最小二乘问题，利用g2o去求解inertial-only map下的因子图，速度，尺度与重力方向都是顶点 initial_alignment.cpp中的linearalignment(),首先根据预积分量的约束，将世界坐标系转换成相机在t0时刻的坐标系，转换成线性最小二乘问题ceres求解
IMU的优化内容 inertialoptimization中默认各个关键帧对应的IMU的pose是一致的，只优化了IMU的速度，在初始化成功后的5秒和15秒又再次初始化。 状态估计器中的Estimator::visualInitialAlign()，计算陀螺仪的偏置校准(加速度偏置没有处理)，同时更新了Bgs后，IMU repropagate；得到尺度s和重力g的方向后，转换到第一帧坐标系，然后转换到世界坐标系
重力g的优化 代码中在g与速度等待优化变量的目标函数求解后直接对齐，得出结果，没有看到更多的判断 在linearalignment中RefineGravity进一步细化了重力，在其切线空间上用两个变量重新参数化重力，迭代四次，对状态向量重新优化。
惯性系的对齐 在IMU的优化过程中目标函数求解过程同时解算出了优化后的rotation matrix 计算出重力在第一个关键帧的测量值后对重力进行参数化，限制模长，判断误差是否大于百分之五，然后重力细化，进行坐标系的对齐。
scale的优化 在初始化及其之后的100秒之内不断优化尺度和重力方向 只在初始化的过程进行优化
求解形式 SO(3) 四元数

scale refinement

在localmapping中，如果慢速运动无法提供对IMU参数的充分观测的话，初始化在最初的15秒内并不能收敛到准确解，在这里作者提出了新颖的尺度优化方案，参见作者paper中的tracking and mapping部分，该部分基于改进后的inertial-only optimization，只估计所有关键帧中的尺度以及重力的方向，该部分优化每10秒在localmapping线程中运行一次，对应代码如下

// scale refinement
// Step 9.3 尺度优化
if (((mpAtlas->KeyFramesInMap())<=100) &&
    ((mTinit>25.0f && mTinit<25.5f)||
     (mTinit>35.0f && mTinit<35.5f)||
     (mTinit>45.0f && mTinit<45.5f)||
     (mTinit>55.0f && mTinit<55.5f)||
     (mTinit>65.0f && mTinit<65.5f)||
     (mTinit>75.0f && mTinit<75.5f))){
    cout << "start scale ref" << endl;
    if (mbMonocular)
        ScaleRefinement();
    cout << "end scale ref" << endl;
}

这里对应的inertial-optimization为论文中figure2d对应的因子图，代码为

void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale)

可以看出该函数的参数为地图点、重力的方向矩阵以及尺度，只对尺度以及重力的方向做了优化，对应的因子图如下：
尺度以及重力方向的优化
代码分析如下，对每个地图中的所有关键帧进行遍历：

void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale)
{
    int its = 10;//设置最大迭代次数
    long unsigned int maxKFid = pMap->GetMaxKFid();
    const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();//从地图中获得所有的关键帧

    // Setup optimizer
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolverX::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();

    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);

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

    // Set KeyFrame vertices (all variables are fixed)设置关键帧的节点，所有的变量保持固定
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKFi = vpKFs[i];
        if(pKFi->mnId>maxKFid)//如果当前关键帧的ID大于所允许的最大ID，跳过这个关键帧
            continue;
        VertexPose * VP = new VertexPose(pKFi);//关键帧的位姿设置为节点
        VP->setId(pKFi->mnId);
        VP->setFixed(true);
        optimizer.addVertex(VP);

        VertexVelocity* VV = new VertexVelocity(pKFi);//关键帧的速度设置为节点
        VV->setId(maxKFid+1+(pKFi->mnId));
        VV->setFixed(true);//设置速度节点固定
        optimizer.addVertex(VV);

        // Vertex of fixed biases 将陀螺仪以及加速度的偏差设置为固定的，并添加到因子图当中
        VertexGyroBias* VG = new VertexGyroBias(vpKFs.front());
        VG->setId(2*(maxKFid+1)+(pKFi->mnId));
        VG->setFixed(true);
        optimizer.addVertex(VG);
        VertexAccBias* VA = new VertexAccBias(vpKFs.front());
        VA->setId(3*(maxKFid+1)+(pKFi->mnId));
        VA->setFixed(true);
        optimizer.addVertex(VA);
    }

    // Gravity and scale 因为重力方向矩阵以及尺度因子是待优化的变量，所以不固定
    VertexGDir* VGDir = new VertexGDir(Rwg);
    VGDir->setId(4*(maxKFid+1));
    VGDir->setFixed(false);
    optimizer.addVertex(VGDir);
    VertexScale* VS = new VertexScale(scale);
    VS->setId(4*(maxKFid+1)+1);
    VS->setFixed(false);
    optimizer.addVertex(VS);

    // Graph edges
    for(size_t i=0;i<vpKFs.size();i++)
    {
        KeyFrame* pKFi = vpKFs[i];
        //对每个关键帧，判断其是否有前一关键帧数据，是否超出检索范围
        if(pKFi->mPrevKF && pKFi->mnId<=maxKFid)
        {
            if(pKFi->isBad() || pKFi->mPrevKF->mnId>maxKFid)
                continue;
            //将优化器中之前加入的节点作为hypergraph中的顶点，根据ID进行选择添加
            g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex* VV1 = optimizer.vertex((maxKFid+1)+pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex* VP2 =  optimizer.vertex(pKFi->mnId);
            g2o::HyperGraph::Vertex* VV2 = optimizer.vertex((maxKFid+1)+pKFi->mnId);
            g2o::HyperGraph::Vertex* VG = optimizer.vertex(2*(maxKFid+1)+pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex* VA = optimizer.vertex(3*(maxKFid+1)+pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex* VGDir = optimizer.vertex(4*(maxKFid+1));
            g2o::HyperGraph::Vertex* VS = optimizer.vertex(4*(maxKFid+1)+1);
            if(!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS)
            {
                Verbose::PrintMess("Error" + to_string(VP1->id()) + ", " + to_string(VV1->id()) + ", " + to_string(VG->id()) + ", " + to_string(VA->id()) + ", " + to_string(VP2->id()) + ", " + to_string(VV2->id()) +  ", " + to_string(VGDir->id()) + ", " + to_string(VS->id()), Verbose::VERBOSITY_NORMAL);

                continue;
            }
            EdgeInertialGS* ei = new EdgeInertialGS(pKFi->mpImuPreintegrated);//预积分结果作为多向边将各个顶点连接起来
            ei->setVertex(0,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VP1));
            ei->setVertex(1,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VV1));
            ei->setVertex(2,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VG));
            ei->setVertex(3,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VA));
            ei->setVertex(4,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VP2));
            ei->setVertex(5,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VV2));
            ei->setVertex(6,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VGDir));
            ei->setVertex(7,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VS));

            optimizer.addEdge(ei);//将预积分连接起来的边加入优化器
        }
    }

    // Compute error for different scales
    optimizer.setVerbose(false);
    optimizer.initializeOptimization();
    optimizer.optimize(its);

    // Recover optimized data 更新待估计的尺度因子以及重力方向
    scale = VS->estimate();
    Rwg = VGDir->estimate().Rwg;
}