Part-1 ORB SLAM3初始化-1

初始化

ORB SLAM3的初始化主要是创建ORB词袋、关键帧数据库、多地图等对象，其步骤如下：

1. 检测配置文件能否打开
2. 加载ORB词袋(ORBVocabulary)
3. 创建关键帧数据库(KeyFrameDatabase)
4. 创建多地图(Atlas)
5. 创建跟踪线程(Tracking)
6. 创建局部建图线程(LocalMapping)
7. 创建闭环线程(LoopClosing)
8. 设置线程间的指针

A.跟踪线程

1. 从配置文件中读取相机参数，创建相机模型(Pinhole/KannalaBrandt8)
   • Pinhole/KannalaBrandt8： 继承自GeometricCamera，输入：相机参数（fx,fy,cx,cy/fx,fy,cx,cy,k1,k2,k3,k4）
   • 添加相机模型到多地图系统中(mpAtlas->AddCamera(mpCamera))

OpenCV 中的 `FileStorage` 类能够读写`.xml`和`.yaml`文件，以 `FileNode` 为单位存储数据，其有以下操作：
* 写入:`FileStorage::WRITE`
* 追加:`FileStorage::APPEND`
* 读取:`FileStorage::WRITE`

2. 从配置文件中读取ORB参数，创建ORB特征提取器(ORBextractor)

• nFeatures: 特征点的数量
• Scale factor：特征金字塔的尺度因子
• nLevels: 特征金子塔的层数
• iniThFAST: 初始Fast阈值
• minThFAST: 最小Fast阈值

mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
//相对于普通情况，单目初始化时提取的特征点数量更多
mpIniORBextractor = new ORBextractor(5*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

3. 从配置文件中读取IMU参数，创建IMU标定(IMU::Calib)、IMU预积分(IMU::Preintegrated)
   • Tbc：相机到IMU的变换矩阵
   • freq：IMU的频率
     • 单位：$hz$
     • 符号：$\frac{1}{\Delta t}$
   • Ng：陀螺仪白噪声
     • 单位：$\frac{\text{ rad }}{s}\frac{1}{\sqrt{H_z}}$
     • 符号：${\sigma }_g$
   • Na：加速计白噪声
     • 单位：$\frac{m}{s^2}\frac{1}{\sqrt{Hz}}$
     • 符号：${\sigma }_a$
   • Ngw：陀螺仪随机游走噪声
     • 单位：$\frac{\text{ rad }}{s^2}\frac{1}{\sqrt{Hz}}$
     • 符号：${\sigma }_{bg}$
   • Naw：加速计随机游走噪声
     • 单位：$\frac{m}{s^3}\frac{1}{\sqrt{Hz}}$
     • 符号：${\sigma }_{ba}$

高斯白噪声的方差从连续时间到离散时间需要乘以一个$\frac{1}{\sqrt{freq}}$
$$\begin{array}{c}{N}_a=N_a^d\sqrt{freq}\\ N_g=N_g^d\sqrt{freq}\end{array}$$
随机游走方差从连续时间到离散时间乘以一个 $\sqrt{freq}$
$$\begin{array}{c}{N}_{aw}=N_{aw}^d\frac{1}{\sqrt{freq}}\\ N_{gw}=N_{gw}^d\frac{1}{\sqrt{freq}}\end{array}$$
高斯白噪声协方差：
$$\left[\begin{array}{cccccc}{N}_g^2& 0& 0& 0& 0& 0\\ 0& N_g^2& 0& 0& 0& 0\\ 0& 0& N_g^2& 0& 0& 0\\ 0& 0& 0& N_a^2& 0& 0\\ 0& 0& 0& 0& N_a^2& 0\\ 0& 0& 0& 0& 0& N_a^2\end{array}\right]$$
随机游走协方差：
$$\left[\begin{array}{cccccc}{N}_{gw}^2& 0& 0& 0& 0& 0\\ 0& N_{gw}^2& 0& 0& 0& 0\\ 0& 0& N_{gw}^2& 0& 0& 0\\ 0& 0& 0& N_{aw}^2& 0& 0\\ 0& 0& 0& 0& N_{aw}^2& 0\\ 0& 0& 0& 0& 0& N_{aw}^2\end{array}\right]$$

ORB特征提取器

1. 设置图像金字塔的尺度因子、逆尺度因子、方差

mvScaleFactor.resize(nlevels);
mvInvScaleFactor.resize(nlevels);

mvLevelSigma2.resize(nlevels);
mvInvLevelSigma2.resize(nlevels);

$$\begin{array}{l}mvScalefacto{r}_i=mvScalefacto{r}_{i-1}\cdot scalefactor\\ mvInvScalefacto{r}_i=\frac{1}{mvScalefacto{r}_i}\\ mvLevelSigma{2}_i=mvScalefacto{r}_i^2\\ mvInvLevelSigma{2}_i=\frac{1}{mvLevelSigma{2}_i}\end{array}$$

2. 预分配每层金字塔的特征点数量

• 分配的方式：
  • 先分配前$n-1$层，再将剩余的特征点$N-sum(0,n-1)$分配给第n层。
• 每层金字塔期望的特征点数量

假设需要提取的特征点数量为$N$，金字塔共有$m$层，第0层图像的宽为$W$，高为$H$ ，对应的面积$H\cdot W=C$，图像金字塔缩放因子为$s(0<s<1)$，在 ORB-SLAM中 ，$m=8，s=1/1.2$。
那么，整个金字塔总面积为：
$$\begin{array}{rl}S& =H\cdot W\cdot {\left(s^2\right)}^0+H\cdot W\cdot {\left(s^2\right)}^1+\cdots +H\cdot W\cdot {\left(s^2\right)}^{(m-1)}\\ & =HW\frac{1-{\left(s^2\right)}^m}{1-s^2}=C\frac{1-{\left(s^2\right)}^m}{1-s^2}\end{array}$$
单位面积应该分配的特征点数目为：
$$N_{avg}=\frac{N}{S}=\frac{N}{C\frac{1-{\left(s^2\right)}^m}{1-s^2}}=\frac{N\left(1-s^2\right)}{C\left(1-{\left(s^2\right)}^m\right)}$$
第0层应该分配的特征点数量为：
$$N_0=\frac{N\left(1-s^2\right)}{1-{\left(s^2\right)}^m}$$
第 i 层应该分配的特征点数量为：
$$N_i=\frac{N\left(1-s^2\right)}{C\left(1-{\left(s^2\right)}^m\right)}C{\left(s^2\right)}^i=\frac{N\left(1-s^2\right)}{1-{\left(s^2\right)}^m}{\left(s^2\right)}^i$$
在ORB-SLAM3 的代码里，不是按照面积均摊的，而是按照面积的开方来均摊特征点的，也就是将上述公式中的$s^2$换成$s$ 即可。

3. 初始化pattern
   pattern为$32\cdot 8\cdot 4=1024$，其中pattern是ORB预设好的。
   参考文献：《BRIEF: Binary Robust Independent Elementary Features 》

4. 预先计算灰度质心法每行对应的终点
   1. 先算弧A->B($v\in (0,vmax)$)段
   2. 再算弧C->B($v\in (r,vmin)$)段

注意：这里算出来vmax和vmin是相等的，可以看打印出来的点中有一个红圈套篮圈

欢迎点赞、收藏、转发，本人微信公众号：火柴的初心