ORBSLAM3 VIO初始化

按照规矩，先讲一下ORBSLAM3中的初始化大致流程

根据ORB-SLAM3的论文介绍， IMU的初始化方法是基于下面3点：

1） 纯视觉SLAM可以提供很好的位姿估计，所以可以用纯视觉的结果来改善IMU的估计；

2） 显示地将尺度表示为一个优化的变量， 尺度会更快地收敛；

3）在IMU初始化阶段，忽略传感器的不确定度将会产生更多不可预知错误。

整个初始化过程分为：

    1.Vision-only MAP Estimation；

    2.Inertial-only MAP Estimation；

    3.Visual-Inertial MAP Estimation。

    4.To improve the initial estimation, visual-inertial BA is performed 5 and 15

seconds after initialization。

与vins-mono不同的是，vins-mono大部分都是用LDLT直接法进行求解，同时还有在线标定外参，具体请参考VINS-MONO初始化

纯视觉初始化和以前的ORBSLAM2是一样的，是利用基础矩阵和单应矩阵进行初始化。然后再采用之前纯视觉估计出来的相机位姿单独的进行纯IMU初始化，主要估计imu的bias和单目尺度，以及坐标系的对齐，具体的步骤如下图：

 由图可知，imu初始化实在LocalMapping主函数中,红色框框的是纯IMU初始化，绿色的是纯视觉初始化，蓝色的VIO初始化，代码具体如下：

void LocalMapping::Run()
{

// 标记状态，表示当前run函数正在运行，尚未结束
mbFinished = false;
// 主循环
while(1)
{
// Tracking will see that Local Mapping is busy
// Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我
// LocalMapping线程处理的关键帧都是Tracking线程发过来的
SetAcceptKeyFrames(false);

// Check if there are keyframes in the queue
// 等待处理的关键帧列表不为空 并且imu正常
if(CheckNewKeyFrames() && !mbBadImu)
{
// std::cout << "LM" << std::endl;
std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();

// BoW conversion and insertion in Map
// Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
ProcessNewKeyFrame();
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

// Check recent MapPoints
// Step 3 根据地图点的观测情况剔除质量不好的地图点
MapPointCulling();
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

// Triangulate new MapPoints
// Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
CreateNewMapPoints();
std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();

mbAbortBA = false;
// 已经处理完队列中的最后的一个关键帧
if(!CheckNewKeyFrames())
{
// Find more matches in neighbor keyframes and fuse point duplications
// Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
SearchInNeighbors();
}

std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
std::chrono::steady_clock::time_point t5 = t4, t6 = t4;
// mbAbortBA = false;

int num_FixedKF_BA = 0;
// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
if(!CheckNewKeyFrames() && !stopRequested())
{
// 地图中关键帧数目大于2个
if(mpAtlas->KeyFramesInMap()>2)
{
// Step 6.1 处于IMU模式并且当前关键帧所在的地图已经完成IMU初始化。这里是因为图像出问题了，采用imu数据？
if(mbInertial && mpCurrentKeyFrame->GetMap()->isImuInitialized())
{
// 计算上一帧到当前帧相机光心的距离 + 上上帧到上一帧相机光心的距离
float dist = cv::norm(mpCurrentKeyFrame->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->GetCameraCenter()) +
cv::norm(mpCurrentKeyFrame->mPrevKF->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->mPrevKF->GetCameraCenter());
// 如果距离大于5厘米，记录当前KF和上一KF时间戳的差，累加到mTinit
if(dist>0.05)
mTinit += mpCurrentKeyFrame->mTimeStamp - mpCurrentKeyFrame->mPrevKF->mTimeStamp;
// 当前关键帧所在的地图已经完成IMU BA2 
if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2())
{
// 如果累计时间差小于10s 并且 距离小于2厘米，认为运动幅度太小，不足以初始化IMU，将mbBadImu设置为true
if((mTinit<10.f) && (dist<0.02))
{
cout << "Not enough motion for initializing. Reseting..." << endl;
unique_lock<mutex> lock(mMutexReset);
mbResetRequestedActiveMap = true;
mpMapToReset = mpCurrentKeyFrame->GetMap();
mbBadImu = true;
}
}
// 条件---------1.1、跟踪成功的内点数目大于75-----1.2、并且是单目--或--2.1、跟踪成功的内点数目大于100-----2.2、并且不是单目 
bool bLarge = ((mpTracker->GetMatchesInliers()>75)&&mbMonocular)||((mpTracker->GetMatchesInliers()>100)&&!mbMonocular);
// BA优化局部地图IMU
Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(), bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2());
}
else
{
// Step 6.2 不是IMU模式或者当前关键帧所在的地图还未完成IMU初始化
std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
// 局部地图BA，不包括IMU数据
// 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA);
std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
}
}

t5 = std::chrono::steady_clock::now();

// Initialize IMU here
// Step 7 当前关键帧所在地图的IMU初始化
if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial)
{
if (mbMonocular)
InitializeIMU(1e2, 1e10, true);
else
InitializeIMU(1e2, 1e5, true);
}

// Check redundant local Keyframes
// Step 8 检测并剔除当前帧相邻的关键帧中冗余的关键帧
// 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到
KeyFrameCulling();

t6 = std::chrono::steady_clock::now();
// Step 9 如果累计时间差小于100s 并且 是IMU模式，进行VIBA
if ((mTinit<100.0f) && mbInertial)
{
// Step 9.1 根据条件判断是否进行VIBA1
// 条件：1、当前关键帧所在的地图还未完成IMU初始化---并且--------2、正常跟踪状态----------
if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called
{
// 当前关键帧所在的地图还未完成VIBA 1
if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){
// 如果累计时间差大于5s，开始VIBA 1
if (mTinit>5.0f)
{
cout << "start VIBA 1" << endl;
mpCurrentKeyFrame->GetMap()->SetIniertialBA1();
if (mbMonocular)
InitializeIMU(1.f, 1e5, true); // 1.f, 1e5
else
InitializeIMU(1.f, 1e5, true); // 1.f, 1e5

cout << "end VIBA 1" << endl;
}
}
//else if (mbNotBA2){
// Step 9.2 根据条件判断是否进行VIBA2
// 当前关键帧所在的地图还未完成VIBA 2
else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){
// 如果累计时间差大于15s，开始VIBA 2
if (mTinit>15.0f){ // 15.0f
cout << "start VIBA 2" << endl;
mpCurrentKeyFrame->GetMap()->SetIniertialBA2();
if (mbMonocular)
InitializeIMU(0.f, 0.f, true); // 0.f, 0.f
else
InitializeIMU(0.f, 0.f, true);

cout << "end VIBA 2" << endl;
}
}

// 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;
//也就是说这里会无限制的在100s内优化尺度和重力
if (mbMonocular)
ScaleRefinement();
cout << "end scale ref" << endl;
}
}
}
}

