[数据结构与算法]树与图以及优化

欢迎访问我的博客首页。

1. 关键帧

??阅读 ORB_SLAM 的源码最好从 KeyFrame.cc 开始。因为 KeyFrame.cc 中定义了一些很重要的数据结构，比如共视图、生成树。同时，KeyFrame.cc 中的函数几乎不会调用其它文件中的函数，而里程计、回环检测、回环闭合、优化等经常调用 KeyFrame.cc 中的函数，所以 KeyFrame.cc 即简单又重要。

1.1 相机坐标系原点在世界坐标系中的坐标

??已知相机坐标到世界坐标的变换是 $R_{wc}$ 和 $t_{wc}$，求相机坐标系原点 $O_c$ 在世界坐标系中的坐标 $O_c^w$：假设相机坐标系原点 $O_c$ 在相机坐标系中的坐标是 $O_c^c$，由 $R_{wc}{O}_c^c+t_{wc}=O_c^w$ 知

$$O_c^w=t_{wc}.\text{\hspace{1em}(1.1)}$$

??已知世界坐标到相机坐标的变换是 $R_{cw}$ 和 $t_{cw}$，求相机坐标系原点 $O_c$ 在世界坐标系中的坐标 $O_c^w$：假设相机坐标系原点 $O_c$ 在世界坐标系中的坐标是 $O_c^w$，由 $R_{cw}{O}_c^w+t_{cw}=O_c^c$ 知

$$O_c^w=?R_{wc}{t}_{cw}.\text{\hspace{1em}(1.2)}$$

??上面两个公式的推导中，很明显 $O_c^c=0$。由公式 (1.1) 和公式 (1.2) 还可以知道 $t_{wc}=?R_{wc}{t}_{cw}$。

1.2 共视图

??有共同地图点的两个关键帧相连，组成共视图。共视图的创建过程：

void KeyFrame::UpdateConnections();
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight);
void KeyFrame::UpdateBestCovisibles();

??有共同地图点的两个关键帧只称为有连接关系，共同地图点数大于等于 15 的两个关键词才有共视关系：

set<KeyFrame *> KeyFrame::GetConnectedKeyFrames() {
    // 与当前关键帧至少有一个共同地图点。
    unique_lock<mutex> lock(mMutexConnections);
    set<KeyFrame *> s;
    for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(); mit != mConnectedKeyFrameWeights.end(); mit++)
        s.insert(mit->first);
    return s;
}

vector<KeyFrame *> KeyFrame::GetVectorCovisibleKeyFrames() {
    // 与当前关键帧至少有 15 个共同地图点。
    unique_lock<mutex> lock(mMutexConnections);
    return mvpOrderedConnectedKeyFrames;
}

1.3 生成树

??一个连通无向图 G 的生成树 T 是 G 的一个极小连通子图，T 含有 G 中的全部 n 个顶点，但只有足以构成一棵树的 n - 1 条边。

??与该关键帧的共同地图点数量最多的关键帧，被选为该关键帧的父结点。生成树的权重就是共同地图点的数量。生成树的创建过程：

void KeyFrame::UpdateConnections() {
    // ...
    // 如果当前关键帧没有爹（mbFirstConnection == true），为它找个爹。
    if (mbFirstConnection && mnId != 0) {
        mpParent = mvpOrderedConnectedKeyFrames.front();
        mpParent->AddChild(this);
        mbFirstConnection = false;
    }
}

1.4 本质图