Newton-PnP:
Real-time Visual Navigation for Autonomous Toy-Drones

While there are dozens of papers that aims to solve the PnP problem, there is still a serious remaining challenge in the context of real-time robotics, especially for weak and lightweight IoT devices as in robotics and drone applications.

Existing provable solvers for the PnP problem typically use external packages such as GloptiPoly [23], SOSTools [36, 41] or SeDuMi [44, 41]. Other solutions, either heuristics, approximations, or methods which solve alternative cost functions are discussed in Section I-D.

The main disadvantages of the above optimal solvers in the context of our robotic application are: (i) Memory. The very generic libraries above require both RAM and external memory, both of which are very limited, especially when it comes to IoT micro-computers such as the Raspberry PI zero (RPI0) micro-computer which we use in our case, that has 1GHz single-core CPU 512MB RAM; see Section IV-B. (ii) Running time. Those libraries are built to tackle a very wide range of optimization problems; they are not tailored for our problem. Thus, their running time is far from being optimal. (iii) deployment dependencies. Compilation and integration is often hard or even impossible, especially for non-Windows operating systems or non-Intel/AMD CPUs, as in our RPI case. Recent results such as [41] and GPOSolver [22] were able to remove dependencies on commercial tools such as MATLAB. However they still depend on optimization libraries such as SeDuMi [44] Mosek [31].

(i) Theoretical guarantees. We propose the first algorithm that, in theory, returns optimal provable results for input data that satisfies some weak assumption which was satisfied in all our experiments. Our algorithm runs in $O(\log\Delta\cdot\log\log(1/\varepsilon))$ time where $\varepsilon>0$ is the accuracy and $\Delta$ is the model precision; see Assumption 1, Theorem 2, and Algorithm 1.

(ii) Self-contained algorithm. Our proposed novel algorithm is self-contained and requires only few lines of code to implement. It does not depend on commercial tools or optimization libraries. It only requires standard and widely used libraries such as Eigen [20].

(iii) Experimental results. We provide extensive experimental results on our own collected real world datasets with ground truths. We empirically demonstrate that our algorithm consistently provides more precise results as compared to the competing methods; see Section IV-A.

(iv) Real-time system. As an application, we build our very own light-weight, low-cost, autonomous, and on-board toy-drone navigation system, which is based on a RPI0 with our novel PnP solver; see Section IV-B and the video.

(v) Open code. We provide full open source code for our algorithm, which can run in real-time on IoT devices [1].

To solve the above challenges we designed a simple algorithm, called Newton-PnP, or NPnP in short.

(i) Newton’s PnP. To provide a self contained algorithm with competitive performance, we follow [41] and formalize the PnP as an instance of a semidefinite programming (SDP) problem in (5), and utilize the simple Newton’s method for solving this objective, without external sophisticated solvers.

However, the classic Newton’s method alone does not support non-equality constraints as the ones that arise in our problem. Hence, we also combine the barrier method [8], as explained in Section III-B. The only external library required is Eigen for basic linear algebra that is common in robotics and real-time applications. In particular, it supports the LAPACK interface that is provided by CPU manufacturers for direct hardware implementations, simply by downloading their packages. In particular, our implementation was easily compiled on a RPI0 and a standard laptop.

(ii) Focus on the dual problem. The first issue when using the barrier method for tackling the SDP instance above, which is called the primal problem, is that the number of free variables is $241$, as also explained in [41]. This would be much too slow for our real-time drone application that runs on an RPI0. To this end, we observe that the corresponding dual optimization problem requires only $70$ unknown variables; see Section III-A. An important observation is that the duality gap is zero; see proof of Theorem 2. That is, an optimal solution to the dual problem corresponds to the optimal solution of the primal version of the problem, with no approximation gap.

(iii) Suggesting a provably good initial solution. The barrier method uses Newton’s method with equality constraints, which is an iterative algorithm that requires an initial solution. This solution must: (i) be feasible, i.e., be in a set that satisfies the constraints of our dual problem, and (ii) the value of the objective function at this initial point must be close to the optimal objective value, since the running time depends linearly on this distance; see Theorem 2.

To this end, we first observe that many of the parameters in our optimization problem remain fixed between different PnP instances. We leverage this observation, along with careful tuning of the barrier method’s parameter, to compute a good initial solution for our problem. This solution is computed in advance and is independent of the input; see Section III-C.

