EKFPnP: Extended Kalman Filter for Camera Pose Estimation in a Sequence of Images

TL;DR

This paper introduces EKFPnP, a method using an Extended Kalman Filter to improve camera pose estimation over sequences of images by accounting for temporal dependencies and feature uncertainties, enhancing robustness against noise.

Contribution

The paper presents a novel EKF-based approach for sequential camera pose estimation that considers feature uncertainty and temporal dependencies, improving robustness over existing methods.

Findings

Enhanced robustness in noisy conditions

Effective in both simulated and real data

Outperforms state-of-the-art methods

Abstract

In real-world applications the Perspective-n-Point (PnP) problem should generally be applied in a sequence of images which a set of drift-prone features are tracked over time. In this paper, we consider both the temporal dependency of camera poses and the uncertainty of features for the sequential camera pose estimation. Using the Extended Kalman Filter (EKF), a priori estimate of the camera pose is calculated from the camera motion model and then corrected by minimizing the reprojection error of the reference points. Experimental results, using both simulated and real data, demonstrate that the proposed method improves the robustness of the camera pose estimation, in the presence of noise, compared to the state-of-the-art.

Equations

$${\mathbf{s}}_{k|k-1}=f({\mathbf{s}}_{k-1|k-1},{\mathbf{u}}_k),$$

$${\mathtt{P}}_{k|k-1}={\mathtt{F}}_{k-1}{\mathtt{P}}_{k-1|k-1}{\mathtt{F}}_{k-1}^T+{\mathtt{G}}_{k-1}{\mathtt{Q}}_{k-1}{\mathtt{G}}_{k-1}^T,$$

$${\mathbf{s}}_{k|k}$$

$${\mathtt{P}}_{k|k}$$

$${\mathtt{K}}_k={\mathtt{P}}_{k|k-1}{\mathtt{H}}_k^T({\mathtt{H}}_k{\mathtt{P}}_{k|k-1}{\mathtt{H}}_k^T+{\mathtt{U}}_k{)}^{-1},$$

$${\mathbf{x}}^c=\mathtt{R}{\mathbf{x}}^w+\mathbf{t}.$$

$${\mathbf{x}}^c=\mathtt{R}({\mathbf{x}}^w-\mathbf{c}).$$

$${\mathbf{s}}_{13\times 1}=[{\mathbf{c}}_{3\times 1}^T,{\mathbf{q}}_{4\times 1}^T,{\mathbf{v}}_{3\times 1}^T,{\omega }_{3\times 1}^T{]}^T.$$

$$f_{13\times 1}(\mathbf{s},n)=\left[\begin{array}{c}{\mathbf{c}}_k+({\mathbf{v}}_k+{\nu }_k)\Delta t\\ {\mathbf{q}}_k\ast \mathbf{q}(({\omega }_k+{\mathbf{\Omega }}_k)\Delta t)\\ {\mathbf{v}}_k+{\nu }_k\\ {\omega }_k+{\mathbf{\Omega }}_k\end{array}\right],$$

$${\mathtt{F}}_{13\times 13}$$

$${\mathtt{G}}_{13\times 6}$$

$${\mathbf{z}}_{2n\times 1}=[x_i^1,x_j^1,\cdots ,x_i^n,x_j^n{]}^T.$$

$$h^{WC}(\mathbf{s},{\mathbf{x}}^w)=\left[\begin{array}{c}0\\ {\mathbf{x}}^c\end{array}\right]=\mathbf{q}\ast \left[\begin{array}{c}0\\ {\mathbf{x}}^w-\mathbf{c}\end{array}\right]\ast {\mathbf{q}}^{\ast },$$

$$h^{CI}({\mathbf{x}}^c)={\mathbf{x}}^I=[f\frac{x_i^c}{x_k^c},f\frac{x_j^c}{x_k^c}].$$

$$h_{2n\times 1}=h^{CI}(h^{WC}({\mathbf{x}}^w)).$$

$${\mathtt{H}}_{2n\times 13}=\frac{\delta h}{\delta \mathbf{s}}=\frac{\delta h^{CI}}{\delta h^{WC}}\frac{\delta h^{WC}}{\delta \mathbf{s}}.$$

