sensors 中科院贺一家博士
首先看下图,点线特征融合VIO系统的观测示意图
IMU预积分
同VINS-Mono
线特征几何表示
1.普吕克坐标表示
普吕克坐标表示用线特征方向向量和线特征与相机坐标系原点所构成的平面的法向量表示,如下所示
L
=
(
n
⊤
,
d
⊤
)
⊤
∈
R
6
\mathcal{L}=\left(\mathbf{n}^{\top}, \mathbf{d}^{\top}\right)^{\top} \in \mathbb{R}^{6}
L=(n⊤,d⊤)⊤∈R6
可以看出普吕克坐标表示有6个自由度,而实际线特征只有4个自由度,因此存在过参数话的问题,这就导致了这种线特征表示不能直接用于无约束优化问题中,但是这种表示有个好处就是利于三角化,且对线特征几何变换易于建模。如下所示设世界坐标系到相机的变化矩阵为
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}} \\ {0} & {\mathbf{1}}\end{array}\right]
Tcw=[Rcw0pcw1]
那么线特征从世界坐标系转化到当前相机坐标系就非常简单,如下所示
L
c
=
[
n
c
d
c
]
=
T
c
v
L
w
=
[
R
c
w
[
p
c
w
]
×
R
c
w
0
R
c
w
]
L
w
\mathcal{L}^{c}=\left[\begin{array}{c}{\mathbf{n}^{c}} \\ {\mathbf{d}^{c}}\end{array}\right]=\mathcal{T}_{c v} \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]=TcvLw=[Rcw0[pcw]×RcwRcw]Lw
线特征的 “三角化”。根据文中的介绍也十分详细了。如上图(b)所示,可以根据线特征端点在c1系下的归一化坐标,以及两个相机原点相对于c1系的坐标(按理说c1帧对应的原点坐标是(0,0,0),c2的要根据变换矩阵中的相对平移知道)就可以根据下面的公式得到c1系下的两个平面
π
1
\pi_1
π1、
π
2
\pi_2
π2。
π
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
然后得到普吕克矩阵表示
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} \pi_{2}^{\top}-\pi_{2} \pi_{1}^{\top} \in \mathbb{R}^{4 \times 4}
L∗=[[d]×−n⊤n0]=π1π2⊤−π2π1⊤∈R4×4
最后可以从矩阵
L
∗
L^{*}
L∗中得到n和d。
2.正交表示
这种表示方式只有4个自由度,主要在优化的过程中使用。表示为(U,W)
其中
U
=
R
(
ψ
)
=
[
n
∥
n
∥
d
∥
d
∥
n
×
d
∥
n
×
d
∥
]
\mathbf{U}=\mathbf{R}(\boldsymbol{\psi})=\left[\begin{array}{ccc}{\frac{\mathbf{n}}{\|\mathbf{n}\|}} & {\frac{\mathbf{d}}{\|\mathbf{d}\|}} & {\frac{\mathbf{n} \times \mathbf{d}}{\|\mathbf{n} \times \mathbf{d}\|}}\end{array}\right]
U=R(ψ)=[∥n∥n∥d∥d∥n×d∥n×d]
ψ
=
[
ψ
1
,
ψ
2
,
ψ
3
]
⊤
\psi=\left[\psi_{1}, \psi_{2}, \psi_{3}\right]^{\top}
ψ=[ψ1,ψ2,ψ3]⊤
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∥]
因此在优化的过程中用下面的形式表示空间线特征
O
=
[
ψ
,
ϕ
]
⊤
\mathcal{O}=[\psi, \phi]^{\top}
O=[ψ,ϕ]⊤
普吕克坐标表示和U矩阵的转换如下
[
n
d
]
=
[
n
∥
n
∥
d
∥
d
∥
n
×
d
∥
n
×
d
∥
]
[
∥
n
∥
0
0
∥
d
∥
0
0
]
\left[\begin{array}{ll}{\mathbf{n}} & {\mathbf{d}}\end{array}\right]=\left[\begin{array}{lll}{\frac{n}{\|\mathbf{n}\|}} & {\frac{\mathrm{d}}{\|\mathbf{d}\|}} & {\frac{\mathbf{n} \times \mathbf{d}}{\|\mathbf{n} \times \mathbf{d}\|}}\end{array}\right]\left[\begin{array}{cc}{\|\mathbf{n}\|} & {0} \\ {0} & {\|\mathbf{d}\|} \\ {0} & {0}\end{array}\right]
[nd]=[∥n∥n∥d∥d∥n×d∥n×d]⎣⎡∥n∥000∥d∥0⎦⎤
一旦空间3D线用正交表示形式优化之后,其相应的普吕克坐标表示相应的可以通过下式计算
L
′
=
[
w
1
u
1
T
,
w
2
u
2
T
]
T
=
1
(
∥
n
∥
2
+
∥
d
∥
2
)
L
\mathcal{L}^{\prime}=\left[w_{1} \mathbf{u}_{1}^{T}, w_{2} \mathbf{u}_{2}^{T}\right]^{T}=\frac{1}{\sqrt{\left(\|\mathbf{n}\|^{2}+\|\mathbf{d}\|^{2}\right)}} \mathcal{L}
L′=[w1u1T,w2u2T]T=(∥n∥2+∥d∥2)1L
视惯紧耦合
下图是视觉VO系统与VIO系统的因子图对比
本文采用基于滑窗的因子图优化算法,在i时刻滑窗内的所有状态量表示如下
X
=
[
x
n
,
x
n
+
1
,
…
,
x
n
+
N
,
λ
m
,
λ
m
+
1
,
…
,
λ
m
+
M
,
O
0
,
O
0
+
1
,
…
,
O
0
+
O
]
⊤
x
i
=
[
p
w
b
i
,
i
q
w
b
i
,
v
i
w
,
b
a
b
i
,
b
g
b
i
]
⊤
,
i
∈
[
n
,
n
+
N
]
\begin{array}{l}{\mathcal{X}=\left[\mathbf{x}_{n}, \mathbf{x}_{n+1}, \ldots, \mathbf{x}_{n+N}, \lambda_{m}, \lambda_{m+1}, \ldots, \lambda_{m+M}, \mathcal{O}_{0}, \mathcal{O}_{0+1}, \ldots, \mathcal{O}_{0+O}\right]^{\top}} \\ {\mathbf{x}_{i}=\left[\mathbf{p}_{w b_{i}, i} \mathbf{q}_{w b_{i}, \mathbf{v}_{i}^{w}, \mathbf{b}_{a}^{b_{i}}, \mathbf{b}_{g}^{b_{i}}}\right]^{\top}, i \in[n, n+N]}\end{array}
X=[xn,xn+1,…,xn+N,λm,λm+1,…,λm+M,O0,O0+1,…,O0+O]⊤xi=[pwbi,iqwbi,viw,babi,bgbi]⊤,i∈[n,n+N]
总的目标函数如下
min
X
ρ
(
∥
r
p
−
J
p
X
∥
Σ
p
2
)
+
∑
i
∈
B
ρ
(
∥
r
b
(
z
b
i
b
i
+
1
,
X
)
∥
Σ
b
i
b
i
+
1
2
)
+
∑
(
i
,
j
)
∈
F
ρ
(
∥
r
f
(
z
f
j
′
c
i
,
X
)
∥
Σ
i
j
c
i
c
i
)
+
∑
(
i
,
l
)
∈
L
ρ
(
∥
r
l
(
z
L
i
′
c
i
,
X
)
∥
Σ
L
i
c
i
c
i
)
\begin{aligned} \min _{\mathcal{X}} \rho\left(\left\|\mathbf{r}_{p}-\mathbf{J}_{p} \mathcal{X}\right\|_{\Sigma_{p}}^{2}\right) &+\sum_{i \in B} \rho\left(\left\|\mathbf{r}_{b}\left(\mathbf{z}_{b_{i} b_{i+1}}, \mathcal{X}\right)\right\|_{\Sigma_{b_{i} b_{i+1}}}^{2}\right) \\ &+\sum_{(i, j) \in F} \rho\left(\left\|\mathbf{r}_{f}\left(\mathbf{z}_{\mathbf{f}_{j}^{\prime}}^{c_{i}}, \mathcal{X}\right)\right\|_{\Sigma_{i_{j}}^{c_{i}}}^{c_{i}}\right)+\sum_{(i, l) \in L} \rho\left(\left\|\mathbf{r}_{l}\left(\mathbf{z}_{\mathcal{L}_{i}^{\prime}}^{c_{i}}, \mathcal{X}\right)\right\|_{\Sigma_{\mathcal{L}_{i}}^{c_{i}}}^{c_{i}}\right) \end{aligned}
Xminρ(∥rp−JpX∥Σp2)+i∈B∑ρ(∥rb(zbibi+1,X)∥Σbibi+12)+(i,j)∈F∑ρ(∥∥∥rf(zfj′ci,X)∥∥∥Σijcici)+(i,l)∈L∑ρ(∥∥∥rl(zLi′ci,X)∥∥∥ΣLicici)
各个状态量的更新方式略,其中特别注意的是q,u,w的增量更新方式。
在优化过程中我们每次迭代都是求解下面的方程
(
H
p
+
H
b
+
H
f
+
H
l
)
δ
X
=
(
b
p
+
b
b
+
b
f
+
b
l
)
\left(\mathbf{H}_{p}+\mathbf{H}_{b}+\mathbf{H}_{f}+\mathbf{H}_{l}\right) \delta \mathcal{X}=\left(\mathbf{b}_{p}+\mathbf{b}_{b}+\mathbf{b}_{f}+\mathbf{b}_{l}\right)
(Hp+Hb+Hf+Hl)δX=(bp+bb+bf+bl)
其中
δ
X
=
[
δ
x
n
,
δ
x
n
+
1
,
…
,
δ
x
n
+
N
,
δ
λ
m
,
δ
λ
m
+
1
,
…
,
δ
λ
m
+
M
,
δ
O
0
,
δ
O
o
+
1
,
…
,
δ
O
o
+
O
]
⊤
δ
x
i
=
[
δ
p
,
δ
θ
,
δ
v
,
δ
b
a
b
i
,
δ
b
g
b
i
]
⊤
,
i
∈
[
n
,
n
+
N
]
\begin{aligned} \delta \mathcal{X} &=\left[\delta \mathbf{x}_{n}, \delta \mathbf{x}_{n+1}, \ldots, \delta \mathbf{x}_{n+N}, \delta \lambda_{m}, \delta \lambda_{m+1}, \ldots, \delta \lambda_{m+M}, \delta \mathcal{O}_{0}, \delta \mathcal{O}_{o+1}, \ldots, \delta \mathcal{O}_{o+O}\right]^{\top} \\ \delta \mathbf{x}_{i} &=\left[\delta \mathbf{p}, \delta \boldsymbol{\theta}, \delta \mathbf{v}, \delta \mathbf{b}_{a}^{b_{i}}, \delta \mathbf{b}_{g}^{b_{i}}\right]^{\top}, i \in[n, n+N] \end{aligned}
δXδxi=[δxn,δxn+1,…,δxn+N,δλm,δλm+1,…,δλm+M,δO0,δOo+1,…,δOo+O]⊤=[δp,δθ,δv,δbabi,δbgbi]⊤,i∈[n,n+N]
H矩阵是各个残差项相对状态量的雅克比矩阵计算出来的,下面则是各个残差项对应的雅克比矩阵的推导。
1.IMU测量模型
IMU预积分误差,同VINS-Mono
2.点特征测量模型
重投影误差,同VINS-Mono
3.线特征测量模型
线特征投影误差计算过程
O
{\mathcal{O}}
O ->
L
w
\mathcal{L}^{w}
Lw ->
L
c
i
\mathcal{L}^{c_i}
Lci ->
l
c
i
l^{c_i}
lci ->
r
l
r_l
rl
其中
1
=
[
l
1
l
2
l
3
]
=
K
n
c
=
[
f
y
0
0
0
f
x
0
−
f
y
c
x
−
f
x
c
y
f
x
f
y
]
n
c
1=\left[\begin{array}{l}{l_{1}} \\ {l_{2}} \\ {l_{3}}\end{array}\right]=\mathcal{K} \mathbf{n}^{c}=\left[\begin{array}{ccc}{f_{y}} & {0} & {0} \\ {0} & {f_{x}} & {0} \\ {-f_{y} c_{x}} & {-f_{x} c_{y}} & {f_{x} f_{y}}\end{array}\right] \mathbf{n}^{c}
1=⎣⎡l1l2l3⎦⎤=Knc=⎣⎡fy0−fycx0fx−fxcy00fxfy⎦⎤nc
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}}^{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(zLlci,X)=[d(slci,llci)d(elci,llci)]
d
(
s
,
l
)
=
s
⊤
l
l
1
2
+
l
2
2
d(\mathbf{s}, \mathbf{l})=\frac{\mathbf{s}^{\top} \mathbf{l}}{\sqrt{l_{1}^{2}+l_{2}^{2}}}
d(s,l)=l12+l22s⊤l
则线特征的投影误差相对状态量的雅克比矩阵可以由链式求导法则计算。
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
)
⊤
l
(
l
1
2
+
l
2
2
)
(
3
2
)
+
u
s
(
l
1
2
+
l
2
2
)
(
1
2
)
−
l
2
(
s
l
c
i
)
⊤
l
(
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
2
)
⊤
l
⊤
l
(
l
1
2
+
l
2
2
)
(
3
2
)
+
u
e
(
l
1
2
+
l
2
2
)
(
1
2
)
−
l
2
(
e
l
2
)
⊤
l
⊤
(
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}{ll}{\frac{-l_{1}\left(\mathbf{s}_{l}^{c_{i}}\right)^{\top} \mathbf{l}}{\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{l}}{\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}^{2}\right)^{\top} \mathbf{l}^{\top} \mathbf{l}}{\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}^{2}\right)^{\top} \mathbf{l}^{\top}}{\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)}}} & {\left.\frac{1}{\left(l_{1}^{2}+l_{2}^{2}\right)^{\left(\frac{1}{2}\right)}}\right]_{2 \times 3}}\end{array}\right.
∂lci∂rl=⎣⎢⎢⎢⎡(l12+l22)(23)−l1(slci)⊤l+(l12+l22)(21)us(l12+l22)(23)−l1(el2)⊤l⊤l+(l12+l22)(21)ue(l12+l22)(23)−l2(slci)⊤l+(l12+l22)(21)vs(l12+l22)(23)−l2(el2)⊤l⊤+(l12+l22)(21)ve(l12+l22)(21)1(l12+l22)(21)1]2×3
∂
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