初始化
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的频率
- 单位：
h
z
hz
hz - 符号：
1
Δ
t
\frac{1}{\Delta t}
Δt1
- 单位：
- Ng：陀螺仪白噪声
- 单位：
rad
s
1
H
z
\frac{\text { rad }}{s} \frac{1}{\sqrt{H_{z}}}
s rad Hz1 - 符号：
σ
g
\sigma_{g}
σg
- 单位：
- Na：加速计白噪声
- 单位：
m
s
2
1
H
z
\frac{m}{s^{2}} \frac{1}{\sqrt{H z}}
s2mHz1 - 符号：
σ
a
\sigma_{a}
σa
- 单位：
- Ngw：陀螺仪随机游走噪声
- 单位：
rad
s
2
1
H
z
\frac{\text { rad }}{s^{2}} \frac{1}{\sqrt{H z}}
s2 rad Hz1 - 符号：
σ
b
g
\sigma_{bg}
σbg
- 单位：
- Naw：加速计随机游走噪声
- 单位：
m
s
3
1
H
z
\frac{m}{s^{3}} \frac{1}{\sqrt{H z}}
s3mHz1 - 符号：
σ
b
a
\sigma_{ba}
σba
- 单位：
高斯白噪声的方差从连续时间到离散时间需要乘以一个
1
f
r
e
q
\frac{1}{\sqrt{freq}}
freq1
N
a
=
N
a
d
f
r
e
q
N
g
=
N
g
d
f
r
e
q
\begin{array}{c} N_{a} = N_{a}^{d}\sqrt{freq} \\ N_{g} = N_{g}^{d}\sqrt{freq} \end{array}
Na=NadfreqNg=Ngdfreq
随机游走方差从连续时间到离散时间乘以一个
f
r
e
q
\sqrt{freq}
freq
N
a
w
=
N
a
w
d
1
f
r
e
q
N
g
w
=
N
g
w
d
1
f
r
e
q
\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
高斯白噪声协方差：
[
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
]
\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⎦⎥⎥⎥⎥⎥⎥⎤
随机游走协方差：
[
N
g
w
2
0
0
0
0
0
0
N
g
w
2
0
0
0
0
0
0
N
g
w
2
0
0
0
0
0
0
N
a
w
2
0
0
0
0
0
0
N
a
w
2
0
0
0
0
0
0
N
a
w
2
]
\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);
m
v
S
c
a
l
e
f
a
c
t
o
r
i
=
m
v
S
c
a
l
e
f
a
c
t
o
r
i
−
1
⋅
s
c
a
l
e
f
a
c
t
o
r
m
v
I
n
v
S
c
a
l
e
f
a
c
t
o
r
i
=
1
m
v
S
c
a
l
e
f
a
c
t
o
r
i
m
v
L
e
v
e
l
S
i
g
m
a
2
i
=
m
v
S
c
a
l
e
f
a
c
t
o
r
i
2
m
v
I
n
v
L
e
v
e
l
S
i
g
m
a
2
i
=
1
m
v
L
e
v
e
l
S
i
g
m
a
2
i
\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
−
1
n-1
n−1层，再将剩余的特征点N
−
s
u
m
(
0
,
n
−
1
)
N-sum(0,n-1)
N−sum(0,n−1)分配给第n层。
- 先分配前
- 每层金字塔期望的特征点数量
假设需要提取的特征点数量为
N
N
N，金字塔共有
m
m
m层，第0层图像的宽为
W
W
W，高为
H
H
H ，对应的面积
H
⋅
W
=
C
H\cdot W =C
H⋅W=C，图像金字塔缩放因子为
s
(
0
<
s
<
1
)
s(0<s<1)
s(0<s<1)，在 ORB-SLAM中 ，
m
=
8
，
s
=
1
/
1.2
m=8，s=1/ 1.2
m=8，s=1/1.2。
那么，整个金字塔总面积为：
S
=
H
⋅
W
⋅
(
s
2
)
0
+
H
⋅
W
⋅
(
s
2
)
1
+
⋯
+
H
⋅
W
⋅
(
s
2
)
(
m
−
1
)
=
H
W
1
−
(
s
2
)
m
1
−
s
2
=
C
1
−
(
s
2
)
m
1
−
s
2
\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
单位面积应该分配的特征点数目为：
N
a
v
g
=
N
S
=
N
C
1
−
(
s
2
)
m
1
−
s
2
=
N
(
1
−
s
2
)
C
(
1
−
(
s
2
)
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层应该分配的特征点数量为：
N
0
=
N
(
1
−
s
2
)
1
−
(
s
2
)
m
N_{0}=\frac{N\left(1-s^{2}\right)}{1-\left(s^{2}\right)^{m}}
N0=1−(s2)mN(1−s2)
第 i 层应该分配的特征点数量为：
N
i
=
N
(
1
−
s
2
)
C
(
1
−
(
s
2
)
m
)
C
(
s
2
)
i
=
N
(
1
−
s
2
)
1
−
(
s
2
)
m
(
s
2
)
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}
Ni=C(1−(s2)m)N(1−s2)C(s2)i=1−(s2)mN(1−s2)(s2)i
在ORB-SLAM3 的代码里，不是按照面积均摊的，而是按照面积的开方来均摊特征点的，也就是将上述公式中的
s
2
s^2
s2换成
s
s
s 即可。
- 初始化pattern
pattern为32
⋅
8
⋅
4
=
1024
32\cdot8\cdot4 = 1024
32⋅8⋅4=1024，其中pattern是ORB预设好的。
参考文献：《BRIEF: Binary Robust Independent Elementary Features 》
- 预先计算灰度质心法每行对应的终点
- 先算弧
A->B(v
∈
(
0
,
v
m
a
x
)
v\in(0,vmax )
v∈(0,vmax))段 - 再算弧
C->B(v
∈
(
r
,
v
m
i
n
)
v\in(r,vmin )
v∈(r,vmin))段
- 先算弧
注意：这里算出来vmax和vmin是相等的，可以看打印出来的点中有一个红圈套篮圈
欢迎点赞、收藏、转发，本人微信公众号：火柴的初心