$$e_{rot}(deg)=ma{x}_{k=1}^3\{arccos(r_{k,true}^T.r_k)\times 180/\pi \},$$

$$e_{trans}(\%)=\Vert t_{true}-t\Vert /\Vert t\Vert \times 100.$$

Full text

Keywords: Camera Pose Estimation, Perspective-n-Point, Extended Kalman Filter

1 Introduction

Camera pose estimation by tracking a 3D object in a video sequence which is known as 3D tracking means continuously identifying camera position and orientation relative to the scene, or, equivalently, the 3D displacement of an object relative to the camera when either the object or the camera is moving [1]. It has many applications in computer vision and robotics. Examples include augmented reality, visual servoing of robots, incremental structure-from-motion, robot navigation, etc. Generally, researchers have concentrated on estimating camera projection matrix from a set of 3D points in a reference world coordinate and their projection into the camera image coordinate.

Perhaps the simplest algorithm of this family is Direct Linear Transformation (DLT) which estimates the camera projection matrix by minimizing the 3D geometric error in the object space [2, 3, 4]. Basic DLT calculates the most general form of the camera projection matrix with 11 degrees of freedom. Thus, it is more fit for uncalibrated camera scenarios, in which case the problem is known as camera resectioning. Although this algorithm can be adapted to more restricted camera models like affine model [4], Ignoring the intrinsic parameters leads to inaccuracies when the camera is calibrated.

The second class of methods is Perspective-n-Point (PnP) techniques in which the calibration parameters are known. In its simplest form, a closed-form solution is obtained from 3 noncollinear point correspondences (P3P) [5, 6, 7] producing up to four possible solutions. Generally, one additional correspondence is used to select the correct solution.

In real-world applications the PnP problem is usually applied to an image sequence. However, to the authors’ knowledge, most PnP solutions compute the camera pose independently from its history. It means that they ignore the time dependency of camera poses through the image sequence. This may cause instability in the estimation of camera motion sequence, especially when the observation noise level increases over time.

In this paper, the PnP problem is considered as a probabilistic estimation process over time. It is assumed that the 3D reference points are fixed and the camera moves smoothly with small accelerations. For this purpose we used Extended Kalman Filter (EKF). EKF is one of the most popular recursive probabilistic filters which estimates the state of nonlinear systems. The new state of the camera is predicted from its motion history and then corrected by minimizing the reprojection error of the reference points. Therefore, the camera state contains the camera pose parameters, linear and angular velocities which are used to predict the camera pose over time.

Using the probabilistic approach, the uncertainties of observations (which may be caused by uncertainties of 3D reference points or 2D tracked points) will be considered in the problem formulation. Other probabilistic methods such as CEPPnP [8] take into account a limiting assumption that the observation uncertainties are known in advance. We ignore this assumption since prior information of observation uncertainties may not be available. By employing the EKF, final results are calculated by combining both camera motion model and reprojection error minimization. Therefore, only a coarse estimate of observation uncertainties is enough to yield accurate results.

The method can evaluate the uncertainty of the pose parameters by estimating the camera state covariance. This would be helpful when a quantitative criterion for measuring the accuracy of each parameter is needed. Many solutions to this problem use image-space error, object-space error or other algebraic error functions as measures of pose reliability. However, they cannot determine the amount of reliability for each camera pose parameter individually.

Moreover, by using EKF, additional sources of information such as inertial sensors can be combined to refine the estimation. Additionally, the prediction step can be used to enhance other components of the algorithm such as feature tracking.

The paper proceeds as follows: Section 2 reviews the related work. Section 3 describes our EKF formulation. In section 4, experimental results are presented comparing our method with the state-of-the-art. Section 5 concludes the paper.

2 Related works

