Joint Estimation of Camera Orientation and Vanishing Points from an Image Sequence in a Non-Manhattan World

Abstract

A widely used approach for estimating camera orientation is to use the points at infinity, i.e., the vanishing points (VPs). Enforcement of the orthogonal constraint between the VPs, known as the Manhattan world constraint, enables an estimation of the drift-free camera orientation to be achieved. However, in practical applications, this approach is neither effective (because of noisy parallel line segments) nor performable in non-Manhattan world scenes. To overcome these limitations, we propose a novel method that jointly estimates the VPs and camera orientation based on sequential Bayesian filtering. The proposed method does not require the Manhattan world assumption, and can perform a highly accurate estimation of camera orientation. In order to enhance the robustness of the joint estimation, we propose a keyframe-based feature management technique that removes false positives from parallel line clusters and detects new parallel line sets using geometric properties such as the orthogonality and rotational dependence for a VP, a line, and the camera rotation. In addition, we propose a 3-line camera rotation estimation method that does not require the Manhattan world assumption. The 3-line method is applied to the RANSAC-based outlier rejection technique to eliminate outlier measurements; therefore, the proposed method achieves accurate and robust estimation of the camera orientation and VPs in general scenes with non-orthogonal parallel lines. We demonstrate the superiority of the proposed method by conducting an extensive evaluation using synthetic and real datasets and by comparison with other state-of-the-art methods.

Appendix: Jacobian Derivation

In the prediction step of the EKF system, the Jacobian of the system model, $\mathbf {F}$, is defined by

$$\begin{aligned} \mathbf {F} = \frac{\partial \mathbf {f}}{\partial \mathbf {x}} = \left[ \begin{array}{cc} \frac{\partial \mathbf {f}_v}{\partial \mathbf {x}_v} &{}\quad \mathbf {0}_{7 \times 2n} \\ \mathbf {0}_{2n \times 7} &{}\quad \mathbf {I}_{2n} \end{array} \right] , \end{aligned}$$

(21)

where n is the number of VPs, $\mathbf {0}_{a \times b}$ is an $a \times b$ zero matrix, and $\mathbf {I}_{c}$ is a $c \times c$ identity matrix. The Jacobian of the camera motion model for the camera state, $\frac{\partial \mathbf {f}_{v}}{\partial \mathbf {x}_{v}}$, is derived from Eq. (7) as

$$\begin{aligned} \frac{\partial \mathbf {f}_{v}}{\partial \mathbf {x}_{v}} = \left[ \begin{array}{cc} \frac{\partial \mathbf {q}_{WC}^{new}}{\partial \mathbf {q}_{WC}^{old}} &{}\quad \frac{\partial \mathbf {q}_{WC}^{new}}{\partial \omega _{C}^{old}} \\ \frac{\partial \omega _{C}^{new}}{\partial \mathbf {q}_{WC}^{old}} &{}\quad \frac{\partial \omega _{C}^{new}}{\partial \omega _{C}^{old}} \end{array} \right] = \left[ \begin{array}{cc} \frac{\partial \mathbf {q}_{WC}^{new}}{\partial \mathbf {q}_{WC}^{old}} &{}\quad \frac{\partial \mathbf {q}_{WC}^{new}}{\partial \omega _{C}^{old}} \\ \mathbf {0}_{3 \times 4} &{}\quad \mathbf {I}_{3} \end{array} \right] . \end{aligned}$$

(22)

Here, the Jacobian of the new camera orientation for the old camera orientation, $\frac{\partial \mathbf {q}_{WC}^{new}}{\partial \mathbf {q}_{WC}^{old}}$, is easily computed as follows.

$$\begin{aligned} \frac{\partial \mathbf {q}_{WC}^{new}}{\partial \mathbf {q}_{WC}^{old}} = \bar{\mathbf {Q}} \left( \mathbf {q} \left( \left( \omega _{C}^{old}+{\varvec{\Omega }} \right) \varDelta t \right) \right) , \end{aligned}$$

(23)

where the function $\bar{\mathbf {Q}}(\mathbf {p})$ is defined as

$$\begin{aligned} \bar{\mathbf {Q}} ( \mathbf {p} ) = \left[ \begin{array}{cccc} p_{1} &{}\quad -\,p_{2} &{}\quad -\,p_{3} &{}\quad -\,p_{4} \\ p_{2} &{}\quad p_{1} &{}\quad p_{4} &{}\quad -\,p_{3} \\ p_{3} &{}\quad -\,p_{4} &{}\quad p_{1} &{}\quad p_{2} \\ p_{4} &{}\quad p_{3} &{}\quad -\,p_{2} &{}\quad p_{1} \end{array} \right] , \end{aligned}$$

