(01)ORB-SLAM2源码无死角解析-(66) BA优化(g2o)→闭环线程:Optimizer::GlobalBundleAdjustemnt→全局优化

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过上一篇博客对闭环线程中的Optimizer::OptimizeEssentialGraph→本质图优化进行了详细讲解。今天主要讲解ORB-SLAM2中的最后一个BA优化(g2o)，即函数Optimizer::GlobalBundleAdjustemnt→全局优化。该函数在两个地方有被调用：①src/Tracking.cc 文件中的Tracking::CreateInitialMapMonocular() 函数；②src/LoopClosing.cc 中的 LoopClosing::RunGlobalBundleAdjustment() 函数。

另外 LoopClosing::RunGlobalBundleAdjustment() 函数被 LoopClosing::CorrectLoop() 调用，且还是通过新启一个线程的方式，调用关系如下：

void LoopClosing::Run()
	void LoopClosing::CorrectLoop()
		mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
			Optimizer::GlobalBundleAdjustemnt(mpMap,        // 地图点对象
                                      10,           // 迭代次数
                                      &mbStopGBA,   // 外界控制 GBA 停止的标志
                                      nLoopKF,      // 形成了闭环的当前关键帧的id
                                      false);   
1
2
3
4
5
6
7
8

对于LoopClosing::RunGlobalBundleAdjustment函数而样，第一个执行的就是Optimizer::GlobalBundleAdjustemnt()函数，所以下面先对 GBA(GlobalBundleAdjustemnt)进行讲解，后续也把GlobalBundleAdjustemnt函数简称为GBA。
 

二、GlobalBundleAdjustemnt

对于该函数，其实现于 src/Optimizer.cc 中，代码十分简单，如下所示：

/**
 * @brief 全局BA： pMap中所有的MapPoints和关键帧做bundle adjustment优化
 * 这个全局BA优化在本程序中有两个地方使用：
 * 1、单目初始化：CreateInitialMapMonocular函数
 * 2、闭环优化：RunGlobalBundleAdjustment函数
 * @param[in] pMap                  地图点
 * @param[in] nIterations           迭代次数
 * @param[in] pbStopFlag            外部控制BA结束标志
 * @param[in] nLoopKF               形成了闭环的当前关键帧的id
 * @param[in] bRobust               是否使用鲁棒核函数
 */
void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
    // 获取地图中的所有关键帧
    vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
    // 获取地图中的所有地图点
    vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
    // 调用GBA
    BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

其逻辑可以说是简单至极，就是获得地图中的所有关键帧，及所有地图点，然后传入到 BundleAdjustment() 函数中进行优化。那么很显然，该函数就是整个全局优化的核心了，那么下面就对齐进行详细讲解吧，代码位于 src/Optimizer 文件中。整体参数如下：

/**
 * @brief bundle adjustment 优化过程
 * 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
 *            g2o::VertexSBAPointXYZ()，MapPoint的mWorldPos
 * 2. Edge:
 *     - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：待优化当前帧的Tcw
 *         + Vertex：待优化MapPoint的mWorldPos
 *         + measurement：MapPoint在当前帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 * 
 * @param[in] vpKFs                 参与BA的所有关键帧
 * @param[in] vpMP                  参与BA的所有地图点
 * @param[in] nIterations           优化迭代次数
 * @param[in] pbStopFlag            外部控制BA结束标志
 * @param[in] nLoopKF               形成了闭环的当前关键帧的id
 * @param[in] bRobust               是否使用核函数
 */
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

对于上述中的顶点与边，在前面的博客都进行过详细的讲解，所以这里就不再重复啰嗦。

1、源码逻辑

$(1):$ 初始化g2o优化器，使用LM算法优化。根据 pbStopFlag 标志判断外部是否请求终止，如果是，那就结束。如果此时没有收到请请求，那么后续外部再请求结束BA，就结束不了了。

$(2):$ 向优化器添加顶点，顶点主要包含→①所有关键帧Sim3位姿(g2o::VertexSE3Expmap);②地图中的所有地图点位姿(g2o::VertexSBAPointXYZ)。$需要注意\to$在添加第0帧关键帧为顶点的时候，调用了vSE3->setFixed(ture)。也就是说有第0帧关键帧不优化(仅第0帧)，其作为参考基准。
        这里需要与前面的本质图优化Optimizer::OptimizeEssentialGraph作一个对比，在本质图优化中，其锁定的是与当前帧匹配上回环候选帧，并没有锁定第0帧，也就是说第0帧参与了优化。

