# Semi-Direct Point-Line Visual Inertial Odometry for MAVs
## 3 Proposed System Implementation
### Back-End Optimization  
#### 3.3.1 Sliding Window Formulation  
The sliding window [28] limits the number of keyframes and prevents the number of poses and features from the over time, so that back-end optimization is always within a limited complexity. The full state variables in the sliding window are defined as:  
\[ \begin{array}{l}\boldsymbol{\chi}=\big{[}\mathbf{x}_{1},\mathbf{x}_{2},..., \mathbf{x}_{n},\mathbf{P}_{wi},\mathbf{P}_{w2},...,\mathbf{P}_{wn},\mathbf{L} _{w1},\mathbf{L}_{w2},...,\mathbf{L}_{w0}\big{]}\\ \mathbf{x}_{i}=\big{[}\mathbf{p}_{b_{i}}^{w},\mathbf{w}_{b_{i}}^{w},\mathbf{q} _{b_{i}}^{w},\mathbf{q}_{a},\mathbf{b}_{g}\big{]},i\in[1,n].\end{array}  \tag{11}\]  
where \(x_{i}\) is the \(i\)th IMU state. \(\mathbf{p}_{b_{i}}^{w}\), \(\mathbf{w}_{b_{i}}^{w}\), and \(\mathbf{q}_{b_{i}}^{w}\) represent position, velocity and orientation, respectively. \(\mathbf{b}_{a}\) and \(\mathbf{b}_{g}\) represent acceleration bias and gyroscope bias, respectively. \(\mathbf{P}_{wi}(i\in[1,m])\) represent the point landmarks, and \(\mathbf{L}_{wi}(i\in[1,o])\) are the 3D line representation formed by the Plucker coordinates. \(m\) and \(o\) are the number of point and line features in the sliding window, respectively. The cost function, which simultaneously optimizes visual and IMU variables, is shown in Equation (12):  
\[\min_{\chi}\Bigg{\{}\|r_{p}-\mathbf{H}_{p}\boldsymbol{\chi}\|^{2}+\sum\limits _{k\in\mathcal{B}}{r_{\mathcal{B}}\|(\tilde{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \boldsymbol{\chi})\|_{\sum_{b_{k+1}}^{b_{k}}}^{2}}+\sum\limits_{p\in\mathcal{P }}{\rho(r_{\mathcal{P}}\|(\tilde{\mathbf{z}}_{p}^{c_{j}},\boldsymbol{\chi}) \|_{\sum_{p}^{c_{j}}}^{2})}+\sum\limits_{l\in\mathcal{L}}{\rho(r_{\mathcal{L }}\|(\tilde{\mathbf{z}}_{l}^{c_{j}},\boldsymbol{\chi})\|_{\sum_{l}^{c_{j}}}^{2} )}\Bigg{\}} \tag{12}\]  
where \(\{r_{p},\mathbf{H}_{p}\}\) are the prior information from marginalization, \(r_{g}(\tilde{\mathbf{z}}_{b_{k+1}}^{b_{k}},\boldsymbol{\chi})\) are the IMU measurement residuals, \(\mathcal{B}\) is the set of IMU states. \(r_{\mathcal{P}}(\tilde{\mathbf{z}}_{p}^{c_{j}},\boldsymbol{\chi})\) and \(r_{\mathcal{L}}(\tilde{\mathbf{z}}_{l}^{c_{j}},\boldsymbol{\chi})\) are the point and line feature re-projection errors, respectively. \(\mathcal{P}\) and \(\mathcal{L}\) are the set of observed point and line features, and \(\rho\) is the Cauchy loss function, which minimizes the influence of outliers. Therefore, the matching error terms can be expressed as follows:  
(a) IMU measurement residual  
Figure 3: Pose refinement. According to Equations (1)-(3), the IMU measurement residuals for two consecutive frames can be calculated as follows:  
\[r_{\mathcal{B}}(\mathbf{\widehat{z}}_{b_{k+1}}^{b_{k}},\boldsymbol{\chi})=\left\{ \begin{array}{ll}\delta\boldsymbol{a}_{b_{k+1}}^{b_{k}}&=\\ \delta\boldsymbol{\theta}_{b_{k+1}}^{b_{k}}&=\\ \delta\boldsymbol{\theta}_{b_{k+1}}^{b_{k}}&=\\ \delta\boldsymbol{\theta}_{b_{k+1}}^{b_{k}}&=\\ \delta\boldsymbol{\theta}_{b_{k}}^{b_{k}}&=\\ \delta\boldsymbol{\theta}_{a}&=\\ \delta\boldsymbol{\mathbf{b}}_{g}&=\\ \end{array} \right.\left\{ \begin{array}{ll}\mathbf{R}_{w}^{b_{k}}(\mathbf{ p}_{b_{k+1}}^{w}-\mathbf{p}_{b_{k}}^{w}-\mathbf{v}_{b_{k}}^{w}\Delta t_{k}+\frac{1}{2} \mathbf{g}^{w}\Delta t_{k}^{2})-\mathbf{\widehat{a}}_{b_{k+1}}^{b_{k}}\\ \mathbf{R}_{w}^{b_{k}}(\mathbf{v}_{b_{k+1}}^{w}-\mathbf{v}_{b_{k}}^{w}+ \mathbf{g}^{w}\Delta t_{ij})-\mathbf{\widehat{\beta}}_{b_{k+1}}^{b_{k}}\\ 2[\mathbf{q}_{b_{k}}^{w-1}\otimes\mathbf{q}_{b_{k+1}}^{w}\otimes(\widehat{ \gamma}_{b_{k+1}}^{b_{k}})^{-1}]_{xyz}\\ \mathbf{b}_{ab_{k+1}}-\mathbf{b}_{ab_{k}}\\ \mathbf{b}_{ab_{k+1}}-\mathbf{b}_{ab_{k}}\\ \mathbf{b}_{ab_{k+1}}-\mathbf{b}_{ab_{k}}\end{array} \right. \tag{13}\]  
where \([*]_{xyz}\) extracts the vector part of the quaternion.