ORB-SLAM3：InertialOptimization()代码分析

此函数定义在inlude/Optimizer.h中，共有4个重载：

// Inertial pose-graph
void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd  &covInertial, bool bFixedVel=false, bool bGauss=false, float priorG = 1e2, float priorA = 1e6);

void static InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG = 1e2, float priorA = 1e6);

void static InertialOptimization(vector<KeyFrame*> vpKFs, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG = 1e2, float priorA = 1e6);

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

在惯导初始化时，即LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)函数中，使用的是第一个重载，本文针对此分析代码。

Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);

参数

• pMap：当前地图；
• Rwg: 待优化的重力方向；
• scale: 待优化的尺度因子；
• bg: 待优化的陀螺仪偏置（gyro bias）;
• ba: 待优化的加速计偏置（accel bias）；
• bMono: 传感器是否为单目的标记位，在调用时设置为true；
• covInertial: 惯导协方差矩阵（貌似无用）；
• bFixedVel: 是否固定关键帧速度不优化的标记位，false；
• bGuass: 惯导噪声是否符合高斯分布的标记位，false；
• priorG: 陀螺仪偏置优化权重；
• priorA: 加速计偏置优化权重。

实现流程分析

设置最大迭代次数，从地图中提取所需信息，并设置G2O优化器：

 Verbose::PrintMess("inertial optimization", Verbose::VERBOSITY_NORMAL);
 int its = 200; // Check number of iterations
 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::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

 if (priorG!=0.f)
     solver->setUserLambdaInit(1e3);

optimizer.setAlgorithm(solver);

使用G2O优化，即构建图优化问题，主要是要把对应的顶点和边设置正确。顶点表示的是待优化的参数或者状态量，边表示的应为这些量之间的约束关系。

将纯视觉SLAM构建出的地图中关键帧的位姿与不带尺度的速度，都作为所构建优化图的顶点：

这里优化的目的为初始化IMU参数使得通过其预积分得到的IMU运动轨迹与视觉SLAM对齐，因此保持关键帧的位姿不变，在G2O构建中对这些节点setFixed(true)。
而关键帧的速度，与尺度因子相关，在这个优化问题中也是待优化的参数，因此速度也应被优化，setFixed(false)。
setID()函数用于为顶点设置一个独特的ID，用于寻址使用，注意到这里关键帧与速度是成对出现的，因此在ID的设置上，也是按顺序对应排列。

// Set KeyFrame vertices (fixed poses and optimizable velocities)
for(size_t i=0; i<vpKFs.size(); i++)
{
    
    
    KeyFrame* pKFi = vpKFs[i];
    if(pKFi->mnId>maxKFid)
        continue;
    VertexPose * VP = new VertexPose(pKFi);
    VP->setId(pKFi->mnId);
    VP->setFixed(true);
    optimizer.addVertex(VP);

    VertexVelocity* VV = new VertexVelocity(pKFi);
    VV->setId(maxKFid+(pKFi->mnId)+1);
    if (bFixedVel)
        VV->setFixed(true);
    else
        VV->setFixed(false);

    optimizer.addVertex(VV);
}

将IMU传感器偏置作为顶点，先验权重作为边，在优化器中定义：

使用setVertex()设置边的两端顶点，使用setInformation()设置信息矩阵：

// Biases
    VertexGyroBias* VG = new VertexGyroBias(vpKFs.front());
    VG->setId(maxKFid*2+2);
    if (bFixedVel)
        VG->setFixed(true);
    else
        VG->setFixed(false);
    optimizer.addVertex(VG);

    VertexAccBias* VA = new VertexAccBias(vpKFs.front());
    VA->setId(maxKFid*2+3);
    if (bFixedVel)
        VA->setFixed(true);
    else
        VA->setFixed(false);

    optimizer.addVertex(VA);
    // prior acc bias
    EdgePriorAcc* epa = new EdgePriorAcc(cv::Mat::zeros(3,1,CV_32F));
    epa->setVertex(0,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VA));
    double infoPriorA = priorA;
    epa->setInformation(infoPriorA*Eigen::Matrix3d::Identity());
    optimizer.addEdge(epa);
    EdgePriorGyro* epg = new EdgePriorGyro(cv::Mat::zeros(3,1,CV_32F));
    epg->setVertex(0,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VG));
    double infoPriorG = priorG;
    epg->setInformation(infoPriorG*Eigen::Matrix3d::Identity());
    optimizer.addEdge(epg);