$(3):$ 把每个地图作为顶点添加到优化器之后，获得能够观测到该地图点的所有关键帧 observations，循环遍历所有观察到当前地图点的所有关键帧记为 pKF，跳过不合法的关键帧。取出该地图点对应该关键帧的2D特征点kpUn，作为观测值(认为其正确)。然后地图点与关键帧之间建立 g2o::EdgeSE3ProjectXYZ 边。

$(4):$ 开始优化，进行迭代。迭代完成之后遍历所有关键帧，获取到优化后的位姿，把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用。 遍历所有地图点，去除其中没有参与优化过程的地图点(可能是随着位姿的不断优化，观测不到该地图点了)。

2、源码注释

BundleAdjustment 函数位于 src/Optimizer.cc 文件中，代码注释如下：

/**
 * @brief bundle adjustment 优化过程
 * 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
 *            g2o::VertexSBAPointXYZ()，MapPoint的mWorldPos
 * 2. Edge:
 *     - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：待优化当前帧的Tcw
 *         + Vertex：待优化MapPoint的mWorldPos
 *         + measurement：MapPoint在当前帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 * 
 * @param[in] vpKFs                 参与BA的所有关键帧
 * @param[in] vpMP                  参与BA的所有地图点
 * @param[in] nIterations           优化迭代次数
 * @param[in] pbStopFlag            外部控制BA结束标志
 * @param[in] nLoopKF               形成了闭环的当前关键帧的id
 * @param[in] bRobust               是否使用核函数
 */
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
                                 int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
    // 不参与优化的地图点
    vector<bool> vbNotIncludedMP;
    vbNotIncludedMP.resize(vpMP.size());

    // Step 1 初始化g2o优化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    // 使用LM算法优化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    // 如果这个时候外部请求终止，那就结束
    // 注意这句执行之后，外部再请求结束BA，就结束不了了
    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    // 记录添加到优化器中的顶点的最大关键帧id
    long unsigned int maxKFid = 0;

    // Step 2 向优化器添加顶点

    // Set KeyFrame vertices
    // Step 2.1 ：向优化器添加关键帧位姿顶点
    // 遍历当前地图中的所有关键帧
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        // 跳过无效关键帧
        if(pKF->isBad())
            continue;
        
        // 对于每一个能用的关键帧构造SE3顶点,其实就是当前关键帧的位姿
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
        // 顶点的id就是关键帧在所有关键帧中的id
        vSE3->setId(pKF->mnId); 
        // 只有第0帧关键帧不优化（参考基准）
        vSE3->setFixed(pKF->mnId==0);
        
        // 向优化器中添加顶点，并且更新maxKFid
        optimizer.addVertex(vSE3);
        if(pKF->mnId>maxKFid)
            maxKFid=pKF->mnId;
    }

    // 卡方分布 95% 以上可信度的时候的阈值
    const float thHuber2D = sqrt(5.99);     // 自由度为2
    const float thHuber3D = sqrt(7.815);    // 自由度为3

    // Set MapPoint vertices
    // Step 2.2：向优化器添加地图点作为顶点
    // 遍历地图中的所有地图点
    for(size_t i=0; i<vpMP.size(); i++)
    {
        MapPoint* pMP = vpMP[i];
        // 跳过无效地图点
        if(pMP->isBad())
            continue;

        // 创建顶点
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        // 注意由于地图点的位置是使用cv::Mat数据类型表示的,这里需要转换成为Eigen::Vector3d类型
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        // 前面记录maxKFid 是在这里使用的
        const int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        // 注意g2o在做BA的优化时必须将其所有地图点全部schur掉，否则会出错。
        // 原因是使用了g2o::LinearSolver这个类型来指定linearsolver,
        // 其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。
        // Ceres库则没有这样的限制
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

        // 取出地图点和关键帧之间观测的关系
        const map<KeyFrame*,size_t> observations = pMP->GetObservations();

        // 边计数
        int nEdges = 0;
        //SET EDGES
        // Step 3：向优化器添加投影边（是在遍历地图点、添加地图点的顶点的时候顺便添加的）
        // 遍历观察到当前地图点的所有关键帧
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
        {

            KeyFrame* pKF = mit->first;
            // 跳过不合法的关键帧
            if(pKF->isBad() || pKF->mnId>maxKFid)
                continue;

            nEdges++;
            // 取出该地图点对应该关键帧的2D特征点
            const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];

            if(pKF->mvuRight[mit->second]<0)
            {
                // 以下是单目相机模式：
                // 构造观测
                Eigen::Matrix<double,2,1> obs;
                obs << kpUn.pt.x, kpUn.pt.y;

                // 创建边
                g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
                // 边连接的第0号顶点对应的是第id个地图点
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                // 边连接的第1号顶点对应的是第id个关键帧
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                // 信息矩阵，也是协方差，表明了这个约束的观测在各个维度（x,y）上的可信程度，在我们这里对于具体的一个点，两个坐标的可信程度都是相同的，
                // 其可信程度受到特征点在图像金字塔中的图层有关，图层越高，可信度越差
                // 为了避免出现信息矩阵中元素为负数的情况，这里使用的是sigma^(-2)
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
                // 使用鲁棒核函数
                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    // 这里的重投影误差，自由度为2，所以这里设置为卡方分布中自由度为2的阈值，如果重投影的误差大约大于1个像素的时候，就认为不太靠谱的点了，
                    // 核函数是为了避免其误差的平方项出现数值上过大的增长
                    rk->setDelta(thHuber2D);
                }

                // 设置相机内参
                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;
                // 添加边
                optimizer.addEdge(e);
            }
            else
            {
                // 双目或RGBD相机按照下面操作
                // 双目相机的观测数据则是由三个部分组成：投影点的x坐标，投影点的y坐标，以及投影点在右目中的x坐标（默认y方向上已经对齐了）
                Eigen::Matrix<double,3,1> obs;
                const float kp_ur = pKF->mvuRight[mit->second];
                obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                // 对于双目输入，g2o也有专门的误差边
                g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
                // 填充
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                // 信息矩阵这里是相同的，考虑的是左目特征点的所在图层
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                e->setInformation(Info);

                // 如果要使用鲁棒核函数
                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    // 由于现在的观测有三个值，重投影误差会有三个平方项的和组成，因此对应的卡方分布的自由度为3，所以这里设置的也是自由度为3的时候的阈值
                    rk->setDelta(thHuber3D);
                }

                // 填充相机的基本参数
                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;
                e->bf = pKF->mbf;

                optimizer.addEdge(e);
            }  
        } // 向优化器添加投影边,也就是遍历所有观测到当前地图点的关键帧

        // 如果因为一些特殊原因,实际上并没有任何关键帧观测到当前的这个地图点,那么就删除掉这个顶点,并且这个地图点也就不参与优化
        if(nEdges==0)
        {
            optimizer.removeVertex(vPoint);
            vbNotIncludedMP[i]=true;
        }
        else
        {
            vbNotIncludedMP[i]=false;
        }
    }

    // Optimize!
    // Step 4：开始优化
    optimizer.initializeOptimization();
    optimizer.optimize(nIterations);

    // Recover optimized data
    // Step 5：得到优化的结果

    // Step 5.1 遍历所有的关键帧
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;

        // 获取到优化后的位姿
        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        if(nLoopKF==0)
        {
            // 原则上来讲不会出现"当前闭环关键帧是第0帧"的情况,如果这种情况出现,只能够说明是在创建初始地图点的时候调用的这个全局BA函数.
            // 这个时候,地图中就只有两个关键帧,其中优化后的位姿数据可以直接写入到帧的成员变量中
            pKF->SetPose(Converter::toCvMat(SE3quat));
        }
        else
        {
            // 正常的操作,先把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用
            pKF->mTcwGBA.create(4,4,CV_32F);
            Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
            pKF->mnBAGlobalForKF = nLoopKF;
        }
    }

    // Step 5.2 Points
    // 遍历所有地图点,去除其中没有参与优化过程的地图点
    for(size_t i=0; i<vpMP.size(); i++)
    {
        if(vbNotIncludedMP[i])
            continue;

        MapPoint* pMP = vpMP[i];

        if(pMP->isBad())
            continue;

        // 获取优化之后的地图点的位置
        g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));

        // 和上面对关键帧的操作一样
        if(nLoopKF==0)  
        {
            // 如果这个GBA是在创建初始地图的时候调用的话,那么地图点的位姿也可以直接写入
            pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
            pMP->UpdateNormalAndDepth();
        }
        else
        {
            // 反之,如果是正常的闭环过程调用,就先临时保存一下
            pMP->mPosGBA.create(3,1,CV_32F);
            Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
            pMP->mnBAGlobalForKF = nLoopKF;
        }// 判断是因为什么原因调用的GBA
    } // 遍历所有地图点,保存优化之后地图点的位姿
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268

 

