ORB SLAM3的初始化主要是创建ORB词袋、关键帧数据库、多地图等对象，其步骤如下：
- 检测配置文件能否打开
- 加载ORB词袋(ORBVocabulary)
- 创建关键帧数据库(KeyFrameDatabase)
- 创建多地图(Atlas)
- 创建跟踪线程(Tracking)
- 创建局部建图线程(LocalMapping)
- 创建闭环线程(LoopClosing)
- 设置线程间的指针
- 从配置文件中读取相机参数，创建相机模型(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 Hz 1
- 符号： σ g sigma_{g} σg
- Na：加速计白噪声
- 单位： m s 2 1 H z frac{m}{s^{2}} frac{1}{sqrt{H z}} s2mHz 1
- 符号： σ a sigma_{a} σa
- Ngw：陀螺仪随机游走噪声
- 单位： rad s 2 1 H z frac{text { rad }}{s^{2}} frac{1}{sqrt{H z}} s2 rad Hz 1
- 符号： σ b g sigma_{bg} σbg
- Naw：加速计随机游走噪声
- 单位： m s 3 1 H z frac{m}{s^{3}} frac{1}{sqrt{H z}} s3mHz 1
- 符号： σ b a sigma_{ba} σba
高斯白噪声的方差从连续时间到离散时间需要乘以一个
1
f
r
e
q
frac{1}{sqrt{freq}}
freq
1
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=Nadfreq
Ng=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=Nawdfreq
1Ngw=Ngwdfreq
1
高斯白噪声协方差：
[
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⎦⎥⎥⎥⎥⎥⎥⎤
- 设置图像金字塔的尺度因子、逆尺度因子、方差
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
Hcdot W =C
H⋅W=C，图像金字塔缩放因子为
s
(
0
<
s
<
1
)
s(0
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 cdotleft(s^{2}right)^{0}+H cdot W cdotleft(s^{2}right)^{1}+cdots+H cdot W cdotleft(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{Nleft(1-s^{2}right)}{Cleft(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{Nleft(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{Nleft(1-s^{2}right)}{Cleft(1-left(s^{2}right)^{m}right)} Cleft(s^{2}right)^{i}=frac{Nleft(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 32cdot8cdot4 = 1024 32⋅8⋅4=1024，其中pattern是ORB预设好的。
参考文献：《BRIEF: Binary Robust Independent Elementary Features 》
- 预先计算灰度质心法每行对应的终点
- 先算弧A->B( v ∈ ( 0 , v m a x ) vin(0,vmax ) v∈(0,vmax))段
- 再算弧C->B( v ∈ ( r , v m i n ) vin(r,vmin ) v∈(r,vmin))段
注意：这里算出来vmax和vmin是相等的，可以看打印出来的点中有一个红圈套篮圈
欢迎点赞、收藏、转发，本人微信公众号：火柴的初心
