General techniques for approximate incidences and their application to the camera posing problem

Camera pose estimation is a fundamental problem in computer vision, which aims at determining the pose and orientation of a camera solely from an image. This localization problem appears in many interesting real-world applications, such as for the navigation of self-driving cars [5], in incremental environment mapping such as Structure-from-Motion (SfM) [1, 11, 13], or for augmented reality [8, 9, 14], where a significant component are algorithms that aim to estimate an accurate camera pose in the world from image data.

Given a three-dimensional point-cloud model of a scene, the classical, but also state-of-the-art approach to absolute camera pose estimation consists of a two-step procedure. First, one matches a large number of features in the two-dimensional camera image with corresponding features in the three-dimensional scene. Then one uses these putative correspondences to determine the pose and orientation of the camera. Typically, the matches obtained in the first step contain many incorrect associations, forcing the second step to use filtering techniques to reject incorrect matches. Subsequently, the absolute 6 degrees-of-freedom (DoF) camera pose is estimated, for example, with a perspective $n$-point pose solver [6] within a RANSAC scheme [4].

In this work we concentrate on the second step of the camera pose problem. That is, we consider the task of estimating the camera pose and orientation from a (potentially large) set of $n$ already calculated image-to-scene correspondences.

Further, we assume that we are given a common direction between the world and camera frames. For example, inertial sensors, available on any smart-phone nowadays, allow to estimate the vertical gravity direction in the three-dimensional camera coordinate system. This alignment of the vertical direction fixes two degrees of freedom for the rotation between the frames and we are left to estimate four degrees of freedom out of the general six. To obtain four equations (in the four remaining degrees of freedom), this setup requires two pairs of image-to-scene correspondences¹¹1As we will see later in detail, each correspondence imposes two constraints on the camera pose. for a minimal solver. Hence a corresponding naive RANSAC-based scheme requires $O(n^{2})$ filtering steps, where in each iterations a pose hypothesis based on a different pair of correspondences is computed and verified against all other correspondences.

Recently, Zeisl et al. [17] proposed a Hough-voting inspired outlier filtering and camera posing approach, which computes the camera pose up to an accuracy of ${\varepsilon}>0$ from a set of $2$D-$3$D correspondences, in $O(n/{\varepsilon}^{2})$ time, under the same alignment assumptions of the vertical direction. In this paper we propose new algorithms that work considerably faster in practice, but under milder assumptions. Our method is based on a reduction of the problem to a problem of counting ${\varepsilon}$-approximate incidences between points and surfaces, where a point $p$ is ${\varepsilon}$-approximately incident (or just ${\varepsilon}$-incident) to a surface $\sigma$ if the (suitably defined) distance between $p$ and $\sigma$ is at most ${\varepsilon}$. This notion has recently been introduced by a subset of the authors in [2], and applied in a variety of instances, involving somewhat simpler scenarios than the one considered here. Our approach enables us to compute a camera pose when the number of correspondences $n$ is large, and many of which are expected to be outliers. In contrast, a direct application of RANSAC-based methods on such inputs is very slow, since the fraction of inliers is small. In the limit, trying all pairs of matches involves $\Omega(n^{2})$ RANSAC iterations. Moreover, our methods enhance the quality of the posing considerably [17], since each generated candidate pose is close to (i.e., consistent with) with many of the correspondences.

Formally, we solve the following problem. We have an error parameter ${\varepsilon}>0$, we lay down a grid on $[0,1]^{d}$ of side length ${\varepsilon}$, and compute, for each vertex $v$ of the grid, a count $I(v)$ of surfaces that are approximately incident to $v$, so that (i) every surface that is ${\varepsilon}$-incident to $v$ is counted in $I(v)$, and (ii) every surface that is counted in $I(v)$ is $\alpha{\varepsilon}$-incident to $v$, for some small constant $\alpha>1$ (but not all $\alpha{\varepsilon}$-incident surfaces are necessarily counted). We output the grid vertex $v$ with the largest count $I(v)$ (or a list of vertices with the highest counts, if so desired).

As we will comment later, (a) restricting the algorithm to grid vertices only does not miss a good pose $v$: a vertex of the grid cell containing $v$ serves as a good substitute for $v$, and (b) we have no real control on the value of $I(v)$, which might be much larger than the number of surfaces that are ${\varepsilon}$-incident to $v$, but all the surfaces that we count are ‘good’—they are reasonably close to $v$. In the computer vision application, and in many related applications, neither of these issues is significant.

