Part-1 ORB SLAM3初始化-1

初始化

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

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

A.跟踪线程

从配置文件中读取相机参数，创建相机模型(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`

从配置文件中读取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);

从配置文件中读取IMU参数，创建IMU标定(IMU::Calib)、IMU预积分(IMU::Preintegrated)
- Tbc：相机到IMU的变换矩阵
- freq：IMU的频率
  - 单位：hzhzhz
  - 符号：1Δt\frac{1}{\Delta t}Δt1
- Ng：陀螺仪白噪声
  - 单位：rad s1Hz\frac{\text { rad }}{s} \frac{1}{\sqrt{H_{z}}}s rad Hz1
  - 符号：σg\sigma_{g}σg
- Na：加速计白噪声
  - 单位：ms21Hz\frac{m}{s^{2}} \frac{1}{\sqrt{H z}}s2mHz1
  - 符号：σa\sigma_{a}σa
- Ngw：陀螺仪随机游走噪声
  - 单位：rad s21Hz\frac{\text { rad }}{s^{2}} \frac{1}{\sqrt{H z}}s2 rad Hz1
  - 符号：σbg\sigma_{bg}σbg
- Naw：加速计随机游走噪声
  - 单位：ms31Hz\frac{m}{s^{3}} \frac{1}{\sqrt{H z}}s3mHz1
  - 符号：σba\sigma_{ba}σba

高斯白噪声的方差从连续时间到离散时间需要乘以一个1freq\frac{1}{\sqrt{freq}}freq1
Na=NadfreqNg=Ngdfreq\begin{array}{c} N_{a} = N_{a}^{d}\sqrt{freq} \\ N_{g} = N_{g}^{d}\sqrt{freq} \end{array} Na=NadfreqNg=Ngdfreq
随机游走方差从连续时间到离散时间乘以一个 freq\sqrt{freq}freq
Naw=Nawd1freqNgw=Ngwd1freq\begin{array}{c} N_{aw} = N_{aw}^{d}\frac{1}{\sqrt{freq}} \\ N_{gw} = N_{gw}^{d}\frac{1}{\sqrt{freq}} \end{array} Naw=Nawdfreq1Ngw=Ngwdfreq1
高斯白噪声协方差：
[Ng2000000Ng2000000Ng2000000Na2000000Na2000000Na2]\begin{bmatrix} 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{bmatrix} ⎣⎢⎢⎢⎢⎢⎢⎡Ng2000000Ng2000000Ng2000000Na2000000Na2000000Na2⎦⎥⎥⎥⎥⎥⎥⎤
随机游走协方差：
[Ngw2000000Ngw2000000Ngw2000000Naw2000000Naw2000000Naw2]\begin{bmatrix} 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{bmatrix} ⎣⎢⎢⎢⎢⎢⎢⎡Ngw2000000Ngw2000000Ngw2000000Naw2000000Naw2000000Naw2⎦⎥⎥⎥⎥⎥⎥⎤

ORB特征提取器

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

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

mvScalefactori=mvScalefactori−1⋅scalefactormvInvScalefactori=1mvScalefactorimvLevelSigma2i=mvScalefactori2mvInvLevelSigma2i=1mvLevelSigma2i\begin{array}{l} mvScalefactor_{i} = mvScalefactor_{i-1} \cdot scalefactor \\ mvInvScalefactor_{i} = \frac{1}{mvScalefactor_{i}} \\ mvLevelSigma2_{i} = mvScalefactor_{i}^{2} \\ mvInvLevelSigma2_{i} = \frac{1}{mvLevelSigma2_{i}} \end{array} mvScalefactori=mvScalefactori−1⋅scalefactormvInvScalefactori=mvScalefactori1mvLevelSigma2i=mvScalefactori2mvInvLevelSigma2i=mvLevelSigma2i1

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

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

假设需要提取的特征点数量为NNN，金字塔共有mmm层，第0层图像的宽为WWW，高为HHH ，对应的面积H⋅W=CH\cdot W =CH⋅W=C，图像金字塔缩放因子为s(0<s<1)s(0<s<1)s(0<s<1)，在 ORB-SLAM中，m=8，s=1/1.2m=8，s=1/ 1.2m=8，s=1/1.2。
那么，整个金字塔总面积为：
S=H⋅W⋅(s2)0+H⋅W⋅(s2)1+⋯+H⋅W⋅(s2)(m−1)=HW1−(s2)m1−s2=C1−(s2)m1−s2\begin{aligned} 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)} \\ &=H W \frac{1-\left(s^{2}\right)^{m}}{1-s^{2}}=C \frac{1-\left(s^{2}\right)^{m}}{1-s^{2}} \end{aligned} S=H⋅W⋅(s2)0+H⋅W⋅(s2)1+⋯+H⋅W⋅(s2)(m−1)=HW1−s21−(s2)m=C1−s21−(s2)m
单位面积应该分配的特征点数目为：
Navg=NS=NC1−(s2)m1−s2=N(1−s2)C(1−(s2)m)N_{a v g}=\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)} Navg=SN=C1−s21−(s2)mN=C(1−(s2)m)N(1−s2)
第0层应该分配的特征点数量为：
N0=N(1−s2)1−(s2)mN_{0}=\frac{N\left(1-s^{2}\right)}{1-\left(s^{2}\right)^{m}} N0=1−(s2)mN(1−s2)
第 i 层应该分配的特征点数量为：
Ni=N(1−s2)C(1−(s2)m)C(s2)i=N(1−s2)1−(s2)m(s2)iN_{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} Ni=C(1−(s2)m)N(1−s2)C(s2)i=1−(s2)mN(1−s2)(s2)i
在ORB-SLAM3 的代码里，不是按照面积均摊的，而是按照面积的开方来均摊特征点的，也就是将上述公式中的s2s^2s2换成sss 即可。

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

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

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

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