An Iterative 5-pt Algorithm for Fast and Robust Essential Matrix Estimation

The essential matrix, first introduced by Longuet-Higgins [5], is a 3× 3 matrix encoding the relative pose information between two views. Conventional approaches for relative pose estimation is to solve a system of linear equations. The 5-pt algorithm [6] is the current state-of-the-art algorithm in relative pose estimation. It is a minimal-set direct solver which solves the essential matrix as a system of polynomial equations. We show in this paper an iterative method which provides robust and real-time essential matrix estimation, capable of 30Hz, frame-rate performance. The benefit of an iterative approach lies in its simplicity and speed. The use of high degree polynomials may lead to ill-conditioning [1] and are often difficult to solve, leading to alternative methods which sacrifice speed for simplicity [1, 3, 4]. Although convergence is not guaranteed, when used within RANSAC, more hypotheses can be evaluated in the same block of time, yielding improved performance. While iterative solvers which minimizes the algebraic epipolar reprojection error have previously been proposed [2, 7], our parametrization is based on a novel geometric error which incorporates the half-plane constraint [8] and thus enforces orientation consistency between points. Figure 1 illustrates the concept of our iterative 5-pt algorithm. A coordinate frame is chosen such that the z-axis ez joins the two camera centres. In this frame, vectors vi and vi and ez are coplanar. The goal is to find a rotation for each of the two cameras that maps from their internal coordinate frame to that of Figure 1. We parametrize the image for each camera as a unit 2-sphere, mapping image points in normalized camera coordinates [x,y,1]T to unit vectors by dividing by √ x2 + y2 +1. At each iteration, the normalized point correspondences ui ↔ ui are left multiplied with the rotations R and R′, giving the rotated point correspondences v = Ru, v′ = R′u′. This rotates the two unit spheres, changing the direction of the epipoles. By rotating the unit spheres such that the z-axis ez is aligned with the epipoles, i.e. ez = Re = R′e′, vi↔ vi become coplanar with the epipoles e,e′. The epipoles can then be computed as e = RT ez, e′ = R′T ez. (1)