std::chrono::steady_clock::time_point t7 = std::chrono::steady_clock::now();

// Step 10 将当前帧加入到闭环检测队列中
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
std::chrono::steady_clock::time_point t8 = std::chrono::steady_clock::now();

double t_procKF = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t1 - t0).count();
double t_MPcull = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
double t_CheckMP = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t3 - t2).count();
double t_searchNeigh = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t4 - t3).count();
double t_Opt = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t5 - t4).count();
double t_KF_cull = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t6 - t5).count();
double t_Insert = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t8 - t7).count();


}
else if(Stop() && !mbBadImu) // 当要终止当前线程的时候
{
// Safe area to stop
while(isStopped() && !CheckFinish())
{
// cout << "LM: usleep if is stopped" << endl;
// 如果还没有结束利索,那么等等它
usleep(3000);
}
// 然后确定终止了就跳出这个线程的主循环
if(CheckFinish())
break;
}

// 查看是否有复位线程的请求
ResetIfRequested();

// Tracking will see that Local Mapping is busy
SetAcceptKeyFrames(true);

// 如果当前线程已经结束了就跳出主循环
if(CheckFinish())
break;

// cout << "LM: normal usleep" << endl;
usleep(3000);
}

// 设置线程已经终止
SetFinish();
}

其中纯IMU初始化的接口在这里使用

if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial)

                {

                    if (mbMonocular)

                        InitializeIMU(1e2, 1e10, true);

                    else

                        InitializeIMU(1e2, 1e5, true);

                }

1e2表示bias的权重，属于经验值吧

void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
{
if (mbResetRequested)
return;

float minTime;
int nMinKF;
if (mbMonocular)
{
minTime = 2.0;
nMinKF = 10;
}
else
{
minTime = 1.0;
nMinKF = 10;
}

if(mpAtlas->KeyFramesInMap()<nMinKF)
return;

// Retrieve all keyframe in temporal order
list<KeyFrame*> lpKF;
KeyFrame* pKF = mpCurrentKeyFrame;
//找到当前帧的最前一帧
while(pKF->mPrevKF)
{
lpKF.push_front(pKF);
pKF = pKF->mPrevKF;
}
// lpKF放置的是最前一帧...前一帧，当前帧
lpKF.push_front(pKF);
vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());
// 如果小于关键帧10，则返回
if(vpKF.size()<nMinKF)
return;
// mFirstTs为最开始的时间戳
mFirstTs=vpKF.front()->mTimeStamp;
// 如果两帧之间的时间差相对较小，则返回
if(mpCurrentKeyFrame->mTimeStamp-mFirstTs<minTime)
return;

