ORB-SLAM3：FullInertialBA()代码分析

此函数定义在inlude/Optimizer.h中，用于进行视觉+IMU的全局BA优化。
除了关键帧Pose之外，还会优化重建地标点位置，以下简称为FIBA函数。

定义

void static FullInertialBA(Map *pMap, int its, const bool bFixLocal=false, const unsigned long nLoopKF=0,
      bool *pbStopFlag=NULL, bool bInit=false, float priorG = 1e2, float priorA=1e6, Eigen::VectorXd *vSingVal = NULL, bool *bHess=NULL);

主要在惯导初始化，即LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)函数中使用。
因在“成熟”的初始化之前，会进行3次对InitializeIMU()的调用，FIBA的调用也随传入参数不同而有所不同。
以下对此函数实现进行分析。

参数

• pMap：当前地图指针；
• its：G2O优化器的最大迭代次数；
• bFixLocal：是否固定局部地图的标记位；
• nLoopKF：检测到LoopClosure中的KF索引；
• pbStopFlag：是否强制结束的标记位；
• bInit：是否为惯导初始化中的FIBA的标记位；
• priorG：陀螺仪偏置先验权重；
• priorA：加速计偏置先验权重；
• vSingVal：貌似无用；
• bHess：貌似无用。

代码分析

从地图中提取相关信息：

long unsigned int maxKFid = pMap->GetMaxKFid();
const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();

配置G2O优化器：

// 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);
solver->setUserLambdaInit(1e-5);
optimizer.setAlgorithm(solver);
optimizer.setVerbose(false);
 
if(pbStopFlag)
    optimizer.setForceStopFlag(pbStopFlag);

关键帧信息作为顶点，添加到优化器中：

1.1、所有符合判断条件的关键帧Pose都要作为优化器的顶点，这些顶点默认是不固定的，即，待优化的；
1.2、若固定局部地图的标记位设置为true，则使用判断条件计算对应关键帧是否会被固定，并记录下应固定而不固定的关键帧个数，即需要优化的关键帧个数；
2.1、若关键帧的IMU初始化标志位为true，则当前帧的速度也要添加到优化图中作为顶点，是否固定由1中对应判断决定；
2.2、若不是惯导初始化过程，则每个关键帧的偏置也要添加到优化图中，作为顶点；
3 、 若函数调用时处于惯导初始化过程中，则只需要将最后一个关键帧的偏置，作为顶点添加到优化图中；
4、 若在固定局部地图模式下，需要优化的关键帧个数小于设定阈值，直接返回。

// Set KeyFrame vertices
KeyFrame* pIncKF;
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);
    pIncKF=pKFi;
    // 1.1
    bool bFixed = false;
    // 1.2
    if(bFixLocal)
    {
        bFixed = (pKFi->mnBALocalForKF>=(maxKFid-1)) || (pKFi->mnBAFixedForKF>=(maxKFid-1));
        if(!bFixed)
            nNonFixed++;
        VP->setFixed(bFixed);
    }
    optimizer.addVertex(VP);
     
    // 2.1
    if(pKFi->bImu)
    {
        VertexVelocity* VV = new VertexVelocity(pKFi);
        VV->setId(maxKFid+3*(pKFi->mnId)+1);
        VV->setFixed(bFixed);
        optimizer.addVertex(VV);
        // 2.2
        if (!bInit)
        {
            VertexGyroBias* VG = new VertexGyroBias(pKFi);
            VG->setId(maxKFid+3*(pKFi->mnId)+2);
            VG->setFixed(bFixed);
            optimizer.addVertex(VG);
            VertexAccBias* VA = new VertexAccBias(pKFi);
            VA->setId(maxKFid+3*(pKFi->mnId)+3);
            VA->setFixed(bFixed);
            optimizer.addVertex(VA);
        }
    }
}
 
// 3
if (bInit)
{
    VertexGyroBias* VG = new VertexGyroBias(pIncKF);
    VG->setId(4*maxKFid+2);
    VG->setFixed(false);
    optimizer.addVertex(VG);
    VertexAccBias* VA = new VertexAccBias(pIncKF);
    VA->setId(4*maxKFid+3);
    VA->setFixed(false);
    optimizer.addVertex(VA);
}
 
