# Semi-Direct Point-Line Visual Inertial Odometry for MAVs
## 3 Proposed System Implementation
### Back-End Optimization
#### 3.3.2 Improved Marginalization  
Marginalization is performed to bound the computational complexity, and the illustration is shown in Figure 5. If the second latest frame is a keyframe, the system will marginalize the oldest frame, including the pose of the oldest frame and some observed visual landmarks. When the oldest frame has co-visibility with other keyframes in the sliding window, that is, they observe the same visual landmarks, marginalization will keep the constraint on other keyframes. Otherwise, the system will retain the IMU measurements attached to this non-keyframe but remove the visual measurements. This process is solved by the Gauss-Newton iterative method and defined in the form \(\mathbf{H}\delta\mathbf{x}=\mathbf{b}\):  
\[\left[ \begin{array}{cc}\mathbf{H}_{m}&\mathbf{H}_{mp}\\ \mathbf{H}_{mp}^{T}&\mathbf{H}_{p}\end{array} \right]\left[ \begin{array}{c} \delta\mathbf{x}_{m}\\ \delta\mathbf{x}_{p}\end{array} \right]=\left[ \begin{array}{c}\mathbf{b}_{m} \\ \mathbf{b}_{p}\end{array} \right] \tag{24}\]  
Figure 5: Illustration of marginalization. where \(\mathbf{b}_{m}\) represents the set of states that are to be marginalized and \(\mathbf{b}_{p}\) represents the set of preserved states [23; 24]. Through the Schur complement, \(\delta\mathbf{x}_{p}\) can be obtained:  
\[(\mathbf{H}_{p}-\mathbf{H}_{mp}^{T}\mathbf{H}_{m}^{-1}\mathbf{H}_{mp})\delta \mathbf{x}_{p}=\mathbf{b}_{p}-\mathbf{H}_{mp}^{T}\mathbf{H}_{m}^{-1}\mathbf{b}_ {m} \tag{25}\]  
which describes the basic marginalization. Thus, the states in \(\mathbf{b}_{m}\) are marginalized together. Equation (25) requires calculating \(\mathbf{H}_{mp}^{T}\mathbf{H}_{m}^{-1}\mathbf{H}_{mp}\), in which the computational complexity is \(\mathcal{O}(i^{3}+i^{2}\times j+j^{2}\times i)\), supposing that \(\mathbf{H}_{m}\) and \(\mathbf{H}_{p}\) have dimensions of \(i\times i\) and \(j\times j\). The above basic marginalization method has high computational complexity, so we adopted the improved marginalization method to solve this problem. Firstly, we divided the state vector \(\mathbf{x}_{m}\) which was be marginalized into two parts: one was uncorrelated states, containing visual landmarks, the other was correlated states, containing the pose of keyframes, velocity and IMU bias. Uncorrelated states were not related to each other, only to correlated states [24]. We described this process using an example, as shown in Figure 6. The states that are to be marginalized are represented by dashed lines, where \(\mathbf{L}_{p_{i}}\in\mathbb{R}^{3}\) and \(\mathbf{L}_{l_{i}}\in\mathbb{R}^{6}\)\((1\leq i\leq 2)\) are points and line segments, respectively, \(\mathbf{P}_{j}\in\mathbb{R}^{6}\)\((1\leq j\leq 4)\) are the pose of keyframes, and \(\mathbf{b}_{k}\in\mathbb{R}^{9}\)\((1\leq k\leq 4)\) are velocity and IMU bias. Figure 7 shows the corresponding Hessian matrix. The state variable that is to be marginalized is \(\mathbf{x}_{m}(\mathbf{L}_{p_{1}},\mathbf{L}_{p_{2}},\mathbf{L}_{l_{i}}, \mathbf{L}_{l_{2}},\mathbf{P}_{1},\mathbf{b}_{1})\), while the state variable that is to be preserved is \(\mathbf{x}_{p}(\mathbf{P}_{2},\mathbf{b}_{2},\mathbf{P}_{3},\mathbf{b}_{3}, \mathbf{P}_{4})\). Figure 6: States in the sliding window. Figure 7: The Hessian matrix corresponding to Equation (24). Firstly, uncorrelated states in \(\mathbf{x}_{m}\) were marginalized, which only contain points and line segments (\(\mathbf{L}_{p_{1}}\), \(\mathbf{L}_{p_{2}}\), \(\mathbf{L}_{i_{1}}\) and \(\mathbf{L}_{i_{2}}\)). The computational complexity for the marginalization of \(\mathbf{L}_{p_{1}}\) in Figure 7 was mainly determined by \(\mathbf{H}_{mp_{1}}^{\mathbf{L}_{p_{1}}T}\mathbf{H}_{lr_{p_{1}}}^{-1}\mathbf{ H}_{mp}^{\mathbf{L}_{p_{1}}}\), which is approximately \(\mathcal{O}_{p_{1}}=\mathcal{O}(3^{3}+3^{2}\times 21+3\times 21^{2})\). The maximum computational complexity will be \(\max\mathcal{O}_{p_{1}}=\mathcal{O}(3^{3}+3^{2}\times 51+3\times 51^{2})\), supposing that \(\mathbf{L}_{p_{1}}\) can be observed by \(\mathbf{P}_{2}\), \(\mathbf{P}_{3}\) and \(\mathbf{P}_{4}\).