## 4 FAST-FUSION Approach
### Heterogeneous Computing Optimizations  
After having the previously described fusion working on top of a standard computer, we needed it to be fast in an embedded configuration to run on the robot in real-time. So, we chose a low-cost microprocessor--Raspberry Pi 3B--and we tried to optimize the developed code to run on this platform. To do so, we use both Raspberry Pi's CPU and GPU with parallel computing techniques.  
To access Raspberry Pi's GPU, the VC4 CL ([[https://github.com/doe300/VC4](https://github.com/doe300/VC4) CL]([https://github.com/doe300/VC4](https://github.com/doe300/VC4) CL)) driver was used. This is an open-source OpenCL 1.2 implementation for Raspberry Pi's GPU that allows the use of OpenCL C++. To facilitate OpenCL usage, an additional layer of abstraction consisting of an OpenCL-wrapper for C++ and ROS was developed. This allowed a communication between the host CPU and the device GPU using simple write and read routines. The implementation layout is represented in Figure 7.  
After validating this tool with small kernels like performing arithmetic operations in arrays we moved to the VO algorithm parallelization.  
After profiling the VO implementation, we concluded that RANSAC was the most time-consuming block. It runs over 2000 iterations performing several loops and solving equations using SVD in each one. In short, this method is constituted by three main steps:  
* randomSample()--calculation of a random set of matches of size 8;
* essentialMatrix()--calculation of the essential matrix \(E\) using the given set; and,
* getInliers()--calculation of the set of inliers for the given essential matrix \(E\).  
As the essentialMatrix() routine uses complex calculations it is hard to parallelize in GPU context. We optimize both randomSample() and getInliers() in GPU and essentialMatrix() in CPU.  
For the GPU-based optimizations, a 16-way single instruction multiple data (SIMD) kernel architecture was adopted since each quad processing unit (QPU) of this device uses 16-way SIMD, executing an instruction with four-way data parallelism, four cycles in a row. This way, our approach for each routine follows the following pattern:  
1. Load the routine input data correspondent to all the RANSAC iterations.
2. Write all the data to the correspondent kernel at once using 16-way vector types.
3. Execute the kernel to all the data in a 16-way vectorized way and load it to a single output array.
4. Read all the output data at once and label the corresponding RANSAC iteration to it.  
In this way, we maximize the GPU performance using vectorized kernels that match with its architecture. Also, we minimize the data transfer delays between the host and the device by performing the communication only once for writing and once for reading.  
To optimize the essentialMatrix() routine on the CPU, we take advantage of its four cores creating four threads. Each one computes a quarter of all the essential matrices corresponding to a quarter of the total number of RANSAC iterations.  
The final solution scheme is represented in Figure 8.  
We achieved a parallel RANSAC method that uses both CPU and GPU optimizations.  
Figure 7: Unified modeling language (UML) diagram of the OpenCL abstraction layer implemented.