## 4 FAST-FUSION Approach
### Omnidirectional Visual Odometry
Starting with a set of 2-D feature matches \((\mu_{c},\mu_{p})\) between the previous \(p\) and current \(c\) images \(\{\mu_{p}\}\longleftrightarrow\{\mu_{c}\}\) with \(\mu_{p}=\{x^{\prime\prime}_{p_{1}},x^{\prime\prime}_{p_{2}},...,x^{\prime \prime}_{p_{n}}\}\) and \(\mu_{c}=\{x^{\prime\prime}_{c_{1}},x^{\prime\prime}_{c_{2}},...,x^{\prime \prime}_{c_{n}}\}\) we apply \(M\) to them obtaining a set of 3-D unit vector matches \(\{\eta_{p}\}\longleftrightarrow\{\eta_{c}\}\) with \(\eta_{p}=\{v_{p_{1}},v_{p_{2}},...,v_{p_{n}}\}\) and \(\eta_{c}=\{v_{c_{1}},v_{c_{2}},...,v_{c_{n}}\}\). Since we use 3-D unit vectors, the conventional epipolar geometry configuration is not suitable. This being said, we use the configuration present in Figure 4.  
In this, instead of projecting the line that contains the previous camera center and the scene point into the current image plane--the so-called epipolar line-- we project it into the current unit sphere. With this configuration, we have now epipolar curves (represented in red in Figure 4) instead of epipolar lines. This means that a point on the unit sphere correspondent to the current image that matches a point on the unit sphere correspondent to the last image lies on an epipolar curve. With this, we have the RANSAC input prepared--the entire set of 3-D unit vector matches--and we are able to compute this method solving for the essential matrix \(E\). So for each iteration, we select a random sample of size eight from the total set of unit vectors and solve  
\[v_{p_{i}}Ev_{c_{i}}=0\]  
Figure 4: Epipolar geometry configuration. for each one. So, for each match we have  
\[ \begin{bmatrix}x_{p_{i}}&y_{p_{i}}&z_{p_{i}}\end{bmatrix} E \begin{bmatrix}x_{c_{i}} \\ y_{c_{i}}\\ z_{c_{i}}\end{bmatrix} =0 \tag{4}\]  
which results in  
\[ \begin{bmatrix}x_{p_{1}}x_{c_{1}}&x_{p_{1}}y_{c_{1}}&x_{p_{1}}z_{c_{1}}&y_{p_{1 }}x_{c_{1}}&y_{p_{1}}y_{c_{1}}&y_{p_{1}}z_{c_{1}}&z_{p_{1}}x_{c_{1}}&z_{p_{1}}y_ {c_{1}}&z_{p_{1}}z_{c_{1}}\end{bmatrix} E^{\prime}=0 \tag{5}\]  
where \(E^{\prime}= \begin{bmatrix}E_{11}&E_{12}&E_{13}&E_{21}&E_{22}&E_{23}&E_{31}&E_{32 }&E_{33}\end{bmatrix} ^{T}\). Using single value decomposition (SVD), the solution of \(E^{\prime}\) and, consequently, of \(E\) are extracted. To calculate the set of inliers we follow the original Libviso2 approach--iterate trough all the matches and use the Sampson Distance [36] to filter the outliers. At the end of all the RANSAC iterations, the final set of matches is available and will be used to compute the essential matrix \(E\). Finally, the essential matrix \(E\) is computed using this set of matches and imposing the rank-2 constraint to it. Then we extract the camera motion \([R|t]\) from it. However, for a given essential matrix \(E\) and considering the previous camera center as reference axis, i.e., \(P_{p}=[I|0]\), there are four different solutions for the current camera matrix \(P_{c}=[R|t]\). To extract the correct solution, a linear triangulation approach is used. Each 3-D unit vector match is triangulated in the following way:  
\[ \begin{cases}\alpha v_{p_{i}}=P_{p}X_{i}\\ \alpha v_{p_{c}}=P_{c}X_{i}\end{cases} \implies \begin{cases}v_{p_{i}}\times P_{p }X_{i}=0\\ v_{c_{i}}\times P_{c}X_{i}=0\end{cases}  \tag{6}\]  
by extending is obtained:  
\[ \begin{cases}x_{p_{i}}p_{p}^{3}-z_{p_{i}}P_{p}^{1}\\ x_{p_{i}}P_{p}^{2}-y_{p_{i}}P_{p}^{1}\\ y_{p_{i}}P_{p}^{3}-z_{p_{i}}P_{p}^{2}\\ x_{c_{i}}P_{c}^{3}-z_{c_{i}}P_{c}^{1}\\ x_{c_{i}}P_{c}^{2}-y_{c_{i}}P_{c}^{1}\\ y_{c_{i}}P_{c}^{3}-z_{c_{i}}P_{c}^{2}\end{cases} X_{i}=0 \tag{7}\]  
where \(X_{i}=[x_{t_{i}}y_{t_{i}}z_{t_{i}}]^{T}\) and the superscript \(P^{i}\) denotes the \(j\)-th row of the projection matrix. Solving the linear equation for all the matches considering the four possible \([R|t]\) solutions and choosing the one that presents the higher number of 3-D triangulated points with positive depth results in the final solution for the camera motion.