将重力方向，与尺度因子，在优化图中定义为顶点：

    // Gravity and scale
    VertexGDir* VGDir = new VertexGDir(Rwg);
    VGDir->setId(maxKFid*2+4);
    VGDir->setFixed(false);
    optimizer.addVertex(VGDir);
    VertexScale* VS = new VertexScale(scale);
    VS->setId(maxKFid*2+5);
    VS->setFixed(!bMono); // Fixed for stereo case
    optimizer.addVertex(VS);

用约束关系构建HyperGraph：

对每个关键帧，判断其是否有前一关键帧数据，是否超出检索范围，是否有预积分数据，来决定其是否可以添加到超图中；
超图的顶点，设置为上述优化器中定义的顶点，使用独特的ID进行寻址；
EdgeInertialGS是一个多向边结构。
通过这种方式，在超图结构中建立了顶点间相互的约束关系。

// Graph edges
    // IMU links with gravity and scale
    vector<EdgeInertialGS*> vpei;
    vpei.reserve(vpKFs.size());
    vector<pair<KeyFrame*,KeyFrame*> > vppUsedKF;
    vppUsedKF.reserve(vpKFs.size());
    std::cout << "build optimization graph" << std::endl;

    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;
            if(!pKFi->mpImuPreintegrated)
                std::cout << "Not preintegrated measurement" << std::endl;

            pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
            g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex* VV1 = optimizer.vertex(maxKFid+(pKFi->mPrevKF->mnId)+1);
            g2o::HyperGraph::Vertex* VP2 =  optimizer.vertex(pKFi->mnId);
            g2o::HyperGraph::Vertex* VV2 = optimizer.vertex(maxKFid+(pKFi->mnId)+1);
            g2o::HyperGraph::Vertex* VG = optimizer.vertex(maxKFid*2+2);
            g2o::HyperGraph::Vertex* VA = optimizer.vertex(maxKFid*2+3);
            g2o::HyperGraph::Vertex* VGDir = optimizer.vertex(maxKFid*2+4);
            g2o::HyperGraph::Vertex* VS = optimizer.vertex(maxKFid*2+5);
            if(!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS)
            {
    
    
                cout << "Error" << VP1 << ", "<< VV1 << ", "<< VG << ", "<< VA << ", " << VP2 << ", " << VV2 <<  ", "<< VGDir << ", "<< VS <<endl;

                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));

            vpei.push_back(ei);

            vppUsedKF.push_back(make_pair(pKFi->mPrevKF,pKFi));
            optimizer.addEdge(ei);

        }
    }

使用G2O解图优化问题，并获取优化后的数值：

// Compute error for different scales
    std::set<g2o::HyperGraph::Edge*> setEdges = optimizer.edges();

    std::cout << "start optimization" << std::endl;
    optimizer.setVerbose(false);
    optimizer.initializeOptimization();
    optimizer.optimize(its);

    std::cout << "end optimization" << std::endl;

    // Recover optimized data
    // Biases
    VG = static_cast<VertexGyroBias*>(optimizer.vertex(maxKFid*2+2));
    VA = static_cast<VertexAccBias*>(optimizer.vertex(maxKFid*2+3));
    Vector6d vb;
    vb << VG->estimate(), VA->estimate();
    bg << VG->estimate();
    ba << VA->estimate();
    scale = VS->estimate();

    IMU::Bias b (vb[3],vb[4],vb[5],vb[0],vb[1],vb[2]);
    Rwg = VGDir->estimate().Rwg;

    cv::Mat cvbg = Converter::toCvMat(bg);

对每个关键帧，更新其相关参数，包括速度，IMU传感器偏置，预积分等：

//Keyframes velocities and biases
    std::cout << "update Keyframes velocities and biases" << std::endl;

    const int N = vpKFs.size();
    for(size_t i=0; i<N; i++)
    {
    
    
        KeyFrame* pKFi = vpKFs[i];
        if(pKFi->mnId>maxKFid)
            continue;

        VertexVelocity* VV = static_cast<VertexVelocity*>(optimizer.vertex(maxKFid+(pKFi->mnId)+1));
        Eigen::Vector3d Vw = VV->estimate(); // Velocity is scaled after
        pKFi->SetVelocity(Converter::toCvMat(Vw));

        if (cv::norm(pKFi->GetGyroBias()-cvbg)>0.01)
        {
    
    
            pKFi->SetNewBias(b);
            if (pKFi->mpImuPreintegrated)
                pKFi->mpImuPreintegrated->Reintegrate();
        }
        else
            pKFi->SetNewBias(b);
    }