We give three algorithms for this camera-pose approximate-incidences problem. The first algorithm simply computes the grid cells that each surface intersects, and considers the number of intersecting surfaces per cell as its approximate ${\varepsilon}$-incidences count. This method takes time $O\left(\frac{n}{{\varepsilon}^{2}}\right)$ for all vertices of our ${\varepsilon}$-size grid. We then describe a faster algorithm using geometric duality, in Section 3. It uses a coarser grid in the primal space and switches to a dual 5-dimensional space (a 5-tuple is needed to specify a $2$D-$3$D correspondence and its surface, now dualized to a point). In the dual space each query (i.e., a vertex of the grid) becomes a 3-dimensional surface, and each original 2-dimensional surface in the primal 4-dimensional space becomes a point. This algorithm takes $O\left(\frac{n^{3/5}}{{\varepsilon}^{14/5}}+n+\frac{1}{{\varepsilon}^{4}}\right)$ time, and is asymptotically faster than the simple algorithm for $n>1/{\varepsilon}^{2}$.

Finally, we give a general method for constructing an approximate incidences data structure for general $k$-dimensional algebraic surfaces (that satisfy certain mild conditions) in ${\mathbb{R}}^{d}$, in Section 4. It extends the technique of Fonseca and Mount [3], designed for the case of hyperplanes, and takes $O(n+{\rm poly}(1/{\varepsilon}))$ time, where the degree of the polynomial in $1/{\varepsilon}$ depends on the number of parameters needed to specify a surface, the dimension of the surfaces, and the dimension of the ambient space. We first present and analyze this technique in full generality, and then apply it to the surfaces obtained for our camera posing problem. In this case, the data structure requires $O(n+1/{\varepsilon}^{6})$ storage and is constructed in roughly the same time. This is asymptotically faster than our primal-dual scheme when $n\geq 1/{\varepsilon}^{16/3}$ (for $n\geq 1/{\varepsilon}^{7}$ the $O(n)$ term dominates and these two methods are asymptotically the same). Due to its generality, the latter technique is easily adapted to other surfaces and thus is of general interest and potential. In contrast, the primal-dual method requires nontrivial adaptation as it switches from one approximate-incidences problem to another and the dual space and its distance function depend on the type of the input surfaces.

We implemented our algorithms and compared their performance on real and synthetic data. Our experimentation shows that, for commonly used values of $n$ and ${\varepsilon}$ in practical scenarios ($n\in[8K,32K]$, ${\varepsilon}\in[0.02,0.03]$), the primal-dual scheme is considerably faster than the other algorithms, and should thus be the method of choice. Due to lack of space, the experimentation details are omitted in this version, with the exception of a few highlights. They can be found in the appendix.

Suppose we are given a pre-computed three-dimensional scene and a two-dimensional picture of it. Our goal is to deduce from this image the location and orientation of the camera in the scene. In general, the camera, as a rigid body in 3-space, has six degrees of freedom, three of translation and three of rotation (commonly referred to as the yaw, pitch and roll). We simplify the problem by making the realistic assumption, that the vertical direction of the scene is known in the camera coordinate frame (e.g., estimated by en inertial sensor on smart phones). This allows us to rotate the camera coordinate frame such that its $z$-axis is parallel to the world $z$-axis, thereby fixing the pitch and roll of the camera and leaving only four degrees of freedom $(x,y,z,\theta)$, where $c=(x,y,z)$ is the location of the camera center, say, and $\theta$ is its yaw, i.e. horizontal the orientation of the optical axis around the vertical direction. See Figure 1.

By preprocessing the scene, we record the spatial coordinates $w=(w_{1},w_{2},w_{3})$ of a discrete (large) set of salient points. We assume that some (ideally a large number) of the distinguished points are identified in the camera image, resulting in a set of image-to-scene correspondences. Each correspondence ${\bf w}=\{w_{1},w_{2},w_{3},\xi,\eta\}$ is parameterized by five parameters, the spatial position $w$ and the position $v=(\xi,\eta)$ in the camera plane of view of the same salient point. Our goal is to find a camera pose $(x,y,z,\theta)$ so that as many correspondences as possible are (approximately) consistent with it, i.e., the ray from the camera center $c$ to $w$ goes approximately through $(\xi,\eta)$ in the image plane, when the yaw of the camera is $\theta$.

