A Birotation Solution for Relative Pose Problems

Previous birotation-based methods [3, 19] have been confined to an ideal stereo vision model (both cameras have an identical intrinsic matrix), primarily because their energy functions are formulated based on the constraint that $\boldsymbol{p}_{1}=(u_{1},v_{1})^{\top}$ and $\boldsymbol{p}_{2}=(u_{2},v_{2})^{\top}$, a given pair of correspondences in the two images, have the same vertical coordinates (i.e., $v_{1}=v_{2}$). Similar constraints can be observed when a single camera translates solely along either the Y-axis or the Z-axis. Specifically, the horizontal coordinates of $\boldsymbol{p}_{1}$ and $\boldsymbol{p}_{2}$ are identical (i.e., $u_{1}=u_{2}$) in the former case, while the ratios of their horizontal to vertical coordinates are identical (i.e., $\frac{u_{1}}{v_{1}}=\frac{u_{2}}{v_{2}}$) in the latter case.

Nevertheless, in general relative pose problems, the intrinsic matrices of the reference and target cameras may differ. It is therefore necessary to reformulate the above-mentioned constraints based on the following basis transformations:

where $\boldsymbol{l}_{1}=(1,0,0)^{\top}$, $\boldsymbol{l}_{2}=(0,1,0)^{\top}$, and $\boldsymbol{l}_{3}=(0,0,1)^{\top}$ are unit vectors representing pure translations along the X-, Y-, and Z-axes of the camera coordinate system, respectively, and $s_{i}\in(-\infty,+\infty)$ denotes the scaling factor corresponding to each transformation. $\boldsymbol{p}^{C}_{1,2}$ and $\boldsymbol{p}_{1,2}$ have the following relation:

where $\bar{\boldsymbol{p}}_{1,2}$ represents the normalized coordinates of $\boldsymbol{p}^{C}_{1,2}$ with respect to depth, $\tilde{\boldsymbol{p}}_{1,2}=(u_{1,2},v_{1,2},1)^{\top}$ denotes the homogeneous coordinates of $\boldsymbol{p}_{1,2}$, and

and

represent the intrinsic matrices of the reference and target cameras, respectively. By combining (7) and (8), we can reformulate the aforementioned three constraints into the following expressions:

which serve as the foundation for developing geometric metrics in the subsequent subsection.

Existing methods [3, 19] formulate the energy function as the sum of absolute residuals calculated based on the constraint presented in (11). In this subsection, we comprehensively analyze the drawbacks of existing methods in terms of absolute residual accumulation, and introduce three geometric metrics defined to quantify the distances between the relative pose to be estimated and the basis transformations.

As illustrated in Fig. 1(a), $\bar{\boldsymbol{p}}_{1}$ and $\bar{\boldsymbol{p}}_{2}$, the normalized coordinates of a pair of correspondences with respect to depth, can be respectively expressed using $\boldsymbol{\Phi}_{1}=(\phi_{1,1},\phi_{2,1},\phi_{3,1})^{\top}$ and $\boldsymbol{\Phi}_{2}=(\phi_{1,2},\phi_{2,2},\phi_{3,2})^{\top}$ as follows:

These angles satisfy the following relationship:

Combining (14) and (15) results in the following expressions:

According to (14) and (16), an absolute residual vector $\boldsymbol{\epsilon}$, formulated based on the geometric constraints presented in (11)-(13), can be expressed in terms of $\boldsymbol{\Phi}_{1}$ and $\boldsymbol{\Phi}_{2}$ as follows:

${\epsilon}_{i}$, the $i$-th element of $\boldsymbol{\epsilon}$, is illustrated in Fig. 1(b)-(d), respectively. As the relative pose to be estimated nears alignment with the $i$-th basis transformation, $\Delta\phi_{i}=\lvert\phi_{i,1}-\phi_{i,2}\rvert$ approaches zero. The ratio of ${\epsilon}_{i}$ to $\Delta\phi_{i}$ can be formulated as follows:

which leads to the following expression:

indicating that ${\epsilon}_{i}$ increases monotonically with respect to $\phi_{i,1}$. Therefore, the implicit weights of residuals currently in use are subject to pixel coordinates. To illustrate this bias, we visualize the normalized implicit weights under different constraints, as shown in Fig. 2. Specifically, under the first constraint, the implicit weight increases along the vertical axis (see Fig. 2(a)), peaking near the top and bottom image boundaries and reaching a minimum near the horizontal midline. Similarly, under the second constraint, the implicit weight increases along the horizontal axis (see Fig. 2(b)), peaking near the left and right image boundaries and reaching a minimum near the vertical midline. Nonetheless, as illustrated in Fig. 2(c), under the third constraint, the implicit weights along the horizontal midline reach the maximum, whereas those along the vertical midline reach the minimum. In addition, the implicit weights are symmetric about the image center and decrease monotonically as the line passing through the center rotates from the horizontal to the vertical midline. This phenomenon emphasizes the instability of previous relative pose estimation approaches, particularly when correspondences from specific regions, such as treetops or sky, are utilized.

