Feature matching is the process of matching M 2D key points from an image, to N stored 3D key points. The goodness of a match between two key points is computed using the descriptors that correspond to each point. In the implementation described here, the Sum of Absolute Different, or SAD, is used as the measure of how well two descriptors match. In particular, a smaller SAD score corresponds to a better match. However, since computing MxN SAD scores is computationally prohibitive, the SAD scores are only computed between M 2D points from the image and n<N 3D points from the sparse 3D map. These n points are selected based on the estimated position of the vehicle/robot. Then, from these scores, the correspondences that give the lowest cumulative SAD score is selected.

Once the feature correspondences are computed, the next step is to compute the pose of the vehicle/robot. In this implementation, a 6D pose is computed, that is, rotation as roll, pitch and yaw and translation as X, Y, Z. In particular, the Perspective-n-Point or PnP method is used for pose estimation. PnP is the problem of estimating the pose of a calibrated camera, given a set of n 3D points in the world, and their corresponding 2D projections in the image plane. In this implementation, the P3P scheme [4] is used in the RANdom SAmple Consensus (RANSAC) framework [5]. At a high level, this is an iterative process where point correspondences are randomly selected in each iteration to refine the pose estimate.

In the next section, the subtasks that make up the visual localization algorithm are mapped to the different components TDA4VM SoC, to showcase the seamless mapping that can be achieved with the SoC.