The PnP problem has been intensely studied in literature, where the proposed methods are broadly categorized into either iterative or non-iterative methods.

The iterative methods usually aim to iteratively minimize complex cost functions, which makes them potentially slow and non-optimal due to local minima of the cost function. However, such method achieve high accuracy when converging properly, and can handle arbitrary number $n$ of input pairs. Examples include [14, 34, 29, 40].

In comparison, the non-iterative methods tend to be unstable in the presence of noise [27], and in many cases gain speed by alternating or approximating the target cost function. Examples include [18, 15, 37, 17, 6], whose running times are polynomial in the number $n$ of input pairs. To enhance the stability of such methods, one can leverage additional redundant points, as in, for example, the well known Direct Linear Transformation (DLT) algorithm [2].

Among the most notable non-iterative results is the popular work of Lepetit et al. [27], termed EPnP, which reduced the problem into recovering a set of $4$ virtual control points, where the resulting quadratic polynomials were solved with simple linearization techniques in $O(n)$ time. Follow-up works have tried to enhance the stability of EPnP by replacing the linearization with polynomial solvers. Among those is the work of Li et al. [28], termed RPnP, which explicitly retrieves the roots of a seventh degree polynomial. However, the proposed improvements have still been shown to be unstable [47]. To resolve these drawbacks, Hesch and Roumeliotis [24] developed the direct least squares (DLS) method requiring $O(n)$ time. Unfortunately, they parameterized rotation by using the Cayley representation, which is degenerate in many cases. The accuracy deteriorates seriously when the camera pose approaches these singularities.

Branch and Bound. In a different line of works, branch and bound (BnB) methods have been proposed. In [21], Hartley et al. proposed a BnB algorithm which solves an approximated version of the problem, taking advantage of the rotation matrix constraints. Olsson et al. [35] proposed a similar algorithm which leverages quaternion representation for the rotation matrix. Unfortunately, such BnB algorithms are unpractical due to their tremendous computational cost.

Globally optimal and efficient methods. The limitations mentioned above led to a search for algorithms that are both provably globally optimal and efficient. For example, [25] utilizes Grobner basis, while the great works [41, 47] reduce the problem to some instance of semidefinite programming (SDP). Common SDP solvers use primal-dual interior-point methods [44] [4]. The most commonly used solution approaches are Mehrotra’s predictor-corrector based algorithms [30][43]. Mehrotra’s approach tackles the Karush-Kuhn-Tucker conditions of the primal and dual optimization problem to reach a global optimum. As for the PnP case, these equations produce $2\cdot 241+70=552$ variables in the Mehrotra’s approach, which is far more than only $86$ in our modified approach; see Section III.

Our algorithm draws inspiration from those theoretically globally optimal methods, but aims to be faster in practice.

Let $P=\left\{p_{1},\cdots,p_{n}\right\}\subseteq\mathbb{R}^{3}$ be a set of 3D (model) points and let $X=\left\{x_{1},\cdots,x_{n}\right\}\subseteq\mathbb{R}^{2}$ be a corresponding set of 2D (observed) points in a calibrated camera frame. Since the intrinsic camera parameters are given, each pixel $x_{i}\in X$ corresponds to a 3D line $\ell_{i}$ passing through the camera center and the pixel $x_{i}$ in the frame, whose direction vector is $v_{i}\in\mathbb{R}^{3}$; see Fig. 1. The pair $(P,L)$ form the input for the PnP problem, where $L$ consists of all the 3D lines that correspond to points in $X$.

The goal is to recover an alignment $(R^{*},t^{*})\in\mathrm{Alignments}$ that minimizes the sum of squared Euclidean distances $\mathrm{dist}^{2}(Rp_{i}+t,\ell_{i})$ between every $p_{i}\in P$ after applying the transformation, and its corresponding line $\ell_{i}$. Formally, the PnP objective reads:

over every $(R,t)\in\mathrm{Alignments}$.

As detailed in [41], we can recover the optimal vector $t^{*}$ as a linear function of the optimal rotation matrix $R^{*}$ by setting the derivative of (1) with respect to $t$ to zero. Plugging $t^{*}$ back, (1) can be rewritten as

for some known matrix of coefficients $M\in\mathbb{R}^{9\times 9}$, and where $v(R)\in\mathbb{R}^{9}$ is the row stacking of $R$.