bInitializing = true;
// 检查一下是否需要插入关键帧，这里是要将最新的关键帧插入
//
while(CheckNewKeyFrames())
{
ProcessNewKeyFrame();
vpKF.push_back(mpCurrentKeyFrame);
lpKF.push_back(mpCurrentKeyFrame);
}

const int N = vpKF.size();
IMU::Bias b(0,0,0,0,0,0);

// Compute and KF velocities mRwg estimation
//如果地图初始化没成功
if (!mpCurrentKeyFrame->GetMap()->isImuInitialized())
{
cv::Mat cvRwg;//世界坐标系和重力方向的旋转量
cv::Mat dirG = cv::Mat::zeros(3,1,CV_32F);//dirG重力
for(vector<KeyFrame*>::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++)
{
if (!(*itKF)->mpImuPreintegrated)
continue;
if (!(*itKF)->mPrevKF)
continue;
// GetImuRotation拿到的是imu到世界的旋转量相当于Rwi
// 通过视觉估计的结果计算对应关键帧时刻IMU的速度
// 假设初始时刻IMU系统的加速度较小， IMU测量主要就是重力,所以这里采用速度去估计重力？
//感觉可以采用加速度均值去估计重力方向，不过这里应该没关系，因为这个是初始值，后面会优化的
dirG -= (*itKF)->mPrevKF->GetImuRotation()*(*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
// 计算两帧之间的平均速度
cv::Mat _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT;
(*itKF)->SetVelocity(_vel);
(*itKF)->mPrevKF->SetVelocity(_vel);
}

dirG = dirG/cv::norm(dirG);
//vhat = (gI x gw) / |gI x gw|
//惯性系下实际重力的方向向量坐标gI
cv::Mat gI = (cv::Mat_<float>(3,1) << 0.0f, 0.0f, -1.0f);
cv::Mat v = gI.cross(dirG);
const float nv = cv::norm(v);
const float cosg = gI.dot(dirG);
const float ang = acos(cosg);
cv::Mat vzg = v*ang/nv;
//mRwg惯性系到世界系的旋转矩阵
cvRwg = IMU::ExpSO3(vzg);
mRwg = Converter::toMatrix3d(cvRwg);
mTinit = mpCurrentKeyFrame->mTimeStamp-mFirstTs;
}
else
{
mRwg = Eigen::Matrix3d::Identity();
mbg = Converter::toVector3d(mpCurrentKeyFrame->GetGyroBias());
mba = Converter::toVector3d(mpCurrentKeyFrame->GetAccBias());
}

mScale=1.0;

mInitTime = mpTracker->mLastFrame.mTimeStamp-vpKF.front()->mTimeStamp;
//进行初始化， 以IMU位姿为基准， 估计IMU bias， 尺度和惯性系与世界系（首帧IMU系）之间的旋转矩阵
// 这里对应论文中的Inertial-only MAP Estimation，InertialOptimization初始化过程中不涉及点， 只有imu预积分的边，各个关键帧时刻对应的IMU位姿固定不变， 但速度会被优化
std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

/*cout << "scale after inertial-only optimization: " << mScale << endl;
cout << "bg after inertial-only optimization: " << mbg << endl;
cout << "ba after inertial-only optimization: " << mba << endl;*/

if (mScale<1e-1)
{
cout << "scale too small" << endl;
bInitializing=false;
return;
}


// Before this line we are not changing the map

unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
if ((fabs(mScale-1.f)>0.00001)||!mbMonocular)
{
//恢复尺度，对齐世界坐标系
mpAtlas->GetCurrentMap()->ApplyScaledRotation(Converter::toCvMat(mRwg).t(),mScale,true);
mpTracker->UpdateFrameIMU(mScale,vpKF[0]->GetImuBias(),mpCurrentKeyFrame);
}
std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();

// Check if initialization OK
if (!mpAtlas->isImuInitialized())
for(int i=0;i<N;i++)
{
KeyFrame* pKF2 = vpKF[i];
pKF2->bImu = true;
}

/*cout << "Before GIBA: " << endl;
cout << "ba: " << mpCurrentKeyFrame->GetAccBias() << endl;
cout << "bg: " << mpCurrentKeyFrame->GetGyroBias() << endl;*/

std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
if (bFIBA)
{
// BA优化
if (priorA!=0.f)
Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, 0, NULL, true, priorG, priorA);
else
Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, 0, NULL, false);
}

