A fast dense stereo matching algorithm with an application to 3D occupancy mapping using quadrocopters

We propose a fast algorithm for computing stereo correspondences and correcting the mismatches. The correspondences are computed using stereo block matching and refined with a depth-aware method. We compute 16 disparities at the same time using SSE instructions. We evaluated our method on the Middlebury benchmark and obtained promosing results for practical realtime applications. The use of SSE instructions allows us to reduce the time needed to process the Tsukuba stereo pair to 8 milliseconds (125 fps) on a Core i5 CPU with 2×3.3 GHz. Our disparity refinement method has corrected 40% of the wrong matches with an additional computational time of 5.2% (0.41ms). The algorithm has been used to build 3D occupancy grid maps from stereo images. We used the datasets provided by the EuRoC Robotic Challenge. The reconstruction was accurate enough to perform realtime safe navigation.