Using quaternions. Due to the difficulty of handling the constraint $R\in\mathrm{SO}(3)$ above, a quaternion based representation for $R$ is used instead. Let $q=(q_{1},q_{2},q_{3},q_{4})\in\mathbb{R}^{4}$, and define $r(q)\in\mathbb{R}^{9}$ as

Now (2) can be rewritten using $r(q)$, where the constraint $R\in\mathrm{SO}(3)$ is implicitly enforced using the quaternion representation and the constraint $\left\lVert q\right\rVert^{2}=1$. The PnP problem can thus be reduced into the following system of polynomials:

There are multiple different approaches for solving polynomial systems as in (3), which include: (i) classical solutions which utilize combinatorics, (ii) Grobner basis solvers [10], and (iii) Semidifinite programming (SDP) [46] using the Sum Of Squares (SOS) decomposition [36]. Assuming there are $m$ equations of constant rank, each containing $d$ unknowns, all the above methods require theoretically $m^{O(d)}$ running time. However, the classical solutions tend to be very impractical due to the large constants hidden in the $O$ notation. Similarly, the Grobner basis methods are not only impractical in most cases, but are also numerically unstable. To this end, we adopt the SOS approach.

SOS hierarchy. The SOS approach is governed by some relaxation level $r$, which controls the trade-off between accuracy and running time; larger relaxation level corresponds to more accurate but slower results. These levels are commonly referred to as The Lasserre hierarchy [26]. The computational time increases exponentially fast when raising the relaxation level. The difficulty lies in balancing the accuracy and the running time, as to provide a theoretically fast algorithm which is also always accurate in practice.

PnP via SOS relaxation. The above SOS relaxation approach was also used in [41], where the first Lasserre hierarchy level was chosen in order to solve (3). Using this first level already leads to relatively high computational times, but produces accurate results. In our work, we utilize the same hierarchy level, for which we empirically obtained accurate results in all our experiments. However, as explained in Section I and in the following sections, we provide an alternative and much faster implementation, which can run faster by up to x$3$ than the alternative algorithms which utilize the same SOS approach as ours, e.g., [41]. This is by exploiting the barrier method, a smart initial guess, along a range of enhancements and techniques. This enables us to develop an efficient algorithm which can run in real-time on a microcomputer; see more details in Section IV.

The primal problem (5) has a dual formulation of the form

For more details on the relation between the primal and dual problems see Section $5.2$ in [8].

Existing solvers. Software such as SeDuMi [44] and Gloptipoly [23] aim to solve such problems via the Centering-Predictor-Corrector method [43], where both the primal and the dual problems, (5) and (6) respectively, are solved simultaneously.

Why the dual formulation? The dual formulation in (6) has only $70$ variables, as compared to the primal problem (5) which has $241$. Furthermore, the solution of the primal problem only provides an SOS decomposition for the SDP objective along with the minimal value of the objective function. However, we are interested in recovering the quaternion vector $q$ from  (3) which minimizes the objective. This quaternion can be extracted from the vector $y$ of the dual problem, as detailed in [26]. To this end, we aim to solve the dual problem only, via the barrier method [8]. As of correctness, by Section 5.9.1 of [8], strong duality exists in our problem. In other words, the optimal solutions for the primal and dual problems are the same.

The barrier method replaces a constraint in an optimization problem with a self-concordant convex barrier to the objective function; see more details in [8]. A suggested barrier for a PSD constraint is the log-determinant function. The barrier method incorporates a weight $t$ that is multiplied by the original objective function, which increases over time, thus the summation of the two components slowly converges to the global minimum or maximum) of the original objective:

Observe that $A$ and $c$ are constants, as detailed in (5) above, and $t$ and $b$ are parameters of this function as they are not constant; $t$ will change during the optimization process, and $b$ depends on the input.

We aim to solve the above maximization problem using Newton steps with equality constraints, as explained in section 10.2 of [8]. Newton steps can be performed relatively fast. The most time consuming operation is formulating the hessian, whose dimensions are $70\times 70$. Each Newton step with equality constraints requires solving a $(70+16)\times(70+16)$ linear equation system, and then performing a line search over that direction; see Algorithm 1. The line search is an exact-line-search; see Section 9.2 of [8].

The convergence analysis given in Section 10.2 of [8] conclude that applying Newton’s method with equality constraints is exactly the same as applying Newton’s method to the reduced problem obtained by eliminating the equality constraints; the convergence guarantees and bounds of Newton’s method for unconstrained problems are also relevant for the linearly constrained instances. Thus, the constraint in (6) does not affect our algorithm’s theoretical convergence and guarantees in what follows.