P3P [5] is one of the first algorithms for the pose estimation of calibrated cameras from 3 correspondences between 3D reference points and their 2D projections. In addition, there is a variety of PnP algorithms deriving closed-form solutions from a limited number of points, namely P4P [5] and P5P [9]. Since these algorithms are only applicable to a small portion of correspondences, they are sensitive to noise. Although other traditional PnP solutions have no restriction on the number of points, they are computationally expensive. For example, the time complexity of [10] as the lowest complexity method is $O(n^{2})$ while it is very sensitive to noise. The time complexity of more robust methods like [11] and [12] are significantly increased to $O(n^{5})$ and $O(n^{8})$ respectively.

Lepetit et al. introduced Efficient PnP (EPnP) [13] which is the first efficient non-iterative O(n) solution. EPnP represents reference points by a weighted sum of four virtual control points. Then the problem is solved using fourth order polynomials with simple linearization techniques. The Robust PnP (RPnP) [14] divides reference points into 3-point subsets in order to generate quadratic polynomials for each subset, and then the squared sum of those polynomials is used as a cost function.

In the Direct-Least-Squares (DLS) [15] method, the PnP problem is solved by minimizing a nonlinear geometric cost function. However, it suffers from rotational degeneracy since the Cayley representation is used for the rotations. The Accurate and Scalable PnP (ASPnP) [16] and the Optimal PnP (OPnP)[17] use a quaternion representation of rotation to overcome this problem and yield more accurate results.

All the mentioned methods are categorized as non-iterative methods in PnP. The iterative approaches formulate the problem as a nonlinear least squares problem. Although iterative algorithms are more robust to outliers and are more accurate, their performance highly depends on good initialization, since the objective function may be trapped in a local minimum. Furthermore, they are computationally more expensive. Some non-iterative methods use iterative algorithms like Gauss-Newton (GN) as a fine-tuner to achieve more accurate results. For example, EPnP used this approach and is commonly known as EPnP+GN in the literature.

One of the fastest iterative PnP algorithms is the LHM method [18]. It minimizes an error metric based on collinearity in object space and relies on an initial estimation of the camera pose with a weak-perspective assumption. In contrast to LHM, the Procrustes PnP [19] iteratively minimizes the error between the object and the back-projected image points.

With the possibility of outliers, it is necessary to incorporate an outlier-removal scheme like RANSAC [5]. Ferraz et al. combine an algebraic outlier rejection strategy with the linear formulation of the PnP solution in EPnP algorithm called Robust Efficient Procrustes PnP (REPPnP)[20]. It sequentially removes correspondences yielding algebraic errors more than a specific threshold. Final results are obtained by iteratively solving the closed-form Orthogonal Procrustes problem. The authors have extended their method to integrate image points uncertainties introducing the Covariant EPPnP (CEPPnP) [8]. To incorporate feature uncertainties in EPnP, a Gaussian distribution models the error for each of the observed 2D feature points. Then the PnP solution is formulated as a maximum likelihood problem, approximated by an unconstrained Sampson error function. It naturally penalizes the noisiest correspondences. However, as noted in the article, the feature uncertainties are assumed to be known in advance.

It is also worth mentioning that some algorithms like EPnP, REPPnP and CEPPnP propose separate solutions for planar and non-planar reference points. As a result, these methods may yield inaccurate results in cases with near-planar configurations [17].

MLPNP [21] uses image points uncertainties to present a new maximum likelihood solution to the PnP problem. First, the uncertainties propagate to the forward-projected bearing vectors. Then the null space of bearing vectors is used to obtain a linear maximum likelihood solution. Finally, the result of the ML estimator is iteratively refined with the Gauss-Newton optimization.

There are some related studies in the field of visual servoing which use the EKF for camera pose estimation; for example, [22] and [23] use EKF and Iterative Adaptive EKF (IAEKF) respectively for real-time control of robot motion. Similar to our method, they formulate the control error in the image space. However, they use the Euler angle representation of the rotation matrix. Apart from the infamous gimbal lock problem, this adds to computational complexity of the algorithm [24].

This paper develops a probabilistic approach to estimate camera poses in a sequence of images without use of any other source of information such as IMU. For this purpose, the uncertainty of both 2D image points and 3D reference points are considered. Additionally, the history of camera motion is also used to calculate an initial estimation of the camera pose. To combine the uncertainty in the observed image points and the camera pose dynamics, Extended Kalman Filter (EKF) is applied recursively over time.