(24)

where $\mathbf {p} = \left[ p_1, p_2, p_3, p_4 \right] ^{\mathrm {T}}$. Let $\omega _{C}^{old} = \left[ w_{1}, w_{2}, w_{3} \right] ^{\mathrm {T}}$. Then, the Jacobian $\frac{\partial \mathbf {q}_{WC}^{new}}{\partial \omega _{C}^{old}}$ is derived as

$$\begin{aligned} \frac{\partial \mathbf {q}_{WC}^{new}}{\partial \omega _{C}^{old}} = \begin{array}{c} \mathbf {Q} \left( \mathbf {q}_{WC}^{old} \right) \left[ \begin{array}{cc} m(\omega _{C}^{old},\varDelta t,1) &{}\quad m(\omega _{C}^{old},\varDelta t,2) \\ n(\omega _{C}^{old},\varDelta t,1) &{}\quad o(\omega _{C}^{old},\varDelta t,1,2) \\ o(\omega _{C}^{old},\varDelta t,2,1)&{}\quad n(\omega _{C}^{old},\varDelta t,2) \\ o(\omega _{C}^{old},\varDelta t,3,1) &{}\quad o(\omega _{C}^{old},\varDelta t,3,2) \end{array} \right. \\ \\ \left. \begin{array}{c} m(\omega _{C}^{old},\varDelta t,3) \\ o(\omega _{C}^{old},\varDelta t,1,3) \\ o(\omega _{C}^{old},\varDelta t,2,3) \\ n(\omega _{C}^{old},\varDelta t,3) \end{array} \right] , \end{array} \end{aligned}$$

(25)

where

$$\begin{aligned} m(\omega _{C}^{old},\varDelta t,i)= & {} -\frac{w_i}{||\omega _{C}^{old} ||} \sin \left( \frac{||\omega _{C}^{old} ||\varDelta t}{2} \right) \frac{\varDelta t}{2} , \end{aligned}$$

(26)

$$\begin{aligned} n(\omega _{C}^{old},\varDelta t,i)= & {} \frac{||\omega _{C}^{old} ||- w_{i}^{2} / ||\omega _{C}^{old} ||}{||\omega _{C}^{old} ||^{2}} \sin \left( \frac{||\omega _{C}^{old} ||\varDelta t}{2} \right) \nonumber \\&+\, \frac{w_i^2}{||\omega _{C}^{old} ||^2} \cos \left( \frac{||\omega _{C}^{old} ||\varDelta t}{2} \right) \frac{\varDelta t}{2} , \end{aligned}$$

(27)

and

$$\begin{aligned} o(\omega _{C}^{old},\varDelta t,i,j)= & {} -\frac{w_{i} w_{j}}{||\omega _{C}^{old} ||^{3}} \sin \left( \frac{||\omega _{C}^{old} ||\varDelta t}{2} \right) \nonumber \\&+\, \frac{w_i w_j}{||\omega _{C}^{old} ||^2} \cos \left( \frac{||\omega _{C}^{old} ||\varDelta t}{2} \right) \frac{\varDelta t}{2} . \ \ \end{aligned}$$

(28)

When the EKF system is updated, the computation of the Jacobian of the measurement model for the camera state, $\mathbf {H}$, is required to correct the predicted state vector. The Jacobian is defined by concatenating the Jacobians for all the line features as follows.

$$\begin{aligned} \mathbf {H} = \left[ \begin{array}{ccccc} \frac{\partial h_{11}}{\partial \mathbf {x}} ^{\mathrm {T}}&\cdots&\frac{\partial h_{ij}}{\partial \mathbf {x}} ^{\mathrm {T}}&\cdots&\frac{\partial h_{nm}}{\partial \mathbf {x}} ^{\mathrm {T}} \end{array} \right] ^{\mathrm {T}} \end{aligned}$$

(29)

The Jacobian of the measurement model for the ith VP and the jth line feature, $\frac{\partial h_{ij}}{\partial \mathbf {x}}$, is computed by

$$\begin{aligned} \frac{\partial h_{ij}}{\partial \mathbf {x}} = \left[ \frac{\partial h_{ij}}{\partial \mathbf {q}_{WC}} \ \ \ \frac{\partial h_{ij}}{\partial \omega _C} \ \ \ \cdots \ \ \ \frac{\partial h_{ij}}{\partial \mathbf {y}_i} \ \ \ \cdots \right] . \end{aligned}$$

(30)