Following the general approach in [2], we use a suitable duality, with some care. We write ${\varepsilon}=2\gamma\delta_{1}\delta_{2}$, for suitable parameters $\gamma$, and ${\varepsilon}/(2\gamma)\leq\delta_{1},\,\delta_{2}\leq 1$, whose concrete values are fixed later, and apply the decomposition scheme developed in [2] tailored to the case at hand. Specifically, we consider the coarser grid $G_{\delta_{1}}$ in the primal space, of cell dimensions $\delta_{1}\times\delta_{1}\times\sqrt{2}\delta_{1}\times c\delta_{1}$, where $c$ is is the constant from Lemma 2.2, that tiles up the domain $Q=[0,1]^{3}\times[-1,1]$ of possible camera positions. For each cell $\tau$ of $G_{\delta_{1}}$, let $S_{\tau}$ denote the set of surfaces that cross either $\tau$ or one of the eight cells adjacent to $\tau$ in the $(z,\kappa)$-directions.²²2The choice of $z$, $\kappa$ is arbitrary, but it is natural for the analysis, given in the appendix. The duality is illustrated in Figure 2.

We discretize the set of all possible positions of the camera by the vertices of the finer grid $G_{\varepsilon}$, defined as $G_{\delta_{1}}$, with ${\varepsilon}$ replacing $\delta_{1}$, that tiles up $Q$. The number of these candidate positions is $m:=O(1/{\varepsilon}^{4})$. For each vertex $q\in G_{\varepsilon}$, we want to approximate the number of surfaces that are ${\varepsilon}$-incident to $q$, and output the vertex with the largest count as the best candidate for the position of the camera. Let $V_{\tau}$ be the subset of $G_{\varepsilon}$ contained in $\tau$. We ensure that the boxes of $G_{\delta_{1}}$ are pairwise disjoint by making them half open, in the sense that if $(x_{0},y_{0},z_{0},\kappa_{0})$ is the vertex of a box that has the smallest coordinates, then the box is defined by $x_{0}\leq x<x_{0}+\delta_{1}$, $y_{0}\leq y<y_{0}+\delta_{1}$, $z_{0}\leq z<z_{0}+\sqrt{2}\delta_{1}$, $\kappa_{0}\leq\kappa<\kappa_{0}+c\delta_{1}$. This makes the sets $V_{\tau}$ pairwise disjoint as well. Put $m_{\tau}=|V_{\tau}|$ and $n_{\tau}=|S_{\tau}|$. We have $m_{\tau}=O\left((\delta_{1}/{\varepsilon})^{4}\right)$ for each $\tau$. Since the surfaces $\sigma_{\bf w}$ are two-dimensional algebraic surfaces of constant degree, each of them crosses $O(1/\delta_{1}^{2})$ cells of $G_{\delta_{1}}$, so we have $\sum_{\tau}n_{\tau}=O(n/\delta_{1}^{2})$.

We now pass to the dual five-dimensional space. Each point in that space represents a correspondence ${\bf w}=(w_{1},w_{2},w_{3},\xi,\eta)$. We use the first three components $(w_{1},w_{2},w_{3})$ as the first three coordinates, but modify the $\xi$- and $\eta$-coordinates in a manner that depends on the primal cell $\tau$. Let $c_{\tau}=(x_{\tau},y_{\tau},z_{\tau},\kappa_{\tau})$ be the midpoint of the primal box $\tau$. For each $\sigma_{\bf w}\in S_{\tau}$ we map ${\bf w}=(w,\xi,\eta)$, where $w=(w_{1},w_{2},w_{3})$, to the point ${\bf w}_{\tau}=(w_{1},w_{2},w_{3},\xi_{\tau},\eta_{\tau})$, where $\xi_{\tau}=\xi-F(c_{\tau};w)$ and $\eta_{\tau}=\eta-G(c_{\tau};w)$, with $F$ and $G$ as given in (3). We have

We take the $\gamma$ provided by Corollary 3.1 as the $\gamma$ in the definition of $\delta_{1}$ and $\delta_{2}$. We map each point $v\in V_{\tau}$ to the dual surface $\sigma^{*}_{v}=\sigma^{*}_{v;\tau}=\{{\bf w}_{\tau}\mid v\in\sigma_{\bf w}\}$. Using (3), we have