三、RunGlobalBundleAdjustment

现在回过头来对 LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) 函数进行讲解，其传入的参数 nLoopKF 看上去是闭环关键帧id,但是在调用的时候给的其实是当前关键帧的id。

在前面提到 LoopClosing::RunGlobalBundleAdjustment() 函数被 LoopClosing::CorrectLoop() 调用，且还是通过新启一个线程的方式。那么也就是说 RunGlobalBundleAdjustment() 可能被多个线程执行，在global BA过程中local mapping线程仍然在工作，这意味着在global BA时可能有新的关键帧产生，但是并未包括在GBA里，
所以和更新后的地图并不连续。需要通过spanning tree来传播。

1、代码逻辑

$(1):$ 传入的参数nLoopKF 看上去是闭环关键帧id,但是在调用的时候给的其实是当前关键帧的id，首先调用 Optimizer::GlobalBundleAdjustemnt() 函数，执行全局BA，优化所有的关键帧位姿和地图中地图点。

$(2):$ 在global BA过程中local mapping线程仍然在工作，这意味着在global BA时可能有新的关键帧产生，但是并未包括在GBA里，所以和更新后的地图并不连续。需要通过spanning tree来传播。

KeyFrame*    mpParent;　父亲
std::set<KeyFrame*>   mspChildrens;　儿子，可能有多个　　　　　　　
1
2