When computing the Jacobian $\frac{\partial h_{ij}}{\partial \mathbf {x}}$, all the Jacobians $\frac{\partial h_{kl}}{\partial \mathbf {y}_k}$ except for $k = i$ and $l = j$ are two-dimensional column vectors consisting of zeros.

Let a quaternion be $\mathbf {q} = \left[ q_1, q_2, q_3, q_4 \right] ^{\mathrm {T}}$.

The Jacobian of the measurement model for the camera orientation, $\frac{\partial h_{ij}}{\partial \mathbf {q}_{WC}}$, is derived using Eq. (8) as

$$\begin{aligned} \frac{\partial h_{ij}}{\partial \mathbf {q}_{WC}} = \left[ \frac{\partial h_{ij}}{\partial q_1} \ \ \ \frac{\partial h_{ij}}{\partial q_2} \ \ \ \frac{\partial h_{ij}}{\partial q_3} \ \ \ \frac{\partial h_{ij}}{\partial q_4} \right] , \end{aligned}$$

(31)

where

$$\begin{aligned} \frac{\partial h_{ij}}{\partial q_1}= & {} 2 \mathbf {d}_i^{\mathrm {T}} \left[ \begin{array}{ccc} q_1 &{}\quad -\,q_4 &{}\quad q_3 \\ q_4 &{}\quad q_1 &{}\quad -\,q_2 \\ -\,q_3 &{}\quad -\,q_2 &{} \quad q_1 \end{array} \right] \mathbf {n}_{ij} , \end{aligned}$$

(32)

$$\begin{aligned} \frac{\partial h_{ij}}{\partial q_2}= & {} 2 \mathbf {d}_i^{\mathrm {T}} \left[ \begin{array}{ccc} q_2 &{}\quad q_3 &{}\quad q_4 \\ q_3 &{}\quad -\,q_2 &{}\quad -\,q_1 \\ q_4 &{}\quad -\,q_1 &{} \quad -\,q_2 \end{array} \right] \mathbf {n}_{ij} , \end{aligned}$$

(33)

$$\begin{aligned} \frac{\partial h_{ij}}{\partial q_3}= & {} 2 \mathbf {d}_i^{\mathrm {T}} \left[ \begin{array}{ccc} -\,q_3 &{} \quad q_2 &{}\quad q_1 \\ q_2 &{}\quad q_3 &{}\quad q_4 \\ -\,q_1 &{} \quad q_4 &{}\quad -\,q_3 \end{array} \right] \mathbf {n}_{ij} , \end{aligned}$$

(34)

and

$$\begin{aligned} \frac{\partial h_{ij}}{\partial q_4}= & {} 2 \mathbf {d}_i^{\mathrm {T}} \left[ \begin{array}{ccc} -\,q_4 &{}\quad -\,q_1 &{}\quad q_2 \\ q_1 &{}\quad -\,q_4 &{}\quad q_3 \\ q_2 &{}\quad q_3 &{}\quad q_4 \end{array} \right] \mathbf {n}_{ij}. \end{aligned}$$

(35)

The Jacobian of the measurement model for the angular velocity, $\frac{\partial h_{ij}}{\partial \omega _{C}}$, is a three-dimensional row vector with zero components since the measurement model does not involve the variables of the angular velocity. The Jacobian of the measurement model for the ith VD, $\frac{\partial h_{ij}}{\partial \mathbf {y}_i}$, is derived as

$$\begin{aligned} \frac{\partial h_{ij}}{\partial \mathbf {y}_i} = \frac{\partial h_{ij}}{\partial \mathbf {d}_i} \frac{\partial \mathbf {d}_{i}}{\partial \mathbf {y}_i} , \end{aligned}$$

(36)

where the left Jacobian of the right term, $\frac{\partial h_{ij}}{\partial \mathbf {d}_i}$, is computed using Eq. (8) as

$$\begin{aligned} \frac{\partial h_{ij}}{\partial \mathbf {d}_i} = \left( \mathbf {R} ( \mathbf {q}_{WC} ) \mathbf {n}_{ij} \right) ^{\mathrm {T}} , \end{aligned}$$

(37)

and the right Jacobian of the right term, $\frac{\partial \mathbf {d}_{i}}{\partial \mathbf {y}_i}$, is computed using Eq. (5) as follows.

$$\begin{aligned} \frac{\partial \mathbf {d}_{i}}{\partial \mathbf {y}_i} = \left[ \begin{array}{cc} -\,\cos \phi _i\sin \theta _i &{}\quad -\,\sin \phi _i\cos \theta _i \\ \cos \phi _i\cos \theta _i &{}\quad -\,\sin \phi _i\sin \theta _i \\ 0 &{}\quad \cos \phi _i \end{array} \right] \end{aligned}$$

(38)