// 4
if(bFixLocal)
{
    if(nNonFixed<3)
        return;
}

IMU信息作为顶点，添加到优化器中；

帧间连接关系作为边，构造优化图结构：

IMU数据主要用于使用预积分，预测两关键帧之间的运动轨迹，因此在使用时要考虑与前一关键帧。
1、 无前一关键帧数据，IMU数据无用；
2.1、更新当前关键帧IMU偏置参数，当前关键帧与前一关键帧，都作为顶点，添加到优化器定义中；
2.2、若非处于初始化过程中，相互联系的两个关键帧的偏置都有待优化，若初始化，只优化上述最后一个关键帧的偏置；
2.3、针对不同初始化阶段，设置因信息不全而忽略当前关键帧的逻辑；
3、 包含当前帧IMU信息的边EdgeInertial，是一个将上述的相关顶点连接起来的多向边，设置鲁棒核参数后，添加到优化图中；
4、 非初始化阶段，需要关注两个相邻关键帧之间偏置的变化，设置将陀螺仪/加速计偏置连接起来的双向边，添加到优化图中；
5、 初始化阶段，只关心整体估计的偏置参数，而不是每一帧的参数，使用先验权重，设置对应单向边，添加到优化图中。

// IMU links
for(size_t i=0;i<vpKFs.size();i++)
{
    KeyFrame* pKFi = vpKFs[i];
     
    // 1
    if(!pKFi->mPrevKF)
    {
        Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!", Verbose::VERBOSITY_NORMAL);
        continue;
    }
 
    if(pKFi->mPrevKF && pKFi->mnId<=maxKFid)
    {
        if(pKFi->isBad() || pKFi->mPrevKF->mnId>maxKFid)
            continue;
        if(pKFi->bImu && pKFi->mPrevKF->bImu)
        {
            // 2.1
            pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
            g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
            g2o::HyperGraph::Vertex* VV1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+1);
 
            g2o::HyperGraph::Vertex* VG1;
            g2o::HyperGraph::Vertex* VA1;
            g2o::HyperGraph::Vertex* VG2;
            g2o::HyperGraph::Vertex* VA2;
         
            // 2.2
            if (!bInit)
            {
                VG1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+2);
                VA1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+3);
                VG2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+2);
                VA2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+3);
            }
            else
            {
                VG1 = optimizer.vertex(4*maxKFid+2);
                VA1 = optimizer.vertex(4*maxKFid+3);
            }
 
            g2o::HyperGraph::Vertex* VP2 =  optimizer.vertex(pKFi->mnId);
            g2o::HyperGraph::Vertex* VV2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+1);
             
            // 2.3
            if (!bInit)
            {
                if(!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2)
                {
                    cout << "Error" << VP1 << ", "<< VV1 << ", "<< VG1 << ", "<< VA1 << ", " << VP2 << ", " << VV2 <<  ", "<< VG2 << ", "<< VA2 <<endl;
 
                    continue;
                }
            }
            else
            {
                if(!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2)
                {
                    cout << "Error" << VP1 << ", "<< VV1 << ", "<< VG1 << ", "<< VA1 << ", " << VP2 << ", " << VV2 <<endl;
 
                    continue;
                }
            }
             
            // 3
            EdgeInertial* ei = new EdgeInertial(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*>(VG1));
            ei->setVertex(3,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VA1));
            ei->setVertex(4,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VP2));
            ei->setVertex(5,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VV2));
 
            g2o::RobustKernelHuber* rki = new g2o::RobustKernelHuber;
            ei->setRobustKernel(rki);
            rki->setDelta(sqrt(16.92));
 
            optimizer.addEdge(ei);
 
            // 4
            if (!bInit)
            {
                EdgeGyroRW* egr= new EdgeGyroRW();
                egr->setVertex(0,VG1);
                egr->setVertex(1,VG2);
                cv::Mat cvInfoG = pKFi->mpImuPreintegrated->C.rowRange(9,12).colRange(9,12).inv(cv::DECOMP_SVD);
                Eigen::Matrix3d InfoG;
                for(int r=0;r<3;r++)
                    for(int c=0;c<3;c++)
                        InfoG(r,c)=cvInfoG.at<float>(r,c);
                egr->setInformation(InfoG);
                egr->computeError();
                optimizer.addEdge(egr);
 
                EdgeAccRW* ear = new EdgeAccRW();
                ear->setVertex(0,VA1);
                ear->setVertex(1,VA2);
                cv::Mat cvInfoA = pKFi->mpImuPreintegrated->C.rowRange(12,15).colRange(12,15).inv(cv::DECOMP_SVD);
                Eigen::Matrix3d InfoA;
                for(int r=0;r<3;r++)
                    for(int c=0;c<3;c++)
                        InfoA(r,c)=cvInfoA.at<float>(r,c);
                ear->setInformation(InfoA);
                ear->computeError();
                optimizer.addEdge(ear);
            }
        }
        else
        {
            cout << pKFi->mnId << " or " << pKFi->mPrevKF->mnId << " no imu" << endl;
        }
    }
}
 