这些指针把的KF组织起来成为一个树形结构，称为 spanning tree。如果全局BA过程是因为意外结束的,那么直接退出GBA
 
$(3):$ 如果当前GBA没有中断请求，则开始更新位姿和地图点。请求局部建图线程停止，等待直到local mapping 结束才会继续后续操作，对地图进行更行之后先上锁。创建list lpKFtoCheck，刚开始只保存了初始化第0个关键帧

$(4):$ 在GBA→Optimizer::GlobalBundleAdjustemnt中，GBA里锁住第一个关键帧位姿没有优化(前面已经讲解，且字体标红)。问：GBA里锁住第一个关键帧位姿没有优化，其对应的pKF->mTcwGBA是不变的吧？那后面调整位姿的意义何在？回答：注意在前面essential graph BA里只锁住了回环帧，没有锁定第0个初始化关键帧位姿。所以第0个初始化关键帧位姿已经更新了 在GBA里锁住第一个关键帧位姿没有优化，其对应的pKF->mTcwGBA应该是essential BA结果，在这里统一更新了。

$(5):$ 通过lpKFtoCheck获得初始化的第0个关键帧的所有子关键帧，先获得父关键帧到当前子关键帧的位姿变换 Tchildc = pChild->GetPose()$\ast$Twc(未优化)，再利用优化后的父关键帧的位姿，转换到世界坐标系下，相当于更新了子关键帧的位姿 pChild->mTcwGBA = Tchildc$\ast$pKF->mTcwGBA。

$(6):$ 然后把所有子关键帧添加到 lpKFtoCheck 中，作为父关键帧，一直这样循环下去。这样最小生成树除了根节点(第0关键帧)，其他的节点都会作为其他关键帧的子节点，这样做可以使得最终所有的关键帧都得到了优化。

$(7):$ 遍历每一个地图点并用更新的关键帧位姿来更新地图点位置→①如果这个地图点直接参与到了全局BA优化的过程,那么就直接重新设置器位姿即可。②如这个地图点并没有直接参与到全局BA优化的过程中,那么就使用其参考关键帧的新位姿来优化自己的坐标。简而言之，把地图世界坐标系下的位姿转换到其参考关键帧相机坐标系下的坐标，然后使用已经纠正过的参考关键帧的位姿,再将该地图点变换到世界坐标系下，就完成了优化。
 