3 Problem formulation

Throughout this paper, matrices, vectors and scalars are denoted by capital letters, bold lowercase letters and plain lowercase letters, respectively.

3.1 Extended Kalman Filter

Kalman filter is a powerful tool for recursively estimating the state of a linear dynamic process from a series of noisy measurements. For non-linear systems Extended Kalman Filter (EKF) is widely used. The EKF approximates a nonlinear model by its first order Taylor expansion. Each step of EKF runs in two phases: prediction and correction.

The prediction phase is independent of the current observation and only employs the dynamic model of the process to calculate a prior estimation of the current state from the posterior estimation of the previous state using the following equation:

[TABLE]

where $\mathbf{s}$ is the state vector and $f(.)$ is the nonlinear dynamic model of the process. The term $\mathbf{u}_{k}$ is the dynamic model noise vector which is independent of the state vector and is commonly distributed with zero mean.

The uncertainty of this prediction can be represented by the covariance matrix $\mathtt{P}_{k|k-1}$:

[TABLE]

where $\mathtt{F}_{k}$ is the derivative of the dynamic model with respect to the state vector $\mathbf{s}$, $\mathtt{G}_{k}$ is the derivative of the dynamic model with respect to $\mathbf{u}_{k}$, and $\mathtt{Q}_{k}$ is the noise covariance.

In the correction phase, the prior estimation of the current state is corrected to the posterior estimation given a new observation. By incorporating the observations, the posterior of the state and its covariance matrix can be estimated as

[TABLE]

where $\mathbf{z}_{k}$ represents the observations, $h(.)$ is the observation model mapping the state space into the observed space $\mathbf{z}_{k}$ and $\mathtt{H}_{k}$ is the derivative of $h(.)$ with respect to the state vector $\mathbf{s}$. The parameter $\mathtt{K}$, known as the Kalman gain, specifies how much the posterior should be affected by observation error and is computed as

[TABLE]

where $\mathtt{U}$ is the observation noise covariance.

To apply EKF to sequential camera pose estimation, our formulation closely follows the approach of MonoSLAM [25]. However, contrary to our approach, MonoSLAM puts the 3D reference points in its state vector which greatly increases the amount of computation due to the very large size of matrices and vectors.

3.2 State vector and motion model

In general, the projection of a perspective camera is defined by its translation vector $\mathbf{t}$ and rotation matrix $\mathtt{R}$ with respect to the world reference coordinates. This transformation for a 3D point $\mathbf{x}^{w}=[x_{i},x_{j},x_{k}]^{T}$ is shown by

[TABLE]

This equation can be expressed with respect to the camera center $\mathbf{c}$:

[TABLE]

It is known that the Rotation matrix should obey both the orthonormal constraint $\mathtt{R}\mathtt{R}^{T}=I$ and the determinant constraint $det(\mathtt{R})=1$. One of the most convenient ways to deal with these constraints is to use the quaternion representation. Furthermore, as a four-element vector, it is more compact than the rotation matrix representation.

Accordingly, the state vector of EKF includes camera center position $\mathbf{c}$, camera orientation $\mathbf{q}$, and both linear and angular velocities $\mathbf{v}$ and $\mathbf{\omega}$:

[TABLE]

For the dynamic model, the camera motion is considered smooth with a constant velocity. In fact both linear and angular accelerations are modeled with zero-mean Gaussian noise $\mathbf{\omega}=[a,\alpha]^{T}$. Thus the dynamic model can be written as:

[TABLE]

where $\mathbf{\nu}=a\Delta t$ and $\mathbf{\Omega}=\alpha\Delta t$ are the velocities caused by linear and angular accelerations, and $q((\mathbf{\omega}_{k}+\mathbf{\Omega})\Delta t)$ is the unit quaternion equivalent of angle $(\mathbf{\omega}_{k}+\mathbf{\Omega})\Delta t$. The symbol ”*” means quaternion product.

To calculate the state prediction covariance of Eq. (2) it is necessary to calculate the Jacobian of $f(.)$ with respect to $\mathbf{s}$ and $\mathbf{\omega}$:

[TABLE]

To see how the derivatives in the quaternion space are calculated please refer to [26]

3.3 Observation model

For EKF-based PnP, observations are 2D correspondences of 3D reference points in the world coordinate, acquired by the moving camera. So, for $n$ 2D points the observations vector is

[TABLE]

The observation model is the projection function mapping the 3D points into the image plane. This mapping is typically done in two steps (Fig. 1).

First, 3D reference points are transformed from the world coordinate system to the camera coordinate system using Eq. (7). Eq. (7) is rewritten in terms of quaternions as

[TABLE]

where $\mathbf{q}$ is the unit-quaternion equivalent to the rotation matrix and $\mathbf{q}^{*}$ is the conjugate of $\mathbf{q}$.

Second, 3D points are transformed from the camera coordinate system to the image plane using pinhole camera model:

[TABLE]

where $f$ is the camera focal length. Thus the observation model is

[TABLE]

3.3.1 Kalman gain

Compared to the usual PnP methods, our method is able to reduce the effect of the observation noise, as the dynamic model based prediction and the observation-based correction steps are combined using the Kalman gain to offer a more robust estimation.

The observation covariance matrix $\mathtt{U}$ of Eq. (5) is the uncertainty of the tracked 2D points in the image sequence. It is assumed that feature tracking errors are independent and identically distributed with a zero-mean Gaussian distribution. Thus, $\mathtt{U}$ is diagonal. In some literatures like CEPPnP [8] and MLPnP [21], it is common to assume that the observation covariance is known a priori, although this is hardly the case in real applications. Our method can handle this issue as the EKF is able to work with an overestimation of the covariance.

To estimate the Kalman gain and the posterior state covariance matrix, the Jacobian of observation model $h$ with respect to the state vector $\mathbf{s}$ can be derived according to

[TABLE]

For details please refer to [26]

4 Experimental results

We compare the proposed algorithm (EKFPnP) to the state-of-the-art, including EPnP+GN [13], DLS [15], PPnP [19], RPnP [14], ASPnP [16], OPnP [17], CEPPnP [8] and MLPnP [21], using both synthetic and real data. We use the MATLAB implementation of the other methods, and made our own data and implementation available at https://github.com/ma-mehralian/ekfpnp_matlab_toolbox.

4.1 Simulated Data

In this section, the accuracy and execution time of EKFPnP are compared with the other methods. Our set up is very similar to that of CEPPnP and MLPnP. All simulations were repeated 100 times independently, and the average errors are reported. The rotation error (degree) is computed as

[TABLE]

where $r_{k,true}^{T}$ and $r_{k}$ are the k-th columns of $R_{true}$ and $R$ respectively. Also, the translation error (%) is defined as

[TABLE]

We generate both non-planar and planar 3D data. In the first setting, the non-planar 3D points were uniformly sampled from the interval $x,y,z\in[-2,2]\times[-2,2]\times[4,8]$. In the second setting the planar 3D points were generated by uniformly sampling $x,y\in[-2,2]\times[-2,2]$ and setting $z=0$. As shown in Fig. 2 the image of the 3D points are taken by a camera moving along a trajectory with 200 virtual camera poses. The camera is moved in such a way that all extrinsic parameters are changed in each step of the trajectory. For all virtual perspective cameras, the focal length is set to 800 pixels, and we assumed an image size of $640\times 480$.

In the first experiment, the competing algorithms are evaluated with different noise levels. Every 10% of 2D image points are perturbed by one of 10 zero-mean Gaussian noise distributions with different standard deviations of $\sigma=[1...10]$. To make the observation noise more realistic, the magnitude of noise is increased gradually over time from 0% to 100%. CEPPnP, MLPnP and the proposed method use 2D points uncertainty as part of the algorithm’s input. We do not use the variance of the observations known for the simulated data. Instead, we provide the algorithms with an overestimation of the variance. Thus, for these 3 algorithms the average value of points’ variances is used as a common variance for all points. For the proposed method, the velocity of the camera in the state vector of the Kalman filter should be initialized. This is done by estimating the camera pose in the first pair of images using EPnP+GN.

