Our proposed Edge SLAM pipeline detects edge points from images and tracks those using optical flow for point correspondence. We further refine these point correspondences using geometrical relationship among three views.
e find the DoG based edge detector is reliable due to its robustness in illumination and contrast changes. We thin the edges further to generate edges of a single pixel width. We apply an edge filtering process described by Juan and Sol upon the thinned edges to calculate connectivity of the edge points.
假设两个端点的归一化坐标分别
s
c
1
=
[
u
s
,
v
s
,
1
]
⊤
\mathbf{s}^{c_{1}}=\left[u_{s}, v_{s}, 1\right]^{\top}
sc1=[us,vs,1]⊤和
e
c
1
=
[
u
e
,
v
e
,
1
]
⊤
\mathbf{e}^{c_{1}}=\left[u_{e}, v_{e}, 1\right]^{\top}
ec1=[ue,ve,1]⊤,相机原点世界坐标系坐标
C
=
[
x
0
,
y
0
,
z
0
]
⊤
\mathbf{C}=\left[x_{0}, y_{0}, z_{0}\right]^{\top}
C=[x0,y0,z0]⊤，那么对应平面
π
\pi
π的计算公式为
π
x
(
x
−
x
0
)
+
π
y
(
y
−
y
0
)
+
π
z
(
z
−
z
0
)
=
0
\pi_{x}\left(x-x_{0}\right)+\pi_{y}\left(y-y_{0}\right)+\pi_{z}\left(z-z_{0}\right)=0
πx(x−x0)+πy(y−y0)+πz(z−z0)=0其中
[
π
x
π
y
π
z
]
=
[
s
c
1
]
×
e
c
1
,
π
w
=
π
x
x
0
+
π
y
y
0
+
π
z
z
0
\left[\begin{array}{c}{\pi_{x}} \\ {\pi_{y}} \\ {\pi_{z}}\end{array}\right]=\left[\mathbf{s}^{c_{1}}\right]_{ \times} \mathbf{e}^{c_{1}}, \quad \pi_{w}=\pi_{x} x_{0}+\pi_{y} y_{0}+\pi_{z} z_{0}
⎣⎡πxπyπz⎦⎤=[sc1]×ec1,πw=πxx0+πyy0+πzz0而由平面
π
1
\pi_1
π1和平面
π
2
\pi_2
π2可以得到特征直线在世界坐标系下的坐标：
L
∗
=
[
[
d
]
×
n
−
n
⊤
0
]
=
π
1
π
2
⊤
−
π
2
π
1
⊤
∈
R
4
×
4
\mathbf{L}^{*}=\left[\begin{array}{cc}{[\mathbf{d}]_{ \times}} & {\mathbf{n}} \\ {-\mathbf{n}^{\top}} & {0}\end{array}\right]=\pi_{1} \boldsymbol{\pi}_{2}^{\top}-\boldsymbol{\pi}_{2} \boldsymbol{\pi}_{1}^{\top} \in \mathbb{R}^{4 \times 4}
L∗=[[d]×−n⊤n0]=π1π2⊤−π2π1⊤∈R4×4上述计算公式的代码在void LineFeatureManager::line_triangulate()函数当中。
直线特征的维护方式，上面通过直线三角化出来的是普吕克矩阵
L
∗
=
[
[
d
]
×
n
−
n
⊤
0
]
\mathbf{L}^{*}=\left[\begin{array}{cc}{[\mathbf{d}]_{ \times}} & {\mathbf{n}} \\ {-\mathbf{n}^{\top}} & {0}\end{array}\right]
L∗=[[d]×−n⊤n0]我们可以非常方便地从中提取线特征的普吕克坐标
d
\mathbf{d}
d和
n
\mathbf{n}
n。普吕克坐标方便用来进行空间变换，但是不方便用来进行优化后更新，因此我们在程序中采用的维护方式是在LineFeatureManager类中维护线特征的正交表示形式，以一个五维向量进行存储，方便后端优化更新时可以直接进行更新，稍微不方便的是在进行空间变换的时候再将其转化为原始的普吕克坐标形式才进行变换。 普吕克坐标的原始表示形式为
L
=
(
n
⊤
,
d
⊤
)
⊤
\mathcal{L}=\left(\mathbf{n}^{\top}, \mathbf{d}^{\top}\right)^{\top}
L=(n⊤,d⊤)⊤，其中
n
\mathbf{n}
n和
d
\mathbf{d}
d是和空间直线相关的两个向量，可以通过下图理解：
普吕克坐标
L
=
(
n
⊤
,
d
⊤
)
⊤
\mathcal{L}=\left(\mathbf{n}^{\top}, \mathbf{d}^{\top}\right)^{\top}
L=(n⊤,d⊤)⊤在矩阵
T
c
w
=
[
R
c
w
p
c
w
0
1
]
\mathbf{T}_{c w}=\left[\begin{array}{cc}{\mathbf{R}_{c w}} & {\mathbf{p}_{c w}} \\ {\mathbf{0}} & {\mathbf{1}}\end{array}\right]
Tcw=[Rcw0pcw1]作用下的空间变换公式如下，操作起来非常方便：
L
c
=
[
n
c
d
c
]
=
T
c
w
L
w
=
[
R
c
w
[
p
c
w
]
×
R
c
w
0
R
c
w
]
L
w
\mathcal{L}^{c}=\left[\begin{array}{l}{\mathbf{n}^{c}} \\ {\mathbf{d}^{c}}\end{array}\right]=\mathcal{T}_{c w} \mathcal{L}_{w}=\left[\begin{array}{cc}{\mathbf{R}_{c w}} & {\left[\mathbf{p}_{c w}\right]_{ \times} \mathbf{R}_{c w}} \\ {0} & {\mathbf{R}_{c w}}\end{array}\right] \mathcal{L}^{w}
Lc=[ncdc]=TcwLw=[Rcw0[pcw]×RcwRcw]Lw普吕克坐标
L
=
(
n
⊤
,
d
⊤
)
⊤
\mathcal{L}=\left(\mathbf{n}^{\top}, \mathbf{d}^{\top}\right)^{\top}
L=(n⊤,d⊤)⊤转化为正交表示形式的公式如下：
U
=
R
(
ψ
)
=
[
n
d
∥
d
∥
n
×
d
∥
n
×
d
∥
]
\mathbf{U}=\mathbf{R}(\psi)=\left[\begin{array}{ccc}{\mathbf{n}} & {\frac{\mathbf{d}}{\|\mathbf{d}\|}} & {\frac{\mathbf{n} \times \mathbf{d}}{\|\mathbf{n} \times \mathbf{d}\|}}\end{array}\right]
U=R(ψ)=[n∥d∥d∥n×d∥n×d]
W
=
[
cos
(
ϕ
)
−
sin
(
ϕ
)
sin
(
ϕ
)
cos
(
ϕ
)
]
=
1
(
∥
n
∥
2
+
∥
d
∥
2
)
[
∥
n
∥
−
∥
d
∥
∥
d
∥
∥
n
∥
]
\mathbf{W}=\left[\begin{array}{cc}{\cos (\phi)} & {-\sin (\phi)} \\ {\sin (\phi)} & {\cos (\phi)}\end{array}\right]=\frac{1}{\sqrt{\left(\|\mathbf{n}\|^{2}+\|\mathbf{d}\|^{2}\right)}}\left[\begin{array}{cc}{\|\mathbf{n}\|} & {-\|\mathbf{d}\|} \\ {\|\mathbf{d}\|} & {\|\mathbf{n}\|}\end{array}\right]
W=[cos(ϕ)sin(ϕ)−sin(ϕ)cos(ϕ)]=(∥n∥2+∥d∥2)1[∥n∥∥d∥−∥d∥∥n∥]其中
U
\mathbf{U}
U是和直线方向有关旋转矩阵，
W
\mathbf{W}
W是和直线距离有关的矩阵，在我们的程序中，我们将
U
\mathbf{U}
U表示成四元数的形式，而
U
\mathbf{U}
U直接通过
ϕ
\phi
ϕ进行存储，因此我们在LineFeatureManager类中维护线特征是一个五维的向量。和这一部分变换相关的代码在utility类当中。
2.2.3 线特征的非线性优化
因为我们维护的线特征在世界坐标系下的普吕克坐标，因此我们想求得线特征的参差的话只需要将线特征的空间坐标投影到某一观测帧的归一化平面上（估计值），然后通过这帧上观测值（线段的两端点），按照下面方式定义线特征的残差：
r
l
(
z
L
l
′
c
i
,
X
)
=
[
d
(
s
l
c
i
,
l
l
c
i
)
d
(
e
l
c
i
,
l
l
c
i
)
]
\mathbf{r}_{l}\left(\mathbf{z}_{\mathcal{L}_{l}^{\prime}}^{c_{i}}, \mathcal{X}\right)=\left[\begin{array}{l}{d\left(\mathbf{s}_{l}^{c_{i}}, \mathbf{l}_{l}^{c_{i}}\right)} \\ {d\left(\mathbf{e}_{l}^{c_{i}}, \mathbf{l}_{l}^{c_{i}}\right)}\end{array}\right]
rl(zLl′ci,X)=[d(slci,llci)d(elci,llci)]其中
d
(
s
,
l
)
=
s
⊤
1
l
1
2
+
l
2
2
d(\mathbf{s}, \mathbf{l})=\frac{\mathbf{s}^{\top} \mathbf{1}}{\sqrt{l_{1}^{2}+l_{2}^{2}}}
d(s,l)=l12+l22s⊤1其中
l
\mathbf{l}
l为空间线特征投影到归一化平面的直线，
e
l
c
i
\mathbf{e}_{l}^{c_{i}}
elci和
e
l
e
i
\mathbf{e}_{l}^{e_{i}}
elei分别为观测值的两端点。 然后我们定义和线特征相关的优化变量为：世界坐标系下的线特征的普吕克坐标的正交表示形式
O
l
\mathcal{O}_{l}
Ol和对应观测帧的空间位姿
x
i
\mathcal{x}^{i}
xi。 雅克比矩阵对优化变量的雅克比矩阵如下：
J
l
=
∂
r
l
∂
l
c
i
∂
l
c
i
∂
L
c
i
[
∂
L
c
i
∂
δ
x
i
∂
L
c
i
∂
L
w
∂
L
w
∂
δ
O
]
\mathbf{J}_{l}=\frac{\partial \mathbf{r}_{l}}{\partial \mathbf{l}^{c_{i}}} \frac{\partial \mathbf{l}^{c_{i}}}{\partial \mathcal{L}^{c_{i}}}\left[\frac{\partial \mathcal{L}^{c_{i}}}{\partial \delta \mathbf{x}^{i}} \quad \frac{\partial \mathcal{L}^{c_{i}}}{\partial \mathcal{L}^{w}} \frac{\partial \mathcal{L}^{w}}{\partial \delta \mathcal{O}}\right]
Jl=∂lci∂rl∂Lci∂lci[∂δxi∂Lci∂Lw∂Lci∂δO∂Lw]其中
∂
r
l
∂
l
c
i
=
[
−
l
1
(
s
l
c
i
)
⊤
1
(
l
1
2
+
l
2
2
)
(
3
2
)
+
u
s
(
l
1
2
+
l
2
2
)
(
1
2
)
−
l
2
(
s
l
c
i
)
⊤
1
(
l
1
2
+
l
2
2
)
(
3
2
)
+
v
S
(
l
1
2
+
l
2
2
)
(
1
2
)
1
(
l
1
2
+
l
2
2
)
(
1
2
)
−
l
1
(
e
l
c
i
)
⊤
1
(
l
1
2
+
l
2
2
)
(
3
2
)
+
u
e
(
l
1
2
+
l
2
2
)
(
1
2
)
−
l
2
(
e
l
c
i
)
⊤
1
(
l
1
2
+
l
2
2
)
(
3
2
)
+
v
e
(
l
1
2
+
l
2
2
)
(
1
2
)
1
(
l
1
2
+
l
2
2
)
(
1
2
)
]
2
×
3
\frac{\partial \mathbf{r}_{l}}{\partial \mathbf{l}^{c_{i}}}=\left[\begin{array}{ccc}\frac{-l_{1}\left(\mathbf{s}_{l}^{c_{i}}\right)^{\top} 1}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{3}{2}\right)}}+\frac{u_{s}}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{1}{2}\right)}} & \frac{-l_{2}\left(\mathbf{s}_{l}^{c_{i}}\right)^{\top} \mathbf{1}}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{3}{2}\right)}}+\frac{v_{S}}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{1}{2}\right)}} & \frac{1}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{1}{2}\right)}} \\ \frac{-l_{1}\left(\mathbf{e}_{l}^{c_{i}}\right)^{\top} 1}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{3}{2}\right)}}+\frac{u_{e}}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{1}{2}\right)}} & \frac{-l_{2}\left(\mathbf{e}_{l}^{c_{i}}\right)^{\top} 1}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{3}{2}\right)}}+\frac{v_{e}}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{1}{2}\right)}} & \frac{1}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{1}{2}\right)}}\end{array}\right]_{2 \times 3}
∂lci∂rl=⎣⎢⎢⎢⎡(l12+l22)(23)−l1(slci)⊤1+(l12+l22)(21)us(l12+l22)(23)−l1(elci)⊤1+(l12+l22)(21)ue(l12+l22)(23)−l2(slci)⊤1+(l12+l22)(21)vS(l12+l22)(23)−l2(elci)⊤1+(l12+l22)(21)ve(l12+l22)(21)1(l12+l22)(21)1⎦⎥⎥⎥⎤2×3
∂
l
c
i
∂
L
c
i
=
[
I
0
]
3
×
6
\frac{\partial \mathbf{l}^{c_{i}}}{\partial \mathcal{L}^{c_{i}}}=[\mathcal{I} \quad \mathbf{0}]_{3 \times 6}
∂Lci∂lci=[I0]3×6
∂
r
l
∂
l
c
i
=
[
T
b
c
−
1
[
R
w
b
⊤
[
d
w
]
×
0
3
×
3
]
T
b
c
−
1
[
[
R
w
b
⊤
(
n
w
+
[
d
w
]
×
p
w
b
)
]
×
]
[
R
w
b
⊤
d
w
]
×
]
0
0
0
]
6
×
15
\frac{\partial \mathbf{r}_{l}}{\partial \mathbf{l}^{c_{i}}}=\left[\begin{array}{ccc} \mathcal{T}_{b c}^{-1}\left[\begin{array}{c}{\mathbf{R}_{w b}^{\top}\left[\mathbf{d}^{w}\right] \times} \\ {\mathbf{0}_{3 \times 3}}\end{array}\right] & \mathcal{T}_{b c}^{-1}\left[\begin{array}{c}{\left[\mathbf{R}_{w b}^{\top}\left(\mathbf{n}^{w}+\left[\mathbf{d}^{w}\right] \times \mathbf{p}_{w b}\right)\right] \times ]} \\ {\left[\mathbf{R}_{w b}^{\top} \mathbf{d}^{w}\right]_{ \times}}\end{array}\right] & \mathbf{0} & \mathbf{0} & \mathbf{0}\end{array}\right]_{6 \times 15}
∂lci∂rl=[Tbc−1[Rwb⊤[dw]×03×3]Tbc−1[[Rwb⊤(nw+[dw]×pwb)]×][Rwb⊤dw]×]000]6×15
∂
L
c
i
∂
L
w
∂
L
w
∂
δ
O
=
T
w
c
−
1
[
0
−
w
1
u
3
w
1
u
2
−
w
2
u
1
w
2
u
3
0
−
w
2
u
1
w
1
u
2
]
6
×
4
\frac{\partial \mathcal{L}^{c_{i}}}{\partial \mathcal{L}^{w}} \frac{\partial \mathcal{L}^{w}}{\partial \delta \mathcal{O}}=\mathcal{T}_{w c}^{-1}\left[\begin{array}{cccc}{\mathbf{0}} & {-w_{1} \mathbf{u}_{3}} & {w_{1} \mathbf{u}_{2}} & {-w_{2} \mathbf{u}_{1}} \\ {w_{2} \mathbf{u}_{3}} & {\mathbf{0}} & {-w_{2} \mathbf{u}_{1}} & {w_{1} \mathbf{u}_{2}}\end{array}\right]_{6 \times 4}
∂Lw∂Lci∂δO∂Lw=Twc−1[0w2u3−w1u30w1u2−w2u1−w2u1w1u2]6×4其中，因为我们程序中观测值为归一化平面上的坐标点，因此
∂
l
c
i
∂
L
c
i
\frac{\partial \mathbf{l}^{c_{i}}}{\partial \mathcal{L}^{c_{i}}}
∂Lci∂lci中第一项为单位阵，如果是图像平面上的点的话第一项为内参矩阵。在程序中我们写了一个LineProjectionFactor类来玩成这项工作。 在完成非线性优化之后就需要对优化变量进行更新，优化变量的更新方式在贺博的文章中是按照下面方式进行更新的
U
′
≈
U
(
I
+
[
δ
ψ
]
x
)
W
′
≈
W
(
I
+
[
0
−
δ
ϕ
δ
ϕ
0
]
)
\begin{aligned} \mathbf{U}^{\prime} & \approx \mathbf{U}\left(\mathbf{I}+[\delta \psi]_{x}\right) \\ \mathbf{W}^{\prime} & \approx \mathbf{W}\left(\mathbf{I}+\left[\begin{array}{cc}{0} & {-\delta \phi} \\ {\delta \phi} & {0}\end{array}\right]\right) \end{aligned}
U′W′≈U(I+[δψ]x)≈W(I+[0δϕ−δϕ0])但是经过我们讨论，和论文中不同的是，我们对于
U
\mathbf{U}
U采用的是四元数的更新方式，而
W
\mathbf{W}
W采用的是
ϕ
+
δ
ϕ
\phi + \delta\phi
ϕ+δϕ的更新方式，这一部分工作在LineLocalParameterization类中。
[1] Concha, A. , & Civera, J. . (2014). Using superpixels in monocular SLAM. IEEE International Conference on Robotics & Automation. IEEE. [2] Maity, S., Saha, A., & Bhowmick, B. (2017). Edge slam: Edge points based monocular visual slam. In Proceedings of the IEEE International Conference on Computer Vision (pp. 2408-2417). [3] Tomono, M. (2009, May). Robust 3D SLAM with a stereo camera based on an edge-point ICP algorithm. In 2009 IEEE International Conference on Robotics and Automation (pp. 4306-4311). IEEE. [4]He, Y., Zhao, J., Guo, Y., He, W., & Yuan, K. (2018). Pl-vio: Tightly-coupled monocular visual–inertial odometry using point and line features. Sensors, 18(4), 1159. [5] (2017). 基于点线综合特征的双目视觉SLAM方法. (Doctoral dissertation, 浙江大学). [6] https://github.com/MichaelGrupp/evo