// 5
if (bInit)
{
    g2o::HyperGraph::Vertex* VG = optimizer.vertex(4*maxKFid+2);
    g2o::HyperGraph::Vertex* VA = optimizer.vertex(4*maxKFid+3);
 
    // Add prior to comon biases
    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);
}

地标点相关参数设置：

const float thHuberMono = sqrt(5.991);
const float thHuberStereo = sqrt(7.815);
 
// 设置地标点在优化器中将使用到的特有ID起点，紧接在上述关键帧所用的ID块之后
const unsigned long iniMPid = maxKFid*5;

地标点位置，作为顶点，添加到优化器定义中：

1、 地标点在世界坐标系中位置，作为顶点添加到优化器；
2.1、获取可观测到当前地标点的关键帧相关信息，用于建立地标点与关键帧之间的联系；
2.2、对可被观测到的每一个关键帧，获取其信息；
2.3、获取关键帧在优化器中的定义，判断该关键帧的POSE是否固定不优化；
2.4、向优化图中添加连接地标点与关键帧的双向边EdgeMono，设置相关参数；
3、 判断当前地标点与全局设置是否不同；
4、 判断是否强制停止。

vector<bool> vbNotIncludedMP(vpMPs.size(),false);
 
for(size_t i=0; i<vpMPs.size(); i++)
{
    // 1
    MapPoint* pMP = vpMPs[i];
    g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
    vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
    unsigned long id = pMP->mnId+iniMPid+1;
    vPoint->setId(id);
    vPoint->setMarginalized(true);
    optimizer.addVertex(vPoint);
     
    // 2.1
    const map<KeyFrame*,tuple<int,int>> observations = pMP->GetObservations();
 
    bool bAllFixed = true;
 
    //Set edges
    for(map<KeyFrame*,tuple<int,int>>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        // 2.2
        // tuple.first is the keyframe id, second as the point id in the keypoint list
        KeyFrame* pKFi = mit->first;
 
        if(pKFi->mnId>maxKFid)
            continue;
 
        if(!pKFi->isBad())
        {
            const int leftIndex = get<0>(mit->second);
            cv::KeyPoint kpUn;
 
            if(leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)]<0) // Monocular observation
            {
                kpUn = pKFi->mvKeysUn[leftIndex];
                Eigen::Matrix<double,2,1> obs;
                obs << kpUn.pt.x, kpUn.pt.y;
 
                EdgeMono* e = new EdgeMono(0);
 
                // 2.3
                g2o::OptimizableGraph::Vertex* VP = dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId));
                if(bAllFixed)
                    if(!VP->fixed())
                        bAllFixed=false;
 
                // 2.4
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, VP);
                e->setMeasurement(obs);
                const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
 
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
 
                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuberMono);
 
                optimizer.addEdge(e);
            }
            else if(leftIndex != -1 && pKFi->mvuRight[leftIndex] >= 0) // stereo observation
            {
                kpUn = pKFi->mvKeysUn[leftIndex];
                const float kp_ur = pKFi->mvuRight[leftIndex];
                Eigen::Matrix<double,3,1> obs;
                obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
 
                EdgeStereo* e = new EdgeStereo(0);
 
                g2o::OptimizableGraph::Vertex* VP = dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId));
                if(bAllFixed)
                    if(!VP->fixed())
                        bAllFixed=false;
 
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, VP);
                e->setMeasurement(obs);
                const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
 
                e->setInformation(Eigen::Matrix3d::Identity()*invSigma2);
 
                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuberStereo);
 
                optimizer.addEdge(e);
            }
 
            if(pKFi->mpCamera2){ // Monocular right observation
                int rightIndex = get<1>(mit->second);
 
                if(rightIndex != -1 && rightIndex < pKFi->mvKeysRight.size()){
                    rightIndex -= pKFi->NLeft;
 
                    Eigen::Matrix<double,2,1> obs;
                    kpUn = pKFi->mvKeysRight[rightIndex];
                    obs << kpUn.pt.x, kpUn.pt.y;
 
                    EdgeMono *e = new EdgeMono(1);
 
                    g2o::OptimizableGraph::Vertex* VP = dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId));
                    if(bAllFixed)
                        if(!VP->fixed())
                            bAllFixed=false;
 
                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                    e->setVertex(1, VP);
                    e->setMeasurement(obs);
                    const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                    e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
 
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(thHuberMono);
 
                    optimizer.addEdge(e);
                }
            }
        }
    }
 
    // 3
    if(bAllFixed)
    {
        optimizer.removeVertex(vPoint);
        vbNotIncludedMP[i]=true;
    }
}
 