Fig. 3 compares the accuracy of the proposed method with other methods for different numbers of correspondences. The first and second rows show the average rotation and translation errors for n=20 to 200 correspondences for the non-planar and planar settings, respectively.

It can be seen that the proposed algorithm outperforms the other methods in terms of the estimation error, especially in the planar case. As expected, since the observation uncertainties are taken into account in the EKFPnP, MLPnP and CEPnP methods, they produce competitive results.

In the second experiment, it is assumed that the number of correspondences remains fixed with n=100 and the noise level varies. Fig. 4 shows the results obtained by ranging the noise variance from 1 to 15. Again the magnitude of noise is increased gradually over time. It can be seen that the proposed method still maintains its superiority over other methods as the noise level increases.

It is hard to compare our runtime performance with other algorithms since our MATLAB implementation is not optimized. Moreover, some other algorithms have improved their performance by using a C++ implementation plus MATLAB MEX. Fig. 5 depicts the running times of all algorithms as the number of points increases.

4.2 Real Data

In these experiments, the accuracy of the proposed method is evaluated on real image sequences. In the first experiment camera pose or, equivalently, the 3D displacement of a box relative to the camera is tracked in the sequence. The second experiment employs camera pose estimation in an incremental structure-from-motion system. CEPPnP [8], MLPnP [21] and EKFPnP, which are the algorithms that consider uncertainties in the observed feature points where applied to above data and results were compared.

4.2.1 Box tracking

In this experiment data consists of a sequence of 573 images of a freely moving box with known dimensions. All frames have a resolution of $1280\times 720$ pixels and the camera was calibrated using Camera Calibration Toolbox for Matlab [27]. The 3D model of the box including 898 ORB features [28] was created. To track the features, Kanade-Lucas-Tomasi (KLT) [29] tracker was employed. As result using 3D model and 2D tracked features we estimate the camera pose and the 3D motion of the box.

The 2D features are tracked in images and used as observations. In the tracking phase, the tracked points are usually corrupted over time by tracking errors or they are even get lost due to occlusions or image brightness inconsistency. Thus, this experiment is a good way to evaluate algorithms’ robustness against tracking errors.

The results obtained using each method are given in Fig. 6. Since the exact amount of tracking error is not known, a constant approximate mean value of 3 pixels per points was used for CEPnP. Clearly, our method outperforms CEPnP, while the results of MLPnP and EKFPnP are quite similar.

4.2.2 Structure from Motion

In this experiment we use part of TempleRing dataset [30] (Fig. 7) to develop structure from motion. The data consists of a sequence of 19, $640\times 480$ images sampled on a ring around a plaster temple.

To estimate camera poses with the same scale of ground truth, we use first two poses from ground truth to initialize 3D model with triangulation of 2D features. We extract SIFT features [31] in each image and match them with the features of the previous view. Using 2D features and 3D model correspondences we estimate camera poses for the rest of sequence. If 3D correspondences of some 2D features do not exist in 3D model we add them to the model by triangulation. Since we use estimated poses to triangulate new 3D points, the error of both 3D model and pose estimation is increased gradually. Fig. 8 shows rotation and translation errors of MLPnP and EKFPnP. Experiments were repeated 50 times independently, and the average errors are reported. We have remove CEPnP from comparison because it produces large error values. As can be seen, the results of both methods are very close.

Fig. 9 and Fig. 10 show the results of SFM for EKFPnP and MLPnP respectively. In both figures the ground truth and estimated poses are shown in green red respectively.

5 Conclusion

In this paper we incorporated both camera motion history and 2D features’ uncertainty to estimate camera pose in a sequence of images. We employed an extended Kalman filter (EKF) as a recursive probabilistic model for estimation of the camera pose in two steps of prediction from the motion model and correction by minimization of the reprojection error. This approach also provides the covariance of the pose estimated parameters which helps to measure the reliability of the results.

Our synthetic and real data experiments revealed that the incorporation of 2D features’ uncertainty and motion dynamics produces results which are more robust against feature tracking errors.