ORB_SLAM3_优化方法 InertialBA

FullInertialBA

FullInertialBA的主要作用是结合视觉和IMU信息进行联合优化，主要作用在LocalMapping中的initializeIMU和LoopClosing中的RunGlobalBundleAdjustment

• 在初始化阶段，bias固定不变，通过先验，约束bias为一个微小量

输入

参数	说明
pMap	地图

初始化

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

设置vertex

对于地图中的关键帧

• VertexPose

VertexPose *VP = new VertexPose(pKFi);
VP->setId(pKFi->mnId);
pIncKF = pKFi;
bool bFixed = false;
if (bFixLocal)
{
   
	bFixed = (pKFi->mnBALocalForKF >= (maxKFid - 1)) || (pKFi->mnBAFixedForKF >= (maxKFid - 1));
	if (!bFixed)
		nNonFixed++;
	VP->setFixed(bFixed);
}
optimizer.addVertex(VP);

• VertexVelocity
  需要该地图完成IMU初始化后才添加，也就是关键帧的bImu为true

VertexVelocity *VV = new VertexVelocity(pKFi);
VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
VV->setFixed(bFixed);
optimizer.addVertex(VV);

• VertexGyroBias
  • 当有先验信息时，只添加最后一帧关键帧的bias
  • 当没有先验信息时，每一帧关键帧都添加bias

VertexGyroBias *VG = new VertexGyroBias(pKFi);
VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
VG->setFixed(bFixed);
optimizer.addVertex(VG);

• VertexAccBias
  • 当有先验信息时，只添加最后一帧关键帧的bias
  • 当没有先验信息时，每一帧关键帧都添加bias

VertexAccBias *VA = new VertexAccBias(pKFi);
VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
VA->setFixed(bFixed);
optimizer.addVertex(VA);

对于地图中的地图点

• VertexSBAPointXYZ

MapPoint *pMP = vpMPs[i];
g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(pMP->GetWorldPos().cast<double>());
unsigned long id = pMP->mnId + iniMPid + 1;
vPoint->setId(id);
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);

设置edge

添加IMU边

• EdgeInertial
  • 当有先验信息时，边的两个Vertex：VG1和VA1为最后一帧关键帧的bias，同时添加先验边EdgePriorAcc和EdgePriorGyro
  • 当没有先验信息时，边的两个Vertex：VG1和VA1为前一关键帧的bias，同时在前后两帧之间添加边EdgeGyroRW]和EdgeAccRW

// 3.1 根据上一帧的偏置设定当前帧的新偏置
pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
// 3.2 提取节点
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;
// 根据不同输入配置相应的偏置节点
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);

// 3.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);
// 9个自由度的卡方检验（0.05）
rki->setDelta(sqrt(16.92));

optimizer.addEdge(ei);

• EdgeGyroRW

EdgeGyroRW *egr = new E