2、源码注释

/**
 * @brief 全局BA线程,这个是这个线程的主函数
 * 
 * @param[in] nLoopKF 看上去是闭环关键帧id,但是在调用的时候给的其实是当前关键帧的id
 */
void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
{
    cout << "Starting Global Bundle Adjustment" << endl;

    // 记录GBA已经迭代次数,用来检查全局BA过程是否是因为意外结束的
    int idx =  mnFullBAIdx;
    // mbStopGBA直接传引用过去了,这样当有外部请求的时候这个优化函数能够及时响应并且结束掉
    // 提问:进行完这个过程后我们能够获得哪些信息?
    // 回答：能够得到全部关键帧优化后的位姿,以及优化后的地图点

    // Step 1 执行全局BA，优化所有的关键帧位姿和地图中地图点
    Optimizer::GlobalBundleAdjustemnt(mpMap,        // 地图点对象
                                      10,           // 迭代次数
                                      &mbStopGBA,   // 外界控制 GBA 停止的标志
                                      nLoopKF,      // 形成了闭环的当前关键帧的id
                                      false);       // 不使用鲁棒核函数

    // Update all MapPoints and KeyFrames
    // Local Mapping was active during BA, that means that there might be new keyframes
    // not included in the Global BA and they are not consistent with the updated map.
    // We need to propagate the correction through the spanning tree
    // 更新所有的地图点和关键帧
    // 在global BA过程中local mapping线程仍然在工作，这意味着在global BA时可能有新的关键帧产生，但是并未包括在GBA里，
    // 所以和更新后的地图并不连续。需要通过spanning tree来传播
    {
        unique_lock<mutex> lock(mMutexGBA);
        // 如果全局BA过程是因为意外结束的,那么直接退出GBA
        if(idx!=mnFullBAIdx)
            return;

        // 如果当前GBA没有中断请求，更新位姿和地图点
        // 这里和上面那句话的功能还有些不同,因为如果一次全局优化被中断,往往意味又要重新开启一个新的全局BA;为了中断当前正在执行的优化过程mbStopGBA将会被置位,同时会有一定的时间
        // 使得该线程进行响应;而在开启一个新的全局优化进程之前 mbStopGBA 将会被置为False
        // 因此,如果被强行中断的线程退出时已经有新的线程启动了,mbStopGBA=false,为了避免进行后面的程序,所以有了上面的程序;
        // 而如果被强行中断的线程退出时新的线程还没有启动,那么上面的条件就不起作用了(虽然概率很小,前面的程序中mbStopGBA置位后很快mnFullBAIdx就++了,保险起见),所以这里要再判断一次
        if(!mbStopGBA)
        {
            cout << "Global Bundle Adjustment finished" << endl;
            cout << "Updating map ..." << endl;
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            // 等待直到local mapping结束才会继续后续操作
            while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished())
            {
				//usleep(1000);
				std::this_thread::sleep_for(std::chrono::milliseconds(1));
			}

            // Get Map Mutex
            // 后续要更新地图所以要上锁
            unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

            // Correct keyframes starting at map first keyframe
            // 从第0个关键帧开始矫正关键帧。刚开始只保存了初始化第0个关键帧
            list<KeyFrame*> lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end());

            // 问：GBA里锁住第一个关键帧位姿没有优化，其对应的pKF->mTcwGBA是不变的吧？那后面调整位姿的意义何在？
            // 回答：注意在前面essential graph BA里只锁住了回环帧，没有锁定第0个初始化关键帧位姿。所以第0个初始化关键帧位姿已经更新了
            // 在GBA里锁住第一个关键帧位姿没有优化，其对应的pKF->mTcwGBA应该是essential BA结果，在这里统一更新了
            // Step 2 遍历并更新全局地图中的所有spanning tree中的关键帧
            while(!lpKFtoCheck.empty())
            {
                KeyFrame* pKF = lpKFtoCheck.front();
                const set<KeyFrame*> sChilds = pKF->GetChilds();
                cv::Mat Twc = pKF->GetPoseInverse();
                // 遍历当前关键帧的子关键帧
                for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
                {
                    KeyFrame* pChild = *sit;
                    // 记录避免重复
                    if(pChild->mnBAGlobalForKF!=nLoopKF)
                    {
                        // 从父关键帧到当前子关键帧的位姿变换 T_child_farther
                        cv::Mat Tchildc = pChild->GetPose()*Twc;
                        // 再利用优化后的父关键帧的位姿，转换到世界坐标系下，相当于更新了子关键帧的位姿
                        // 这种最小生成树中除了根节点，其他的节点都会作为其他关键帧的子节点，这样做可以使得最终所有的关键帧都得到了优化
                        pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;
                        // 做个标记，避免重复
                        pChild->mnBAGlobalForKF=nLoopKF;

                    }
                    lpKFtoCheck.push_back(pChild);
                }
                // 记录未矫正的关键帧的位姿
                pKF->mTcwBefGBA = pKF->GetPose();
                // 记录已经矫正的关键帧的位姿
                pKF->SetPose(pKF->mTcwGBA);
                // 从列表中移除
                lpKFtoCheck.pop_front();
            }

            // Correct MapPoints
            const vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();

            // Step 3 遍历每一个地图点并用更新的关键帧位姿来更新地图点位置
            for(size_t i=0; i<vpMPs.size(); i++)
            {
                MapPoint* pMP = vpMPs[i];

                if(pMP->isBad())
                    continue;

                // 如果这个地图点直接参与到了全局BA优化的过程,那么就直接重新设置器位姿即可
                if(pMP->mnBAGlobalForKF==nLoopKF)
                {
                    // If optimized by Global BA, just update
                    pMP->SetWorldPos(pMP->mPosGBA);
                }
                else 
                {
                    // 如这个地图点并没有直接参与到全局BA优化的过程中,那么就使用其参考关键帧的新位姿来优化自己的坐标
                    // Update according to the correction of its reference keyframe
                    KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();

                    // 如果参考关键帧并没有经过此次全局BA优化，就跳过 
                    if(pRefKF->mnBAGlobalForKF!=nLoopKF)
                        continue;

                    // Map to non-corrected camera
                    cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
                    cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
                    // 转换到其参考关键帧相机坐标系下的坐标
                    cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;

                    // Backproject using corrected camera
                    // 然后使用已经纠正过的参考关键帧的位姿,再将该地图点变换到世界坐标系下
                    cv::Mat Twc = pRefKF->GetPoseInverse();
                    cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
                    cv::Mat twc = Twc.rowRange(0,3).col(3);

                    pMP->SetWorldPos(Rwc*Xc+twc);
                }
            }

            // 释放
            mpLocalMapper->Release();

            cout << "Map updated!" << endl;
        }

        mbFinishedGBA = true;
        mbRunningGBA = false;
    } 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150

 

