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, 1e5cout << "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 mapunique_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初始化。

ORBSLAM3 VIO初始化相关推荐

一文读懂复杂的VIO初始化
作者:小飞飞 | 来源:3DCV 在公众号「3D视觉工坊」后台,回复「原论文」即可获取论文pdf. 视觉惯性融合技术应用于机器人和自动驾驶方面.单目相机和低成本IMU成为最佳选择.因为VIO系统的高度 ...
【学习总结】VIO初始化学习1：Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU
最近看了一篇论文,很是头大,大概看懂了个所以然.记录一下. 论文:Monocular Visual–Inertial State Estimation With Online Initializati ...
从零学习VINS-Mono/Fusion源代码（五）：VIO初始化
本节分析VIO初始化部分 VINS-Mono/Fusion代码学习系列: 从零学习VINS-Mono/Fusion源代码(一):主函数从零学习VINS-Mono/Fusion源代码(二):前端图像跟 ...
【SLAM】ORB-SLAM3解析——综述(1)
之前学习VINS和LIO-SAM的时候都是代码流,不是很重视看论文,现在有空学ORB-SLAM3了,这一次,先看一下论文.考虑到边上班边学,更新的会比较慢.看完论文之后,对这个系统最大的主观感受就是它 ...
视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
LIO-livox - 激光IMU初始化模块分析
激光IMU初始化模块学习概述 TryMAPInitialization() 计算初始的先验旋转(world->local) 求解MAP优化总结概述 LIO-LIVOX中激光IMU的初始化的 ...
SLAM算法知识荟萃
文章目录 SLAM 自动驾驶八股四元数在表示空间旋转时的优势是什么? 介绍自动驾驶系统介绍回环检测介绍词袋模型手撕对极约束使用OpenCV找到四边形的边界介绍卡尔曼滤波推导卡尔曼增益介 ...
mysql 协议说明_MySQL认证协议_MySQL
本文是针对MySQL 5.5.9写的.MySQL协议是向老版本兼容的.老版本的MySQL Client可能不理解下面的某些字段而忽略掉. 实际使用的时候,服务器的协议版本应当大于等于客户端.遗憾的是, ...
由粗到精学习LVI-SAM：论文原文解析
作者丨Alvin一路向东@知乎来源丨https://zhuanlan.zhihu.com/p/463778905 编辑丨3D视觉工坊前言本文是LVI-SAM学习系列的第三部分,在深入分析LVI- ...
SLAM 论文阅读和分类整理
前言:以前读论文,都是靠脑子硬记,哪个实验室,谁,哪一年在什么会议上发了一篇关于什么的论文.当需要回溯的时候,每篇论文能给出个大概,不具体,找起来也麻烦,以后就在这个 List 里分类整理已经读过的论 ...

ORBSLAM3 VIO初始化

ORBSLAM3 VIO初始化相关推荐

最新文章

热门文章