To overcome this limitation, we adjust the three constraints in (11)-(13) using an inverse trigonometric function, as expressed as follows:

The new absolute residual vector $\boldsymbol{\epsilon}^{\prime}$ can therefore be expressed as follows:

which eliminates the influence of implicit weights in residual accumulation. Comparisons between the absolute residuals ${\epsilon}_{i}$ and ${\epsilon}_{i}^{\prime}$ are also given in Fig. 1(b)-(d).

In accordance with the new constraints defined in (20)-(22), we introduce three geometric metrics that correspond to the three basis transformations as follows:

The value of $d_{i}$ represents the distance between the actual relative pose and the $i$-th basis transformation. A smaller $d_{i}$ indicates a closer alignment between the actual relative pose and the $i$-th basis transformation. When exactly one of the three distances approaches zero, the actual relative pose aligns with the corresponding basis transformation. Additionally, it is worth noting that when the camera remains stationary, all three distances approach zero.

Building on the above-mentioned basis transformations and geometric metrics, we introduce three birotation solutions to the relative pose estimation problem.

We use two rotation matrices, $\boldsymbol{R}_{1}$ and $\boldsymbol{R}_{2}$, to apply pure rotation transformations to the reference and target camera coordinate systems, respectively, in order to align them with the $i$-th basis transformation. This process can be formulated as follows:

where $\boldsymbol{p}^{C^{\prime}}_{1,2}$ denotes the transformed coordinates of $\boldsymbol{p}^{C}_{1,2}$ in the new camera coordinate systems. We refer to (27) as the $i$-th birotation model for each $i\in\{1,2,3\}$, where the corresponding parameters $\boldsymbol{R}_{1}$, $\boldsymbol{R}_{2}$, $s_{i}$, and $\boldsymbol{l}_{i}$ together constitute the $i$-th birotation solution for the relative pose problem.

By substituting the $i$-th birotation model into the geometric metrics presented in (24)-(26), the three geometric metrics can be reformulated as functions of $\boldsymbol{R}_{1}$ and $\boldsymbol{R}_{2}$, as follows:

where $(i\ j\ k)$ forms an even permutation [27] of the set $\{1,2,3\}$ when $i\in\{1,3\}$, and an odd permutation when $i=2$. Given the scaling factor $s_{i}$, the $i$-th birotation solution can be obtained by minimizing $d_{i}$.

The relative pose $\boldsymbol{P}$ and the essential matrix $\boldsymbol{E}$ can be derived from the birotation solutions. By multiplying both sides of $\tilde{\boldsymbol{p}}^{C}_{2}=\boldsymbol{P}\tilde{\boldsymbol{p}}^{C}_{1}$ with the matrix $\begin{pmatrix}\boldsymbol{R}_{2}&\boldsymbol{0}\end{pmatrix}$, we have the following expression:

According to the $i$-th birotation model defined in (27), the following relationship holds:

By combining (29) and (30), the rotation matrix $\boldsymbol{R}$ and translation vector $\boldsymbol{t}$ can be determined from the $i$-th birotation solution as follows:

Ideally, the three birotation solutions yield an identical relative pose. The essential matrix $\boldsymbol{E}$ can then be derived from the birotation solutions as follows:

However, it should be noted here that all three birotation solutions are not unique, as each solution has six DoFs (three from each rotation matrix), while the relative pose problem with ambiguous scale has only five DoFs. Specifically, rotating both the reference and target camera coordinate systems by the same angle around the translation vector does not change their relative pose, which geometrically demonstrates the redundant DoF in each birotation solution. Thus, additional geometric constraints should be incorporated into our proposed relative pose estimation algorithm, detailed in the next subsection, to resolve this ambiguity.

According to the constraints presented in (20)-(22) and the birotation models defined in (27), we can obtain three $N$-entry residual vectors using $N$ pairs of correspondences, as expressed as follows:

where

$\boldsymbol{\theta}_{1,i}$ and $\boldsymbol{\theta}_{2,i}$ denote the rotation vectors corresponding to the rotation matrices $\boldsymbol{R}_{1,i}$ and $\boldsymbol{R}_{2,i}$ in the $i$-th birotation model, respectively, $\bar{\boldsymbol{p}}_{1,n}$ and $\bar{\boldsymbol{p}}_{2,n}$ represent the normalized coordinates of the $n$-th pair of correspondences with respect to depth, $\boldsymbol{r}_{1,j,i}$ and $\boldsymbol{r}_{1,k,i}$ denote the $j$-th and $k$-th row vector of $\boldsymbol{R}_{1,i}$, respectively, and $\boldsymbol{r}_{2,j,i}$ and $\boldsymbol{r}_{2,k,i}$ are the $j$-th and $k$-th row vector of $\boldsymbol{R}_{2,i}$, respectively. $\hat{d}_{i}$, the discretized approximation of $d_{i}$ defined in (28), can be yielded by accumulating residuals defined in (34) as follows:

