# Semi-Direct Point-Line Visual Inertial Odometry for MAVs
## 3 Proposed System Implementation
### Back-End Optimization
#### 3.3.1 Sliding Window Formulation
(b) Point feature error term  
As shown in Figure 4, the point feature re-projection error is represented as the distance between the observed projection position and currently estimated projection position of the 3D point, which can be defined as:  
\[r_{\mathcal{P}}(\mathbf{\widehat{z}}_{p}^{c_{j}},\boldsymbol{\chi})=\mathbf{ p}_{p}^{c_{j}}-\pi_{\mathrm{c}}\Big{(}\mathbf{T}_{w}^{c_{j}}\cdot\mathbf{P}_{wp} \Big{)} \tag{14}\]  
where \(\mathbf{p}_{p}^{c_{j}}\) is the observed point in camera frame \(j\) and \(\mathbf{P}_{wp}\) is the matching 3D point. The Jacobian of the point feature re-projection error relative to the pose increment can be obtained by the chain rule:  
\[\frac{\partial r_{\mathcal{P}}}{\partial\delta\xi}=\frac{\partial r_{\mathcal{ P}}}{\partial\mathbf{p}}\frac{\partial\mathbf{p}}{\partial\delta\xi} \tag{15}\]  
with  
\[\frac{\partial r_{\mathcal{P}}}{\partial\mathbf{p}}=-\left[ \begin{array}{ cccc}\frac{f_{x}}{z_{c}}&0&-\frac{f_{x}x_{c}}{z_{c}^{2}}\\ 0&\frac{f_{y}}{z_{c}}&-\frac{f_{y}y_{c}}{z_{c}^{2}}\end{array} \right] \tag{16}\]  
\[\frac{\partial\mathbf{p}}{\partial\delta\xi}=\left[ \begin{array}{cccccc}1&0&0 &0&z_{c}&-y_{c}\\ 0&1&0&-z_{c}&0&-x_{c}\\ 0&0&1&y_{c}&x_{c}&0\end{array} \right] \tag{17}\]  
(c) Line feature error term  
Figure 4: Illustration of the visual reprojection errors. According to Figure 4, the line feature re-projection error is represented as the distance from two endpoints (\(\mathbf{p}_{s}=[u_{1}\ v_{1}\ 1]^{T}\) and \(\mathbf{p}_{e}=[u_{2}\ v_{2}\ 1]^{T}\)) of the matching line segment to the projecting line I. Combining Equations (5) and (6), this can be calculated as below [29]:  
\[r_{\mathcal{L}}(\mathbf{\tilde{z}}_{I}^{c_{i}},\mathbf{\chi})=\left[ \begin{array}[] {cc}d(\mathbf{p}_{s}^{c_{i}},\mathbf{f}_{i}^{c_{i}})\\ d(\mathbf{p}_{eI}^{c_{i}},\mathbf{f}_{i}^{c_{i}})\end{array} \right] \tag{18}\]  
where,  
\[d(\mathbf{p},\mathbf{1})=\frac{\mathbf{p}^{T}\mathbf{1}}{\sqrt{l_{1}^{2}+l_{2 }^{2}}} \tag{19}\]  
The Jacobian line feature re-projection error relative to the pose increment can be obtained as follows:  
\[\frac{\partial r_{\mathcal{L}}}{\partial\delta\xi}=\frac{\partial r_{ \mathcal{L}}}{\partial\mathbf{l}}\frac{\partial\mathbf{l}}{\partial\mathbf{L }_{\mathcal{L}}}\frac{\partial\mathbf{L}_{\mathcal{L}}}{\partial\delta\xi} \tag{20}\]  
with  
\[\frac{\partial r_{\mathcal{L}}}{\partial\mathbf{l}}=\left[ \begin{array}{ ccc}\frac{u_{1}l_{2}^{2}-l_{1}l_{2}v_{1}-l_{1}l_{3}}{\left(l_{1}^{2}+l_{2}^{2} \right)^{\frac{3}{2}}}&\frac{v_{1}l_{1}^{2}-l_{1}l_{2}u_{1}-l_{2}l_{3}}{\left( l_{1}^{2}+l_{2}^{2}\right)^{\frac{3}{2}}}&\frac{1}{\left(l_{1}^{2}+l_{2} \right)^{\frac{3}{2}}}\\ \frac{u_{2}l_{2}^{2}-l_{1}l_{2}v_{2}-l_{1}l_{3}}{\left(l_{1}^{2}+l_{2}^{2} \right)^{\frac{3}{2}}}&\frac{v_{2}l_{1}^{2}-l_{1}l_{2}u_{2}-l_{2}l_{3}}{\left( l_{1}^{2}+l_{2}\right)^{\frac{3}{2}}}&\frac{1}{\left(l_{1}^{2}+l_{2} \right)^{\frac{3}{2}}}\end{array} \right] \tag{21}\]  
\[\frac{\partial\mathbf{l}}{\partial\mathbf{L}_{\mathcal{L}}}=\left[ \begin{array} []{cccccc}f_{x}&0&0&0&0\\ 0&f_{y}&0&0&0&0\\ -f_{x}x_{x}&f_{x}c_{y}&f_{x}f_{y}&0&0&0\end{array} \right] \tag{22}\]  
\[\frac{\partial\mathbf{L}_{\mathcal{L}}}{\partial\delta\xi}=\left[ \begin{array} []{cccc}-[\mathbf{R}_{\omega}^{c}\mathbf{n}_{\omega}]_{\times}-[\mathbf{p}_{w }^{c}]_{\times}[\mathbf{R}_{\omega}^{c}\mathbf{d}_{w}]_{\times}&-[\mathbf{R}_{ \omega}^{c}\mathbf{d}_{w}]_{\times}\\ -[\mathbf{R}_{\omega}^{c}\mathbf{d}_{w}]_{\times}&&&0\end{array} \right] \tag{23}\]