PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features

sensors 中科院贺一家博士
首先看下图，点线特征融合VIO系统的观测示意图

IMU预积分

同VINS-Mono

线特征几何表示

1.普吕克坐标表示

普吕克坐标表示用线特征方向向量和线特征与相机坐标系原点所构成的平面的法向量表示，如下所示
$\mathcal{L}=\left(\mathbf{n}^{\top}, \mathbf{d}^{\top}\right)^{\top} \in \mathbb{R}^{6}$L=(n⊤,d⊤)⊤∈R6
可以看出普吕克坐标表示有6个*度，而实际线特征只有4个*度，因此存在过参数话的问题，这就导致了这种线特征表示不能直接用于无约束优化问题中，但是这种表示有个好处就是利于三角化，且对线特征几何变换易于建模。如下所示设世界坐标系到相机的变化矩阵为
$\mathbf{T}_{c w}=\left[\begin{array}{cc}{\mathbf{R}_{c w}} & {\mathbf{p}_{c w}} \\ {0} & {\mathbf{1}}\end{array}\right]$Tcw​=[Rcw​0​pcw​1​]
那么线特征从世界坐标系转化到当前相机坐标系就非常简单，如下所示
$\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​]=Tcv​Lw​=[Rcw​0​[pcw​]×Rcw​​Rcw​​]Lw
线特征的 “三角化”。根据文中的介绍也十分详细了。如上图（b）所示，可以根据线特征端点在c1系下的归一化坐标，以及两个相机原点相对于c1系的坐标（按理说c1帧对应的原点坐标是（0,0,0），c2的要根据变换矩阵中的相对平移知道）就可以根据下面的公式得到c1系下的两个平面$\pi_1$π1​、$\pi_2$π2​。
$\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
$\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​=πx​x0​+πy​y0​+πz​z0​
然后得到普吕克矩阵表示
$\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∗中得到n和d。
2.正交表示
这种表示方式只有4个*度，主要在优化的过程中使用。表示为（U,W）
其中
$\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​​]
$\psi=\left[\psi_{1}, \psi_{2}, \psi_{3}\right]^{\top}$ψ=[ψ1​,ψ2​,ψ3​]⊤
$\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∥​]
因此在优化的过程中用下面的形式表示空间线特征
$\mathcal{O}=[\psi, \phi]^{\top}$O=[ψ,ϕ]⊤
普吕克坐标表示和U矩阵的转换如下
$\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]$[n​d​]=[∥n∥n​​∥d∥d​​∥n×d∥n×d​​]⎣⎡​∥n∥00​0∥d∥0​⎦⎤​
一旦空间3D线用正交表示形式优化之后，其相应的普吕克坐标表示相应的可以通过下式计算
$\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′=[w1​u1T​,w2​u2T​]T=(∥n∥2+∥d∥2)​1​L

视惯紧耦合

下图是视觉VO系统与VIO系统的因子图对比

本文采用基于滑窗的因子图优化算法，在i时刻滑窗内的所有状态量表示如下
$\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​,i​qwbi​,viw​,babi​​,bgbi​​​]⊤,i∈[n,n+N]​
总的目标函数如下
$\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​−Jp​X∥Σp​2​)​+i∈B∑​ρ(∥rb​(zbi​bi+1​​,X)∥Σbi​bi+1​​2​)+(i,j)∈F∑​ρ(∥∥∥​rf​(zfj′​ci​​,X)∥∥∥​Σij​ci​​ci​​)+(i,l)∈L∑​ρ(∥∥∥​rl​(zLi′​ci​​,X)∥∥∥​ΣLi​ci​​ci​​)​
各个状态量的更新方式略，其中特别注意的是q，u,w的增量更新方式。
在优化过程中我们每次迭代都是求解下面的方程
$\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​)
其中
$\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.线特征测量模型
线特征投影误差计算过程
${\mathcal{O}}$O -> $\mathcal{L}^{w}$Lw -> $\mathcal{L}^{c_i}$Lci​ -> $l^{c_i}$lci​ -> $r_l$rl​
其中
$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=⎣⎡​l1​l2​l3​​⎦⎤​=Knc=⎣⎡​fy​0−fy​cx​​0fx​−fx​cy​​00fx​fy​​⎦⎤​nc
$\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​(zLl​ci​​,X)=[d(slci​​,llci​​)d(elci​​,llci​​)​]
$d(\mathbf{s}, \mathbf{l})=\frac{\mathbf{s}^{\top} \mathbf{l}}{\sqrt{l_{1}^{2}+l_{2}^{2}}}$d(s,l)=l12​+l22​​s⊤l​
则线特征的投影误差相对状态量的雅克比矩阵可以由链式求导法则计算。
$\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​]
其中
$\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​​

$\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​[0w2​u3​​−w1​u3​0​w1​u2​−w2​u1​​−w2​u1​w1​u2​​]6×4​