- 1类似于‘json: cannot unmarshal string into Go struct field Device.type of type uint8‘_fata[0000] load podsandboxconfig: json: cannot unm
- 2MSF+APK注入绕过手机防护上线远控及安全防护建议_520apkhook
- 3全新升级SQLCoder-7B-2：领先的自然语言至SQL转换模型，性能显著提升
- 4基于SpringBoot+ElasticSearch 的Java底层框架的实现_springboot elasticsearch-java
- 5Rust中的结构体_rust 结构体
- 6微信小程序 实现上传图片前裁剪功能_微信小程序剪裁
- 7c语言实现单链表(数据结构)_c语言单链表实现
- 8css3使边框具有渐变的效果_border-top-color 渐变
- 9下一个AI前沿与革命：KAN 下_kan网络 样条
- 10微信小程序 图片裁剪（兼容性最强）_wx.cropimage
【PL-SLAM】原理部分_plgslam
赞
踩
1. Overview
PL-SLAM的原理如下图所示，主要包括三部分：视觉里程计VO，局部建图，闭环。
A. map
map 包括以下，
- a set of KFs（关键帧）
- the detected 3-D landmarks (both keypoints and line segments)
- a covisibility graph
- a spanning tree
其中，
- 关键帧包含了，观测到的立体特征和其描述子（利用字典计算出的左图描述子）、相机3D位姿信息。
- 路标包含了，一系列观测、每个被观测到的路标最具代表性的描述子、路标点的3D位置、路标线段的方向和其端点的坐标。
- 共视图建立了共视信息的模型，节点表示关键帧，边表示关键帧之间共视路标（文中设定共视路标的数目≥20）。如下图所示，用绿边表示的共视关系。
- 为了快速闭环优化，采用了本征图（essential graph），比共视图稀疏些，因其控制共视路标的数目≥100。
- 生成树表示了包含关键帧的图的最小连接表示。与本征图一起用于闭环。
B. Feature Tracking
注意，此处 feature 包括 point 和 line segment。
从一系列双目立体帧中跟踪特征，并计算它们的3D位置和其关联的协方差矩阵表示的不确定性。接着，3D路标被投影到新的相机位姿，并最小化投影误差以获取相机位姿增量和其估计的协方差。在每一帧重复这个过程，简单执行 frame-to-frame VO，直到新的关键帧插入地图map。
一旦关键帧插入地图，局部建图和闭环检测便同时进行（并行但不同线程）。
C. Local Mapping
局部建图，在共视图中，在新关键帧、上一关键帧和连接上一关键帧的其余关键帧之间，寻找新的特征对应关系。这样，构建了局部地图，其中包括与当前关键帧共视至少20个路标的所有关键帧，以及它们所观测到的所有路标。最后对局部地图执行优化。
D. Loop Closure
与局部建图并行，闭环检测采用基于BoW方法。在相机运动期间，捕获的 KFs 的所有视觉描述子都存储在数据库中，该数据库随之用于查找与当前帧相似的帧。仅当围绕此 KF 的局部序列也相似时，最佳匹配才被视为闭环候选。 最后，估算当前 KF 与闭环候选者之间的相对 *SE（3）*转换，以便在找到正确的估计的情况下，通过位姿图优化（PGO）过程校正闭环中涉及的所有 KFs 姿态 。
notice
这的局部建图和闭环，与ORB-SLAM中的方法一样，其目的是为了减小BA优化的计算量。
在BA优化时，采用近年流行的 relative techniques，而非消耗更大的 global approaches。
2. Feature Tracking
简而言之，沿着一连串的立体帧跟踪点和线段，然后通过最小化投影误差来计算相机的3D运动及其不确定性。 注意，此过程仅执行相机位姿的优化，而不优化被跟踪特征的3D位置，后者在后续的 LBA 过程中优化。
由于实验中使用的双目立体相机是经过预先标定的，因此可以通过直接使用其外部参数来确定观察到的关键点和直线的3D位置，从而在单个立体镜头中对这些特征进行初始化。
A. Point Features
采用ORB特征，关键点检测性能好，描述子匹配快速。为了减少匹配的离群点，我们仅考虑满足以下条件的测量：左图像中的最佳匹配对应于右图像中的最佳匹配，即它们是互为最佳的匹配。此外，还过滤掉出，在描述子空间中，那些 “ 第二最佳匹配的距离 ” 小于 “ 最佳匹配的距离 ” 的两倍的匹配，以确保对应关系足够有意义。（更鲁棒吧）
B. Line Segment Features（LSD线段特征+LBD描述子）
LSD方法已用于提取线段，从而提供了高精度和可重复性。对于立体匹配和帧到帧跟踪，使用 LBD 方法 （line band descriptor）提供的二进制描述子来增强线段，这能够根据线的局部外观找到线之间的对应关系。 与点的情况类似，我们检查两个候选特征是否互为最佳匹配，并且特征是否足够有意义。 最后，我们利用线段提供的有用的几何信息来滤除那些具有不同方向和长度的线匹配，以及那些在端点视差上有很大差异的线匹配。 注意，此过滤器有助于系统保留大量的结构线，从而允许基于点和线形成更一致的地图[见下图]。
C. Motion Estimation
将关键点和线段从第一帧反向投影到下一帧。 然后，采用鲁棒的高斯-牛顿法，最小化线和关键点投影误差来迭代估计相机的运动。为了处理离群值，我们采用了 Pseudo-Huberr 损失函数并执行两步最小化。最后，获得两个连续帧之间的增量运动估计，可以通过以下正态分布来建模：
ξ
t
,
t
+
1
∼
N
(
ξ
t
,
t
+
1
∗
,
Σ
ξ
t
,
t
+
1
∗
)
（
1
）
\boldsymbol{\xi}_{t, t+1} \sim \mathcal{N}\left(\boldsymbol{\xi}_{t, t+1}^{*}, \boldsymbol{\Sigma}_{\boldsymbol{\xi}_{t, t+1}^{*}}\right)（1）
ξt,t+1∼N(ξt,t+1∗,Σξt,t+1∗)（1）
其中
ξ
t
,
t
+
1
∗
∈
s
e
(
3
)
\boldsymbol{\xi}_{t, t+1}^{*} \in \mathfrak{s e}(3)
ξt,t+1∗∈se(3) 是在
t
t
t 和
t
+
1
t+1
t+1之间的相机运动的6维向量 , 而
Σ
ξ
t
,
t
+
1
∗
\boldsymbol{\Sigma}_{\boldsymbol{\xi}_{t, t+1}^{*}}
Σξt,t+1∗ 估计的运动的协方差。
D. Keyframe Selection
为了确定何时在地图中插入新的KF，采用了 relative motion estimation 的不确定性。 因此，根据（1），通过以下表达式将不确定性从协方差矩阵转化为标量，称为熵：
h
(
ξ
)
=
3
(
1
+
log
(
2
π
)
)
+
0.5
log
(
∣
Σ
ξ
∣
)
（
2
）
h(\boldsymbol{\xi})=3(1+\log (2 \pi))+0.5 \log \left(\left|\boldsymbol{\Sigma}_{\xi}\right|\right)（2）
h(ξ)=3(1+log(2π))+0.5log(∣Σξ∣)（2）
然后，对于给定的 KF i，我们检查 “ KF i与当前帧 i + u 之间的运动估计熵 ” 与 “ KF i与其首个连续帧 i + 1之间的熵 ” 之间的比率，即
α
=
h
(
ξ
i
,
i
+
u
)
h
(
ξ
i
,
i
+
1
)
（
3
）
\alpha=\frac{h\left(\boldsymbol{\xi}_{i, i+u}\right)}{h\left(\boldsymbol{\xi}_{i, i+1}\right)}（3）
α=h(ξi,i+1)h(ξi,i+u)（3）
如果 α 值低于某个预先确定的阈值（在实验中已将其设置为0.9），则将帧 i + u 作为新的 KF 插入到系统中。注意，要计算（2）中的表达式，需要非连续帧之间的位姿增量的不确定性。 由于（1）仅估计连续帧之间的增量运动，因此通过一阶高斯传播技术来组合一系列此类估计，以获得两个非连续 KF 之间的协方差。
3. Local mapping
本节描述了在插入新的 KF 时系统的行为，该行为主要包括执行局部地图的 BA 优化，该局部地图就是，“ 通过共视图与当前 KF 连接的那些 KFs ” + “ 由这些局部 KFs 所观察到的路标 ”。
A. Keyframe Insertion
每次 VO 选择一个KF，都会将其插入 SLAM 系统并优化局部地图。首先，我们优化当前和先前KF之间的相对位姿变化的估计，因为由 VO 提供的位姿是通过合成中间帧之间的相对运动来估计的。为此，我们考虑了第2节中描述的几何约束，并在 KFs 之间执行了数据关联，并获得了在其中观察到的一致的共同特征集。 然后，执行与第2-C节中介绍的优化类似的优化，为此，我们将VO 线程提供的位姿用作高斯-牛顿最小化的初始估计。计算完 KFs 之间的相对位姿变化后，将当前的 KF 插入系统，包括以下内容。
- 1）关键帧的索引。
- 2）其 3D 位姿的信息，包括绝对位姿、与前一个 KF 的相对位姿及其相关的不确定性。
- 3）新的 3D 路标，通过存储其 2D 图像坐标和其描述子进行初始化。已存在的路标的新观测值也将添加到地图中。
最后，还在当前帧的未匹配特征观测值与局部地图中的地标之间寻找新的对应关系。
B. Local Bundle Adjustment
插入
K
F
\mathrm{KF}
KF 之后, 接下来执行局部地图的 BA 优化。定义一个向量
ψ
\psi
ψ ，其包含了待优化的变量， 它们是 每个关键帧 KF 的
se
(
3
)
\operatorname{se}(3)
se(3) 位姿
ξ
i
w
\boldsymbol{\xi}_{i w}
ξiw， 每个
3
D
3\mathrm{D}
3D 点的位置
X
w
j
，
\mathbf{X}_{w j}，
Xwj， 和每个线段的端点的3D坐标:
{
P
w
k
,
Q
w
k
}
.
\left\{\mathbf{P}_{w k}, \mathbf{Q}_{w k}\right\} .
{Pwk,Qwk}. 然后，将观测值和投影到观测帧的路标之间的投影误差最小化
ψ
∗
=
argmin
ψ
∑
i
∈
K
l
[
∑
j
∈
P
l
e
i
j
⊤
Σ
e
i
j
−
1
e
i
j
+
∑
k
∈
L
l
e
i
k
⊤
Σ
e
i
k
−
1
e
i
k
]
（
4
）
\boldsymbol{\psi}^{*}=\underset{\boldsymbol{\psi}}{\operatorname{argmin}} \sum_{i \in \mathcal{K}_{l}}\left[\sum_{j \in \mathcal{P}_{l}} \mathbf{e}_{i j}^{\top} \boldsymbol{\Sigma}_{\mathbf{e}_{i j}}^{-1} \mathbf{e}_{i j}+\sum_{k \in \mathcal{L}_{l}} \mathbf{e}_{i k}^{\top} \boldsymbol{\Sigma}_{\mathbf{e}_{i k}}^{-1} \mathbf{e}_{i k}\right]（4）
ψ∗=ψargmini∈Kl∑⎣⎡j∈Pl∑eij⊤Σeij−1eij+k∈Ll∑eik⊤Σeik−1eik⎦⎤（4）
其中 K l , P l , \mathcal{K}_{l}, \mathcal{P}_{l}, Kl,Pl, 和 L l \mathcal{L}_{l} Ll 分别指的是局部 KFs 集合 ，点集, 和线段集。
在（4）中，点的重投影误差
e
i
j
\mathbf{e}_{i j}
eij，按如下计算，
e
i
j
=
x
i
j
−
π
(
ξ
i
w
,
X
w
j
)
（
5
）
\mathbf{e}_{i j}=\mathbf{x}_{i j}-\boldsymbol{\pi}\left(\boldsymbol{\xi}_{i w}, \mathbf{X}_{w j}\right)（5）
eij=xij−π(ξiw,Xwj)（5）
其中函数
π
:
s
e
(
3
)
×
R
3
↦
R
2
\boldsymbol{\pi}: \mathfrak{s e}(3) \times \mathbb{R}^{3} \mapsto \mathbb{R}^{2}
π:se(3)×R3↦R2 表示重投影，先将世界坐标系下第
j
j
j 个 3D 点
X
w
j
\mathbf{X}_{w j}
Xwj 放入第
i
i
i 个
K
F
\mathrm{KF}
KF的局部参考坐标系下，即,
X
i
j
,
\mathbf{X}_{i j},
Xij, 再把这个点投射到图像中。
关于线段，将3D线段的投影端点与其在图像平面中对应的无限线之间的距离作为误差函数。在第 i 帧观测到的第 k 条线段的重投影误差
e
i
k
\mathbf{e}_{i k}
eik，有
e
i
k
=
[
l
i
k
⋅
π
(
ξ
i
w
,
P
w
k
)
l
i
k
⋅
π
(
ξ
i
w
,
Q
w
k
)
]
（
6
）
\mathbf{e}_{i k}=\left[lik⋅π(ξiw,Pwk)lik⋅π(ξiw,Qwk)
其中
P
w
k
\mathbf{P}_{w k}
Pwk 和
Q
w
k
\mathbf{Q}_{w k}
Qwk 指的是世界坐标下线段端点的 3D 坐标，
l
i
k
\mathbf{l}_{i k}
lik是与第 i 个KF 中的第 k 个线段相对应的无穷线的方程式，可以用齐次坐标中的线段的二维端点之间的叉积获得，即：
l
i
k
=
p
i
k
×
q
i
k
\mathbf{l}_{i k}=\mathbf{p}_{i k} \times \mathbf{q}_{i k}
lik=pik×qik。
利用Levenberg-Marquardt优化方法可以迭代地解决（4）中的问题，为此我们需要估计Jacobian矩阵和Hessian矩阵，
Δ
ψ
=
[
H
+
λ
diag
(
H
)
]
−
1
J
⊤
W
e
（
7
）
\Delta \boldsymbol{\psi}=[\mathbf{H}+\lambda \operatorname{diag}(\mathbf{H})]^{-1} \mathbf{J}^{\top} \mathbf{W} \mathbf{e}（7）
Δψ=[H+λdiag(H)]−1J⊤We（7）
其中误差向量
e
\mathbf{e}
e包含了点和线段的重投影误差。（7）和更新步骤（8），
ψ
′
=
ψ
⊕
Δ
ψ
（
8
）
\boldsymbol{\psi}^{\prime}=\boldsymbol{\psi} \oplus \Delta \boldsymbol{\psi}（8）
ψ′=ψ⊕Δψ（8）
可以递归应用，直到收敛为止，从而得到最佳
ψ
\boldsymbol{\psi}
ψ，从中可以更新局部 KF 和路标的位置。 注意，鉴于
ψ
\boldsymbol{\psi}
ψ 中变量的不同性质，更新方程无法直接应用于整个矢量。
应强调的是，在（4）中，点和线中的误差影响分别由 Σ e i j − 1 \boldsymbol{\Sigma}_{\mathbf{e}_{i j}}^{-1} Σeij−1 和 Σ e i k − 1 \boldsymbol{\Sigma}_{\mathbf{e}_{i k}}^{-1} Σeik−1 加权，代表与每个投影误差不确定性相关的协方差矩阵的逆 。但实际上，将此类协方差设置为单位矩阵并遵循与2-C节中所述方法类似的方法更为有效，因为它引入了稳健的权重，并且还处理了离群值的存在。
最后，移除地图中观测少于三次的路标点。
4. Loop closure
采用基于所提取的关键点和线段的二进制描述子的 BoW 方法，以鲁棒地应对数据关联和闭环检测。
简而言之，BoW技术将从图像中提取的所有信息（关键点和线段的描述子）汇总到一个单词向量中，为此，它使用了从不同图像数据集离线构建的字典表。 然后，随着相机的移动，从捕捉的图像中计算出单词并存储在数据库中，从而随后将用于搜索与当前关键帧最相似的图像。
A. Loop-Closure Detection
闭环检测既要找到类似于当前正在处理的图像，又要估算它们之间的相对位姿变化，如下所述。
1）Visual Place Recognition：
针对点和线段，建立两个特定的字典和数据库。这样，在每个时间步，并行检索关键点和线段数据库中最相似的图像，以查找闭环。双重搜索的动机是，某些场景可能用线条而不是关键点来描述，反之亦然。 因此，同时使用这两种方法并合并它们的结果使我们能够精确数据库查询的输出，从而减少计算量。
为了说明这一点，首先定义一个相似度矩阵，该矩阵在每一行中都包含相似度值，其值的范围为[0，1]，于是计算相似度矩阵。
具体地，如下图4(a) 中的矩阵仅使用ORB关键点来计算，以建立字典和数据库，而图4（b）中的矩阵仅依赖于线段。调色板从蓝色（分数= 0）到红色（分数= 1）。注意到，根据关键点，图像看起来相似的地方，在第一个矩阵中会出现一些淡黄色区域。这表明存在潜在的闭环，但实际上它们只是假阳闭环。但是，第二个矩阵没有这种现象。另一方面，基于关键点的检测，由于不相似图像的分数差异通常更大，因此第一个矩阵具有更大的区别性。因此，当检测潜在的闭环时，可以将组合查询两个特征数据库而产生的图像相似度，以提高鲁棒性。
作者制定了一套点线特征的加权求和的方式（
s
k
s_{k}
sk表示关键点分数，
s
l
s_{l}
sl表示线段分数），来计算图像之间的相似度得分，根据的两个标准，分别是 strength 和 dispersion。 前者根据图像中检测到的一组特征中关键点或线条的特征数量，按比例对相似性评分加权，而后者则考虑特征在图像中的离散程度（ 分散度越高，权重越大）。得到鲁棒的相似度评分如下：
s
t
=
0.5
(
n
k
/
(
n
k
+
n
l
)
+
d
k
/
(
d
k
+
d
l
)
)
s
k
+
0.5
(
n
l
/
(
n
k
+
n
l
)
+
d
l
/
(
d
k
+
d
l
)
)
s
l
（
10
）
st=0.5(nk/(nk+nl)+dk/(dk+dl))sk+0.5(nl/(nk+nl)+dl/(dk+dl))sl
其中，
n
k
n_{k}
nk，
n
l
n_{l}
nl为提取的关键点和线段的数量，
d
k
d_{k}
dk，
d
l
d_{l}
dl表示离散值——分别计算检测到特征的 x 、y 坐标的方差之和的平方根。针对线段特征，x 和 y 为线段中点。
此公式给予两种特征都相同的重要性（因此采用0.5系数）。
将上述策略与其余四种方法作比较，采用四种数据集测试。策略3即对应（10），表现最好。
2）估计相对运动
一旦有了闭环候选，需要滤掉假阳的闭环。 恢复形成闭环的两个KF之间的相对姿态（我们把其中一个命名为当前帧，另一个为旧帧），来实现筛除假阳。首先，先在这两个关键帧寻找特征匹配，接着在当前帧与旧帧关联的局部地图之间寻找新对应关系。接着估计两个KF之间的刚性变换 ξ ^ i j ∈ s e ( 3 ) \hat{\boldsymbol{\xi}}_{i j} \in \mathfrak{s e}(3) ξ^ij∈se(3)。最后，我们再检查闭环候选的一致性，检测方法如下：
- 协方差矩阵 Σ ξ ~ i j \Sigma_{\tilde{\xi}_{i j}} Σξ~ij 的最大特征值小于0.01
- 获得的平移和旋转不能分别超过0.50米和3度。
- 估计计算时，内点所占比例大于50%;
关于第一个标准，不确定性矩阵的特征值的较大值（参见（1））通常指出了病态Hessian矩阵，这很可能是由于特征匹配集中存在大量离群值 。确保协方差矩阵的最大特征值低于某个阈值，能够检测到可能不正确的闭环候选并将其丢弃。
关于第二个标准，为估计的姿态设置了最大平移和旋转限制，因为基于BoW的方法通常会提供外观和位姿非常相似的匹配，因此若两帧之间的位姿有很大变化，通常为错误的闭环检测。
关于第三个标准，由于运动估计会受到离群值以及视觉场景识别的错误关联的强烈影响，因此在优化之后设置检测到的离群值的最小比率。
B. Loop Correction
在估计了轨迹中所有连续的闭环之后，融合闭环的两侧，校正沿环分布的误差。将问题作为PGO（位姿图优化？）来解决，其中节点为关键帧，边由本征图和生成树给定。定义以下误差函数为
s
e
(
3
)
\mathfrak{s e}(3)
se(3)差：（不太懂）
r
i
j
(
ξ
i
w
,
ξ
j
w
)
=
log
(
exp
(
ξ
^
i
j
)
⋅
exp
(
ξ
j
w
)
⋅
exp
(
ξ
i
w
)
−
1
)
（
11
）
\mathbf{r}_{i j}\left(\boldsymbol{\xi}_{i w}, \boldsymbol{\xi}_{j w}\right)=\log \left(\exp \left(\hat{\boldsymbol{\xi}}_{i j}\right) \cdot \exp \left(\boldsymbol{\xi}_{j w}\right) \cdot \exp \left(\boldsymbol{\xi}_{i w}\right)^{-1}\right)（11）
rij(ξiw,ξjw)=log(exp(ξ^ij)⋅exp(ξjw)⋅exp(ξiw)−1)（11）
其中，log 映射表示
S
E
(
3
)
↦
s
e
(
3
)
SE(3) \mapsto \mathfrak{s e}(3)
SE(3)↦se(3)，exp 映射表示
s
e
(
3
)
↦
\mathfrak{s e}(3) \mapsto
se(3)↦
S
E
(
3
)
S E(3)
SE(3)。
用 g2o 库来解决这个G2O问题，从而生成关键帧的最优位姿。我们更新KFs的位姿，以及它们所观测到的地标的位姿，并且，还要将回环两边的局部地图进行融合，方法是首先融合在估计它们的相对位姿时所匹配的地标点，然后再在未匹配的地标之间寻找新的匹配关系。
参考
https://zhuanlan.zhihu.com/p/41371645