四、结语

到目前为止，可以说关于 ORB-SLAM2 的核心部分全部都讲解完成了，建档梳理一下，我们已经讲解了如下内容：

1、图像金字塔_ORB特征点、ORBextractor、地图初始化→单目初始化
2、SVD奇异值分解、求解Homography,Fundamental、重投影误差,卡方检验
3、分解Homography,恢复Rt、基本本质矩阵Essential 分解恢复 Rt
4、特征点三角化、单目SFM地图初始化、尺度不确定性、共视图、本质图、拓展图
5、双目Stereo相机立体匹配,SAD算法、ORB特征匹配、
6、参考关键帧追踪TrackReferenceKeyFrame()、恒速模型跟踪当前普通帧TrackWithMotionModel()、重定位跟踪 Relocalization()
7、EPnP算法、局部地图跟踪TrackLocalMap()、SearchInNeighbors():融合重复地图点
8、计算Sim3、CorrectLoop→位姿传播，地图点矫正，地图点融合，共视关系更新
9、Optimizer::PoseOptimization→仅位姿优化、Optimizer::OptimizeSim3→Sim3变换优化、Optimizer::OptimizeEssentialGraph→本质图优化、Optimizer::GlobalBundleAdjustemnt→全局优化
......
1
2
3
4
5
6
7
8
9
10

其上还有很多东西没有列举，现在回顾来看，ORB-SLAM2 涉及到的东西太多了，是一个大工程。但是可以说是对其每一个细节都进行了分析。理论知识已经讲解完成了，后续还回讲解一些工程中比较实用的东西。
 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件