std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now();

// If initialization is OK
//更新tracker端维护的local map
mpTracker->UpdateFrameIMU(1.0,vpKF[0]->GetImuBias(),mpCurrentKeyFrame);
if (!mpAtlas->isImuInitialized())
{
cout << "IMU in Map " << mpAtlas->GetCurrentMap()->GetId() << " is initialized" << endl;
mpAtlas->SetImuInitialized();
mpTracker->t0IMU = mpTracker->mCurrentFrame.mTimeStamp;
mpCurrentKeyFrame->bImu = true;
}

mbNewInit=true;
mnKFs=vpKF.size();
mIdxInit++;

for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
{
(*lit)->SetBadFlag();
delete *lit;
}
mlNewKeyFrames.clear();

mpTracker->mState=Tracking::OK;
bInitializing = false;

/*cout << "After GIBA: " << endl;
cout << "ba: " << mpCurrentKeyFrame->GetAccBias() << endl;
cout << "bg: " << mpCurrentKeyFrame->GetGyroBias() << endl;
double t_inertial_only = std::chrono::duration_cast<std::chrono::duration<double> >(t1 - t0).count();
double t_update = std::chrono::duration_cast<std::chrono::duration<double> >(t3 - t2).count();
double t_viba = std::chrono::duration_cast<std::chrono::duration<double> >(t5 - t4).count();
cout << t_inertial_only << ", " << t_update << ", " << t_viba << endl;*/

mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex();

return;
}

函数中我们可以得出，重力对齐在InertialOptimization之后进行一次，FullInertialBA分两种情况， 第一种用于初始化， 这种在IMU初始化的时候使用，IMU bias处理方式与InertialOptimization一样， 所有时刻bias都一样， 只有一个vertex, bias优化过程中约束到初始值；另一种用于IMU初始化之后， 各个预积分内的bias不一致，优化时约束前后两个相邻预积分bias之间的差值。

void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA)
{
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);

// 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)//初始化的时候bFixedVel为false
VV->setFixed(true);
else
VV->setFixed(false);

optimizer.addVertex(VV);
}

// 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
// acc 和 gyro bias 在优化过程中约束在0值附近， 且初始化所有时刻的bias认为是相同的，所有用同一个vertex
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);

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

}
}

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

scale = VS->estimate();

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

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

}
}

InertialOptimization初始化过程中不涉及点， 只有imu预积分的边，各个关键帧时刻对应的IMU位姿固定不变， 但速度会被优化。而在论文中也说明了，初始化成功之后，还在5s和15s再次进行初始化，如果是单目的话，还会优化尺度和重力方向。得到最佳效果。这个在主函数中有解释。

我们来看看后面再次优化尺度和重力方向的函数

void LocalMapping::ScaleRefinement()
{
// Minimum number of keyframes to compute a solution
// Minimum time (seconds) between first and last keyframe to compute a solution. Make the difference between monocular and stereo
// unique_lock<mutex> lock0(mMutexImuInit);
if (mbResetRequested)
return;

// Retrieve all keyframes in temporal order
list<KeyFrame*> lpKF;
KeyFrame* pKF = mpCurrentKeyFrame;
while(pKF->mPrevKF)
{
lpKF.push_front(pKF);
pKF = pKF->mPrevKF;
}
lpKF.push_front(pKF);
vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());

while(CheckNewKeyFrames())
{
ProcessNewKeyFrame();
vpKF.push_back(mpCurrentKeyFrame);
lpKF.push_back(mpCurrentKeyFrame);
}

const int N = vpKF.size();

mRwg = Eigen::Matrix3d::Identity();
mScale=1.0;

std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale);
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

if (mScale<1e-1) // 1e-1
{
cout << "scale too small" << endl;
bInitializing=false;
return;
}

// Before this line we are not changing the map
unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
if ((fabs(mScale-1.f)>0.00001)||!mbMonocular)
{
mpAtlas->GetCurrentMap()->ApplyScaledRotation(Converter::toCvMat(mRwg).t(),mScale,true);
mpTracker->UpdateFrameIMU(mScale,mpCurrentKeyFrame->GetImuBias(),mpCurrentKeyFrame);
}
std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();

for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
{
(*lit)->SetBadFlag();
delete *lit;
}
mlNewKeyFrames.clear();

double t_inertial_only = std::chrono::duration_cast<std::chrono::duration<double> >(t1 - t0).count();

// To perform pose-inertial opt w.r.t. last keyframe
mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex();

return;
}

以上就是ORBSLAM3的VIO初始化。