IMU 预积分： 见后续博客
参考论文：
- Inertial-Only Optimization for Visual-Inertial Initialization
- ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
根据ORB-SLAM3的论文介绍， IMU的初始化基于以下观察：
- Pure monocular SLAM can provide very accurate initial maps, whose main problem is that scale is unknown.Solving first the vision-only problem will enhance IMU initialization. 纯视觉SLAM可以提供很好的位姿估计，所以可以用纯视觉的结果来改善IMU的估计；
- Scale converges much faster when it is explicitly represented as an optimization variable, instead
of using the implicit representation of BA. 显示地将尺度表示为一个优化的变量， 尺度会更快地收敛 - Ignoring sensor uncertainties during IMU initialization produces large unpredictable errors.在IMU初始化阶段，忽略传感器的不确定度将会产生更多不可预知错误。
整个初始化过程分为：
- Vision-only MAP Estimation；
- Inertial-only MAP Estimation；
- Visual-Inertial MAP Estimation。
- To improve the initial estimation, visual-inertial BA is performed 5 and 15
seconds after initialization
初始化主要作用是估计imu的bias， 恢复单目的尺度， 对齐到惯性系。
IMU的初始化是在local mapping结束后， 且有足够多的关键帧（10帧）的情况下进行，此阶段对应 “Vision-only MAP Estimation”。此时已得到基于纯视觉得到的相机位姿，通过转换可以得到IMU位姿， 初始化以相机(或者IMU)位姿为参考， 去估计IMU的相关参数(主要是IMU bias、尺度以及惯性系到首帧pose的旋转变换)。
if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial)
{
if (mbMonocular)
InitializeIMU(1e2, 1e10, true);
else
InitializeIMU(1e2, 1e5, true);
}
注意此处的1e2等数字， 是IMU bias先验置信度的一个衡量， 该值越大， 优化时bias更倾向于0，属于hardcode， 不知道作者为何选择这些数字，笔者已在github提出相关issue， 静候回复！
下面来看看具体源码：
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.push_front(pKF);
vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());
if(vpKF.size()<nMinKF) //关键帧数量限制
return;
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
// 通过视觉估计的结果计算对应关键帧时刻IMU的速度
// 假设初始时刻IMU系统的加速度较小， IMU测量主要就是重力， 通过重力在首帧的测量值dirG以及在惯性系的真实值[0,0,-9.81]可以得到惯性系到首帧IMU系得旋转矩阵mRwg。
if (!mpCurrentKeyFrame->GetMap()->isImuInitialized())
{
cv::Mat cvRwg;
cv::Mat dirG = cv::Mat::zeros(3,1,CV_32F);
for(vector<KeyFrame*>::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++)
{
if (!(*itKF)->mpImuPreintegrated)
continue;
if (!(*itKF)->mPrevKF)
continue;
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);
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;
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;
std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
//进行初始化， 以IMU位姿为基准， 估计IMU bias， 尺度和惯性系与世界系（首帧IMU系）之间的旋转矩阵
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();
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)
{
//恢复尺度， 并将世界系(首帧IMU系)对齐到惯性系
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;
}
std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
if (bFIBA)
{
// 来一次有IMU预积分约束的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
mpTracker->UpdateFrameIMU(1.0,vpKF[0]->GetImuBias(),mpCurrentKeyFrame); //更新tracker端维护的local map
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(); //清空缓存区，why？
mpTracker->mState=Tracking::OK;
bInitializing = false;
mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex();
return;
}
注意： 在FullInertialBA前后都会进行尺度回复和bias更新，但重力对齐只在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)
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) //初始化的时候bFixedVel为false
VA->setFixed(true);
else
VA->setFixed(false);
optimizer.addVertex(VA);
// prior acc and gyro 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位姿固定不变， 但速度会被优化， 此处对应论文2中的Inertial-only MAP Estimation。
另外在初始化成功以后，在5s后和15秒后分别再进行初始化， 同时对于单目而言，在初始化成功后再不断地优化scale和重力方向, 以使得初始化结果更佳。这应该是从工程经验得来的。
if ((mTinit<100.0f) && mbInertial)
{
if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called
{
if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){
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){
else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){
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
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;
}
}
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;
}
与论文1不同之处有： 无判断此次优化结果是否合理;
scale的初始化采用论文2中的方法， 初始值设为1，这个值在第一次初始化成功后100s内会不断被优化， 而不是如论文1里那样， 尝试多个初始化的尺度，最后选择残差最小的作为最终的结果， 个人觉得这种做法好处就是更利于实时性。
4602
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