// 4
if(pbStopFlag)
    if(*pbStopFlag)
        return;

进行图优化：

optimizer.initializeOptimization();
optimizer.optimize(its);

获取优化后关键帧相关参数：

包括POSE，速度，偏置参数等。

// Recover optimized data
//Keyframes
for(size_t i=0; i<vpKFs.size(); i++)
{
    KeyFrame* pKFi = vpKFs[i];
    if(pKFi->mnId>maxKFid)
        continue;
    VertexPose* VP = static_cast<VertexPose*>(optimizer.vertex(pKFi->mnId));
    if(nLoopId==0)
    {
        cv::Mat Tcw = Converter::toCvSE3(VP->estimate().Rcw[0], VP->estimate().tcw[0]);
        pKFi->SetPose(Tcw);
    }
    else
    {
        pKFi->mTcwGBA = cv::Mat::eye(4,4,CV_32F);
        Converter::toCvMat(VP->estimate().Rcw[0]).copyTo(pKFi->mTcwGBA.rowRange(0,3).colRange(0,3));
        Converter::toCvMat(VP->estimate().tcw[0]).copyTo(pKFi->mTcwGBA.rowRange(0,3).col(3));
        pKFi->mnBAGlobalForKF = nLoopId;
 
    }
    if(pKFi->bImu)
    {
        VertexVelocity* VV = static_cast<VertexVelocity*>(optimizer.vertex(maxKFid+3*(pKFi->mnId)+1));
        if(nLoopId==0)
        {
            pKFi->SetVelocity(Converter::toCvMat(VV->estimate()));
        }
        else
        {
            pKFi->mVwbGBA = Converter::toCvMat(VV->estimate());
        }
 
        VertexGyroBias* VG;
        VertexAccBias* VA;
        if (!bInit)
        {
            VG = static_cast<VertexGyroBias*>(optimizer.vertex(maxKFid+3*(pKFi->mnId)+2));
            VA = static_cast<VertexAccBias*>(optimizer.vertex(maxKFid+3*(pKFi->mnId)+3));
        }
        else
        {
            VG = static_cast<VertexGyroBias*>(optimizer.vertex(4*maxKFid+2));
            VA = static_cast<VertexAccBias*>(optimizer.vertex(4*maxKFid+3));
        }
 
        Vector6d vb;
        vb << VG->estimate(), VA->estimate();
        IMU::Bias b (vb[3],vb[4],vb[5],vb[0],vb[1],vb[2]);
        if(nLoopId==0)
        {
            pKFi->SetNewBias(b);
        }
        else
        {
            pKFi->mBiasGBA = b;
        }
    }
}

获取优化后地标点相关参数：

//Points
for(size_t i=0; i<vpMPs.size(); i++)
{
    if(vbNotIncludedMP[i])
        continue;
 
    MapPoint* pMP = vpMPs[i];
    g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+iniMPid+1));
 
    if(nLoopId==0)
    {
        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 = nLoopId;
    }
}