## 4 FAST-FUSION Approach
### Motion Scale Calculation  
To complement the camera motion estimation from our omnidirectional VO approach, a planar LIDAR sensor is considered to recover the scale factor. This approach is divided into four essential steps:  
1. Transformation of the range measurement of the LIDAR into the camera referential frame;
2. projection of the LIDAR measures in the camera referential frame into the omnidirectional image;
3. search for associations between image features and LIDAR measures in the omnidirectional image; and
4. scale calculation using the associations found. To perform the transformation of the range measures to the camera referential frame, we measured the physical distance from the camera center to the LIDAR. We apply a transformation \(H=[R|t]\) to the range measures to convert them to the desired referential. The transformation corresponds to the displacement between the two referentials. For each range measure, we perform the following transformation:  
\[\psi_{i}=H \begin{bmatrix}\kappa_{i}cos(\theta_{i})\\ \kappa_{i}sin(\theta_{i})\\ 0\end{bmatrix}  \tag{8}\]  
where \(\psi_{i}=[x_{l_{i}}\ y_{l_{i}}\ z_{l_{i}}]^{T}\), \(\kappa_{i}\) is the range measure and \(\theta_{i}\) is its correspondent angle. After that, to obtain the range measures as 2-D pixel points in the omnidirectional image, the inverse camera model \(M^{-1}\) described in Definition 2 is applied. The final set of 2-D range measures are the ones who are mapped inside the omnidirectional image. In other words, the ones who are inside the camera field of view. The next step consists in associating the LIDAR measures projected into the image with 2-D feature points present in the current image frame. To do so, a search on the 2-D LIDAR measures neighborhood is computed. As linear searching is computationally expensive, a simplification was performed using the assumption that the vertical standard deviation of LIDAR measures on the image is small. While projecting them into the image, their average \(y\) coordinate is computed. In this way, in the first stage, we search for features that present vertical distance to the average smaller than 10 pixels. This allowed us to highly reduce the number of features that are searched in the neighborhood of the LIDAR measures. Then a search is performed in the selected set of features for the ones who present a horizontal pixel distance smaller than five pixels from each LIDAR measure. A representative scheme of this formulation can be found in Figure 5. In Figure 5a is represented the projection of the LIDAR measures in the omnidirectional image viewed from the side. Figure 5b represents the neighborhoods of each projection given by the interception of the yellow zone that represents the first search in \(y\) with the blue delimiters that represent the \(x\) tolerance of search. The green dots represent image features. Figure 6 shows the real projection of LIDAR measures in the omnidirectional image in black and the associated features in white. A small error is visible associated with the projection due to calibration errors, camera model imperfections, and errors in the projection \(H\). This error does not have a high impact on the final estimation of scale due to the filter effect of the performed average described bellow. In other words, the matches between LIDAR measures and 2-D image features that represent a physical outlier, i.e., that do not describe the same point in the world, get diluted in all the other inliers. After matching 2-D feature points with 2-D range measures, it is possible to estimate the scale factor. These feature points were already triangulated using their respective matches in the previous image frame. Thus, we already have a set of matches between the 2-D LIDAR measures and 3-D triangulated feature points.