where $\boldsymbol{\Lambda}$ is a diagonal matrix in which each element $\lambda_{n}$ adaptively takes a value of either 0 or 1 to indicate the reliability of the $n$-th pair of correspondences. By incorporating a regularization term into $\hat{d}_{i}$, the complete energy function for the $i$-th birotation model can be formulated as follows:

where $R(\boldsymbol{\theta}_{1,i},\boldsymbol{\theta}_{2,i})=\|\boldsymbol{\theta}_{1,i}\|^{2}_{2}+\|\boldsymbol{\theta}_{2,i}\|^{2}_{2}$ denotes the $L_{2}$ regularization term, and $\alpha>0$ represents the regularization parameter. The regularization term is incorporated to reduce the energy function’s non-convexity and ensure the solution’s uniqueness.

The $i$-th birotation solution can be yielded by minimizing (36) using our newly designed iterative optimization approach. Specifically, based on (31), the three birotation solutions are initialized using the prior rotation $\boldsymbol{R}_{init}$ and translation $\boldsymbol{t}_{init}$ as follows:

An overview of a single iteration of the optimization process is illustrated in Fig. 3. We first derive a first-order approximation of $\boldsymbol{e}_{i}$, which is then applied to the energy function $E_{i}$ as follows:

where $\Delta\boldsymbol{\Theta}_{i}=(\Delta\boldsymbol{\theta}^{\top}_{1,i},\Delta\boldsymbol{\theta}^{\top}_{2,i})^{\top}$ denotes the increment vector at each iteration, and

is a matrix storing the gradients of $\boldsymbol{e}_{i}$ with respect to $\boldsymbol{\theta}_{1,i}$ and $\boldsymbol{\theta}_{2,i}$ computed using the left perturbation model [18, 19], which avoids the need to compute the Baker-Campbell-Hausdorff Jacobian matrix at each iteration. Under the definitions of $\boldsymbol{p}^{C^{\prime}}_{1,n,i}=\boldsymbol{R}_{1,i}\boldsymbol{p}^{C}_{1,n}=(x^{\prime}_{1,n,i},y^{\prime}_{1,n,i},z^{\prime}_{1,n,i})^{\top}$ and $\boldsymbol{p}^{C^{\prime}}_{2,n,i}=\boldsymbol{R}_{2,i}\boldsymbol{p}^{C}_{2,n}=(x^{\prime}_{2,n,i},y^{\prime}_{2,n,i},z^{\prime}_{2,n,i})^{\top}$, we can derive the following expressions:

Setting the derivative of (42) with respect to $\Delta\boldsymbol{\Theta}_{i}$ to zero yields the following expression:

Subsequently, $\boldsymbol{R}_{1,i}$ and $\boldsymbol{R}_{2,i}$ are updated using the left perturbation model, as follows:

where $\boldsymbol{R}^{\prime}_{1,i}$ and $\boldsymbol{R}^{\prime}_{2,i}$ denote the updated rotation matrices after one iteration. The iterative process terminates when either the value or the change rate of $\frac{\hat{d}_{i}}{N}$ falls below predefined thresholds. After the iteration terminates, the final solution is selected from the three birotation solutions based on the following criteria:

where weighting coefficients $(\beta_{1},\beta_{2},\beta_{3})$ can be empirically adjusted to prioritize one of the three birotation solutions, which can greatly enhance the reliability of the final solution. The relative rotation $\boldsymbol{R}$ and the line along which the translation vector $\boldsymbol{t}$ lies can then be derived from $\boldsymbol{R}_{1}$, $\boldsymbol{R}_{2}$, and their corresponding index $\hat{i}$ using (31).

In addition, although the absolute value of the scaling factor $s_{\hat{i}}$ is unknown, its sign can be determined based on the transformed coordinates $\boldsymbol{p}^{C^{\prime}}_{1,n,\hat{i}}$ and $\boldsymbol{p}^{C^{\prime}}_{2,n,\hat{i}}$ using the following expression when $z^{\prime}_{1,n,\hat{i}}$ and $z^{\prime}_{2,n,\hat{i}}$ are positive:

When the depth of either $\boldsymbol{p}^{C^{\prime}}_{1,n,\hat{i}}$ or $\boldsymbol{p}^{C^{\prime}}_{2,n,\hat{i}}$ is negative, the direction of the translation is determined by computing the depths of both $\boldsymbol{p}^{C}_{1,n}$ and $\boldsymbol{p}^{C}_{2,n}$. The translation yielding two positive depths is selected as the valid solution. With convergence-related parameters fixed, the computational complexity of our relative pose estimation algorithm remains $\mathcal{O}(N)$.