By Corollary 3.1, the points ${\bf w}_{\tau}$, for the surfaces $\sigma_{\bf w}$ that cross $\tau$, lie in the region $R_{\tau}=[0,1]^{3}\times[-\gamma\delta_{1},\gamma\delta_{1}]^{2}$. We partition $R_{\tau}$ into a grid $G_{\delta_{2}}$ of $1/\delta_{2}^{5}$ small congruent boxes, each of dimensions $\delta_{2}\times\delta_{2}\times\delta_{2}\times(2\gamma\delta_{1}\delta_{2})\times(2\gamma\delta_{1}\delta_{2})=\delta_{2}\times\delta_{2}\times\delta_{2}\times{\varepsilon}\times{\varepsilon}$.

Exactly as in the primal setup, we make each of these boxes half-open, thereby making the sets of dual vertices in the smaller boxes pairwise disjoint. We assign to each of these dual cells $\tau^{*}$ the set $S^{*}_{\tau^{*}}$ of dual points that lie in $\tau^{*}$, and the set $V^{*}_{\tau^{*}}$ of the dual surfaces that cross either $\tau^{*}$ or one of the eight cells adjacent to $\tau^{*}$ in the $(\xi_{\tau},\eta_{\tau})$-directions. Put $n_{\tau^{*}}=|S^{*}_{\tau^{*}}|$ and $m_{\tau^{*}}=|V^{*}_{\tau^{*}}|$. Since the dual cells are pairwise disjoint, we have $\sum_{\tau^{*}}n_{\tau^{*}}=n_{\tau}$. Since the dual surfaces are three-dimensional algebraic surfaces of constant degree, each of them crosses $O(1/\delta_{2}^{3})$ grid cells, so $\sum_{\tau^{*}}m_{\tau^{*}}=O\left(m_{\tau}/\delta_{2}^{3}\right)$.

We compute, for each dual surface $\sigma^{*}_{v}$, the sum $\sum_{\tau^{*}}|S^{*}_{\tau^{*}}|$, over the dual cells $\tau^{*}$ that are either crossed by $\sigma^{*}_{v}$ or that one of their adjacent cells in the $(\xi_{\tau},\eta_{\tau})$-directions is crossed by $\sigma^{*}_{v}$. We output the vertex $v$ of $G_{\varepsilon}$ with the largest resulting count, over all primal cells $\tau$.

The following theorem establishes the correctness of our technique. Its proof is given in Appendix B.

In this section we present a general technique to preprocess a set of algebraic surfaces into a data structure that can answer approximate incidences queries. In this technique we round the $n$ original surfaces into a set of canonical surfaces, whose size depends only on ${\varepsilon}$, such that each original surface has a canonical surface that is “close” to it. Then we build an octree-based data structure for approximate incidences queries with respect to the canonical surfaces. However, to reduce the number of intersections between the cells of the octree and the surfaces, we further reduce the number of surfaces as we go from one level of the octree to the next, by rounding them in a coarser manner into a smaller set of surfaces.

This technique has been introduced by Fonseca and Mount [3] for the case of hyperplanes. We describe as a warmup step, in Section C of the appendix, our interpretation of their technique applied to hyperplanes. We then extend here the technique to general surfaces, and apply it to the specific instance of 2-surfaces in 4-space that arise in the camera pose problem.

We have a set $S$ of $n$ $k$-dimensional surfaces in ${\mathbb{R}}^{d}$ that cross the unit cube $[0,1]^{d}$, and a given error parameter ${\varepsilon}$. We assume that each surface $\sigma\in S$ is given in parametric form, where the first $k$ coordinates are the parameters, so its equations are

Moreover, we assume that each $\sigma\in S$ is defined in terms of $\ell$ essential parameters ${\bf t}=(t_{1},\ldots,t_{\ell})$, and $d-k$ additional free additive parameters ${\bf f}=(f_{k+1},\ldots,f_{d})$, one free parameter for each dependent coordinate. Concretely, we assume that the equations defining the surface $\sigma\in S$, parameterized by ${\bf t}$ and ${\bf f}$ (we then denote $\sigma$ as $\sigma_{{\bf t},{\bf f}}$), are

For each equation of the surface that does not have a free parameter in the original expression, we introduce an artificial free parameter, and initialize its value to $0$. (We need this separation into essential and free parameters for technical reasons that will become clear later.) We assume that ${\bf t}$ (resp., ${\bf f}$) varies over $[0,1]^{\ell}$ (resp., $[0,1]^{d-k}$).

We assume that the functions $F_{j}$ are all continuous and differentiable, in all of their dependent variables ${\bf x}$, ${\bf t}$ and ${\bf f}$ (this is a trivial assumption for ${\bf f}$), and that they satisfy the following two conditions.