Local Bundle Adjustment #

Local Bundle Adjustment (LBA) 是ORB-SLAM2中用于优化局部地图的关键步骤。它通过优化当前关键帧及其共视关键帧的位姿以及它们观测到的地图点来最小化重投影误差，从而提高局部地图的精度。

主要步骤 #

1. 将当前关键帧及其共视关键帧的pose作为顶点(vertex)加入优化器 #

遍历当前关键帧及其共视关键帧
使用g2o::VertexSE3Expmap表示位姿顶点，内部使用李代数表示SE3
调用Converter::toSE3Quat将马氏距矩阵转换为四元数+平移向量的格式
设置顶点的id，以及是否固定(fix)该顶点

初始关键帧

在 Local BA 中,初始关键帧 (第一个关键帧) 的位姿是固定的,不会被优化。因为整个 SLAM 系统需要一个世界坐标系的基准,通常选择初始关键帧的相机坐标系作为世界坐标系。

// Set Local KeyFrame vertices
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
    KeyFrame* pKFi = *lit;
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    ...
    vSE3->setFixed(pKFi->mnId==0); // 初始关键帧 id 为 0,需要固定
    optimizer.addVertex(vSE3);
    ...
}

固定关键帧 (Fixed Keyframes) 除了参考关键帧，Local BA 还会固定一些关键帧不优化，它们被称为 Fixed Keyframes。Fixed Keyframes 是指能观测到 local map points，但不属于 local keyframes 的那些关键帧。它们的位姿在 Local BA 过程中是固定的，不会被优化。

// Set Fixed KeyFrame vertices
for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
{
    KeyFrame* pKFi = *lit;
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    ...
    vSE3->setFixed(true); // 设置为固定，不优化
    optimizer.addVertex(vSE3);
    ...
}

2. 添加MapPoint作为顶点 #

除了共视关键帧，LocalBA还会把MapPoint也作为顶点加入到优化中。步骤如下:

对于每个MapPoint，构造一个g2o::VertexSBAPointXYZ表示3D点的顶点
使用pMP->GetWorldPos()获取3D点坐标，并转换成g2o::Vector3d类型
顶点的id为对应的MapPoint id + maxKFid + 1 4. 将该顶点设置为边缘化的，因为MapPoint不是主要优化对象，优化中会被边缘化掉以保持稀疏性
将顶点加入优化器

这一步相当于告诉优化器除了关键帧位姿，3D点的坐标也是需要优化的。

3. 添加边(edge) #

接下来需要构建误差函数，即边。对于每一个地图点:

获取该地图点的观测关键帧及其索引
对每一个观测关键帧:
- 如果是单目:
  - 构建g2o::EdgeSE3ProjectXYZ边
  - 设置该边连接的地图点顶点和关键帧顶点
  - 设置测量值为归一化平面坐标
  - 设置信息矩阵，即协方差的逆
  - 设置相机内参
  - 添加鲁棒核函数
- 如果是双目:
  - 构建g2o::EdgeStereoSE3ProjectXYZ边
  - 设置该边连接的地图点顶点和关键帧顶点
  - 设置测量值为归一化平面坐标(左目)和视差
  - 设置信息矩阵，即协方差的逆
  - 设置相机内参
  - 添加鲁棒核函数
将构建好的边加入优化器

4. 启动优化 #

// 启动优化
optimizer.initializeOptimization();
optimizer.optimize(5); 

// 剔除外点后再次优化
optimizer.initializeOptimization(0);
optimizer.optimize(10);

这里先进行5次优化，然后剔除外点后再进行10次优化，提高精度。

5. 剔除外点 #

for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
    g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
    MapPoint* pMP = vpMapPointEdgeMono[i];

    if(pMP->isBad())
        continue;

    if(e->chi2()>5.991 || !e->isDepthPositive())
    {
        KeyFrame* pKFi = vpEdgeKFMono[i];
        vToErase.push_back(make_pair(pKFi,pMP));
    }
}

遍历每一条边，根据其chi2误差和深度值来判断是否为外点。如果是外点，就从关键帧中删除该地图点的观测。

6. 优化结果写回 #

//Keyframes
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
    KeyFrame* pKF = *lit;
    g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
    g2o::SE3Quat SE3quat = vSE3->estimate();
    pKF->SetPose(Converter::toCvMat(SE3quat));
}

//Points
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
    MapPoint* pMP = *lit;
    g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
    pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
    pMP->UpdateNormalAndDepth();
}