As mentioned in Section I, the bound over the number of required Newton steps depends linearly on the distance between the initial objective function value $f_{t}(y_{0})$ and its optimal value $f_{t}(y^{*})$, for the current weight $t$.

To this end, we observe that the matrix $A$ in (5)–(7) depends on the PnP problem, but it is independent of a specific PnP instance (the input sets of paired points and lines), which is encoded in the vector $b\in\mathbb{R}^{70}$. Also, if we define $h(y)=\log(\det(\mathrm{mat}((c-A^{T}y)_{16:240})))$, the vector

is also independent of $b$. Hence, $\tilde{y}\in\mathbb{R}^{70}$ above can be computed once (offline) for all future instances. Indeed, it is encoded in our open code [1]; see also Algorithm 1.

In this section we provide our main algorithm for solving the PnP problem via the SDP Dual formulation and the barrier method, as explained in the previous section; see Algorithm 1. Our main claim is given in Theorem 2.

Overview of Algorithm 1. Algorithm 1 takes as input a paired set of 3D points and lines $(P,L)$ as described in Section II-A, as well as the desired accuracy $\varepsilon>0$. The algorithm aims to output an alignment $(R,t)$ that satisfies

where the minimum is over every pair in $\mathrm{Alignments}$. This is by maximizing the objective function in (6).

We first compute the matrix $A$ and vectors $b$ and $c$ from Eq. (5)–(7). Then, at Line 1, we recover an initial guess $y\in\mathbb{R}^{70}$ for Eq. (7), as described in Section III-C. After that, we tune some parameters which are necessary for bounding the running time of the algorithm; see Theorem 2.

In the loop at Line 1 we apply the barrier method, where at each iteration we: (i) utilize Newton’s method (Lines 1–1) to minimize $f_{t}(\cdot)$ from (7), (ii) multiply $t$ by $\mu=50$, and (iii) repeat. Each Newton step requires computing the Hessian and the gradient of this function in order to recover the optimization direction $v$. The optimal point along the direction $v$ is recovered via exact line search at Line 1. The condition for the number of Newton iterations is derived in Section $9.5.2$ of [8]. Lastly, we extract the desired output from the recovered $y$.

The following theorem gives our main claim of correctness and running time for Algorithm 1. While the running time depends on the model’s finite-precision $\Delta$, in practice the number of iterations is almost always in $[15,30]$.

We implemented our PnP Algorithm from Section III in C++, and in this section we evaluate its empirical results on real-world datasets for pose estimation and autonomous drone navigation. Open source code can be found in [1]. The hardware used was a standard HP ZBook laptop with an Intel Core i7-10750H CPU @2.60GHzx12 and 32GB of RAM. The results demonstrate that the proposed PnP Algorithm consistently achieves more accurate and stable results as compared to the state of the art and widely common PnP implementations.

To demonstrate a potential usages of our fast and accurate PnP solver, we placed a micro computer equipped with a small camera, namely, a Raspberry PI Zero chip [45] with its dedicated RGB camera, on top of the toy drone from the previous section. The goal was to perform autonomous navigation along a predefined route in an indoor environment, given a known 3D map of the space. Such a 3D map was constructed in a pre-processing step using a known SLAM system [32]. To our knowledge, this is the first such light-weight (<$100$gr), autonomous, and on-board nano-drone navigation system. The system is also low-cost (<$125\$$). The autonomous flight is presented in the supplementary video.

Accuracy. As presented in Fig. 2, our NPnP consistently achieves results with an error smaller by x$1.5$ up to x$10$, as compared to the completing methods. The only exception is the SDP, which, as expected, outputs the exact same results as NPnP. As the graphs demonstrate, the globally optimal algorithms achieve smaller errors compared to the non-provable alternatives.

Time comparison. Firstly, we observe that, in practice, the number of Newton iterations that were actually needed was always a small constant ($[15,30]$), independent of $\Delta$. Secondly, as our main concern is the accuracy of the output results, our time comparison is focused only on the most accurate competing methods. As the graphs show, those methods are NPnP and SDP, which obtain roughly the same accuracy. As Fig. 2 shows, our naive implementation of NPnP is already faster by up to x$3$ as compared to SDP, and more consistent. We leave further code optimizations to our naive implementation for future work.