# Parallax Bundle Adjustment on Manifold with Convexified Initialization

Liyang Liu1, Teng Zhang1, Yi Liu2, Brenton Leighton1, Liang Zhao1, Shoudong
Huang1 and Gamini Dissanayake1 1Liyang Liu, Teng Zhang, Brenton Leighton,
Liang Zhao, Shoudong Huang and Gamini Dissanayake are with the Center for
Autonomous Systems (CAS), University of Technology Sydney, Ultimo, NSW 2007,
Australia. {Liyang.Liu, Teng.Zhang, Brenton.Leighton, Liang.Zhao,
Shoudong.Huang<EMAIL_ADDRESS>Yi Liu is with School of
Automation, Huazhong University of Science and Technology, Wuhan 430074,
China<EMAIL_ADDRESS>
###### Abstract

Bundle adjustment (BA) with parallax angle based feature parameterization has
been shown to have superior performance over BA using inverse depth or XYZ
feature forms. In this paper, we propose an improved version of the parallax
BA algorithm (PMBA) by extending it to the manifold domain along with
observation-ray based objective function. With this modification, the problem
formulation faithfully mimics the projective nature in a camera’s image
formation, BA is able to achieve better convergence, accuracy and robustness.
This is particularly useful in handling diverse outdoor environments and
collinear motion modes. Capitalizing on these properties, we further propose a
pose-graph simplification to PMBA, with significant dimensionality reduction.
This pose-graph model is convex in nature, easy to solve and its solution can
serve as a good initial guess to the original BA problem which is
intrinsically non-convex. We provide theoretical proof that our global
initialization strategy can guarantee a near-optimal solution. Using a series
of experiments involving diverse environmental conditions and motions, we
demonstrate PMBA’s superior convergence performance in comparison to other BA
methods. We also show that, without incremental initialization or via third-
party information, our global initialization process helps to bootstrap the
full BA successfully in various scenarios, sequential or out-of-order,
including some datasets from the “Bundle Adjustment in the Large” database.

##  I Introduction

Structure from Motion (SfM) / visual SLAM estimates 3D scene structures and
camera poses simultaneously from 2D images. Bundle adjustment is the gold
standard method of SfM, in that it finds optimal pose and map in the least
squares sense to best explain the data. Solving such a non-linear least
squares problem typically requires iterative Newton methodology: start with an
initial guess, repetitively add increments by solving a normal equation until
convergence.

TABLE I: Three types of Newton-based methods GN | LM | DL
---|---|---
$\triangle\mathbf{x}=\mathbf{H}^{-1}\mathbf{e}(\mathbf{x})$ | $\triangle\mathbf{x}=(\mathbf{H}+\lambda\mathbf{I})^{-1}\mathbf{e}(\mathbf{x})$ | $\triangle\mathbf{x}=(\lambda_{1}\mathbf{H}^{-1}+\lambda_{2}\mathbf{I})\mathbf{e}(\mathbf{x})$
Figure 1: Compare BA for “Malaga dataset”: existence of collinear features
(yellow dots) cause IDP (brown) and XYZ (green) to differ significantly from
Ground Truth (red); PMBA (blue) and PBA [1] (orange) do not show this issue,
with PMBA having the fastest convergence rate, see Fig. 8(_a_).

As shown in Table I, this approach comes in three forms: original Gauss-Newton
(GN) when the equation is easy to solve (the Hessian matrix $\mathbf{H}$ has a
small condition number), Levenberg Macquardt (LM) as a damped GN if Hessian is
near singular, and DogLeg (DL) as a combination of GN and the steepest descent
method for fast convergence. LM is a favourite of the BA community for its
safe handling despite its slowness. GN and DL are both considered risky due to
the large step size and are often avoided.

Problematic features
In many modern BA systems [2][3][4], a 3D feature point is parameterized as
Euclidean coordinates (XYZ) or inverse depth (IDP). A well-known problem for
these representations is that when far away features exist or when camera
poses observing a feature are collinear with the feature, the Hessian becomes
ill-conditioned. A small change in error function leads to a large jump in the
state variable, significantly affecting BA’s robustness, efficiency and
accuracy. See Fig. 1 and Fig. 8(_a_) for illustration of failure in
conventional BA.

To deal with this problem, several remedies are commonly adopted. The
fundamental principle is separate treatment for problematic features and good
ones. ORB-SLAM [5] uses a prudent feature selection strategy where features
with in-sufficient parallax angles are discarded. A hybrid method was proposed
in [6], that first estimates camera orientations with remote features then
optimises with poses and near features. The vision smart factor proposed in
[7] (implemented in GTSAM [4]) shares the same approach of [6]. It avoids
degenerate cases by using a flexible-size error function. Recently [8]
proposed a solution in which less weighting is given to the error terms for
“problematic” features.

Compared to the afore-mentioned methods, our proposed algorithm PMBA treats
the problem with a totally different viewpoint. We argue that  the root cause
for ill-conditioned cases is that feature uncertainty for conventional BAs is
NOT uniformly bounded. In our previous work [9][1], we used three angles
(elevation, azimuth and parallax) to define structure of a feature without
involving depth. [1] demonstrated that this parameterization is closer to the
measurement space of projective geometry, parallax-based BA (we call it PBA in
this paper) is more robust and efficient compared to BA’s in XYZ or IDP form.
We will present our improved manifold version – PMBA that faithfully complies
with projective geometry in computer vision. This results in a non-singular
Hessian and a bounded error function that is suitable for faster
implementation.

Initialization methods
BA due to its highly non-convex nature, requires good initial estimate to
converge to global minimum. The common initialization methods include
incremental or global. In incremental strategy, with a simple start, many mid-
level BAs are performed on each new pose insertion. Incremental strategy draws
the criticism that it is slow and leads to drifting for long sequences of
data. Example systems are VisualSFM [10], Bundler [11] and ORB-SLAM [5]. The
alternative is global initialization where all camera poses are initialised
simultaneously. Global SfM thus bootstrapped shows higher efficiency and
accuracy. This strategy exposes many research challenges, and has been studied
intensively in [12][13][14][15].

This paper builds on the previous PBA algorithm [9][1] and makes the following
improvements: (1) recalls the conventional BA methods and analyzes its
limitations (Section II); (2) an improved PBA on manifold formulation that is
able to fully avoid “problematic feature” induced ill-conditioned cases
(Section III) ; (3) a simple but effective global initialization method using
convexified pose-graph model that is compatible with PMBA, which can guarantee
a near-optimal solution (Section IV); (4) to demonstrate the two improvements,
we provide both theoretical proof and experimental results from a series of
large-scale datasets, sequential or out-of-order (Section V).

Notations:

  * •

${S}(\mathbf{x})$ is a skew symmetric matrix from vector
$\mathbf{x}\in\mathbb{R}^{3}$, equivalent to cross-product operator,
${S}(\mathbf{x})\mathbf{y}=\mathbf{x}\times\mathbf{y}$

  * •

The term $\mathbf{T}_{i}=(\mathbf{R}_{i},\mathbf{p}_{i})\in\mathbb{SE}(3)$
represents the camera pose at time-step $i$.

  * •

Subscript (l) indicates frame is local.

  * •

Decoration $\,\breve{}\,$ indicates vector is normalized:
$\breve{\mathbf{N}}_{j,i}=\frac{\mathbf{N}_{j,i}}{\|\mathbf{N}_{j,i}\|}$.

##  II Background Knowledge

In this section, we first recall the monocular SLAM problem and conventional
BA. We then analyze the potential problems in this formulation.

The visual SLAM problem estimates camera poses
$\mathbf{T}=\\{(\mathbf{R}_{i},\mathbf{p}_{i})\\}_{i=1,\cdots,M}$ and feature
positions $\mathbf{f}=\\{\mathbf{f}_{j}\in\mathbb{R}^{3}\\}_{j=1,\cdots,N}$
from a set of images $\\{I_{i}\\}$. When the feature $j$ is observed from the
pose $\mathbf{T}_{i}$, the monocular sensor intercepts the light ray
$\mathbf{N}_{j,i}$ that passes through its centre to the feature point in the
form of image pixel $\mathbf{u}_{m_{j,i}}$, as shown in Fig. 2(_a_). Table II
lists different expressions the observation ray can have.

|
---|---
|  (_a_) Camera measurement formation
---
and BA error functions
|  (_b_) PMBA reformatted as a QPLC
---
problem: minimize cross product
Figure 2: Projective vision and error functions in PMBA TABLE II: Various forms of observation ray in this paper Global ray |  Global ray direction | Local ray |  Local ray direction
---|---|---|---
$\mathbf{N}_{j,i}=\mathbf{f}_{j}-\mathbf{p}_{i}$ | $\mathbf{\breve{N}}_{j,i}=\frac{\mathbf{f}_{j}-\mathbf{p}_{i}}{\|\mathbf{f}_{j}-\mathbf{p}_{i}\|}$ | $\mathbf{N}_{j,i}^{(l)}=\mathbf{R}_{i}^{\intercal}(\mathbf{f}_{j}-\mathbf{p}_{i})$ | $\mathbf{\breve{N}}_{j,i}^{(l)}=\frac{\mathbf{R}_{i}^{\intercal}(\mathbf{f}_{j}-\mathbf{p}_{i})}{\|\mathbf{R}_{i}^{\intercal}(\mathbf{f}_{j}-\mathbf{p}_{i})\|}$

The information $\mathbf{N}_{i,j}$ encodes constitute constraints in a maximum
a posterior (MAP) problem for poses and points.

$\min_{\mathbf{T},\mathbf{f}}\sum_{i,j}\|e_{ij}(\breve{\mathbf{N}}_{j,i}^{(l)})\|^{2}=\min_{\mathbf{T},\mathbf{f}}\sum_{i,j}\|e_{ij}(\frac{\mathbf{R}_{i}^{\intercal}\mathbf{N}_{j,i}}{\|\mathbf{R}_{i}^{\intercal}\mathbf{N}_{j,i}\|})\|^{2}.$
(1)

In conventional BA, the error function $e_{ij}(\cdot)$ is given by:

$e_{ij}(\mathbf{f}_{j}):=\mathbf{K}\circ\pi(\mathbf{R}_{i}^{\intercal}(\mathbf{f}_{j}-\mathbf{p}_{i}))-\mathbf{u}_{m_{j,i}}\quad\in\mathbb{R}^{2}.$
(2)

BA with conventional parameterization and cost function suffers from the
issues listed below:

  * •

Ill-conditioned case due to problematic features: Although these features
still contain some information, they cause singularity in the Hessian matrix,
a main contribution to GN divergence and numerical instability.

  * •

Slow convergence: To deal with singularity, slow LM is commonly used for safe
increment, DL and GN are avoided, and efficiency is compromised for stability.

  * •

Stop criteria: Small changes in the error cost lead to large variation in the
state variable, making it difficult to specify a consistent stop criterion.

  * •

Local minimum: the error function (2) does not distinguish between in-frustum
or behind camera features, thus causing many local minima and saddle points. A
good example is the two-view geometry problem in which there are multiple
global minima for the BA formulation such that further manual intervention is
needed to pick the feasible solution.

In light of above discussion, safe-handling of ill-conditioned cases is
vitally important for robustness, accuracy and efficiency of visual SLAM.

##  III Parallax Bundle Adjustment on Manifold

In this section, we introduce the BA method using parallax angle in manifold
domain (PMBA). We provide a thorough theoretical analysis on the boundedness
of its information matrix, hence prove its smooth convergence without issues
of singularity. We also show the error function is bounded and globally
continuous. All these factors lead to possibility of faster optimization
method DL, a significant improvement than previous work [9][1].

###  III-A Feature parameterization

|
---|---
(_a_) Feature $\mathbf{f}_{j}$ anchored by $\mathbf{p}_{m_{j}}$ and $\mathbf{p}_{a_{j}}$, $\theta_{j}$ is parallax angle between anchor rays, $\mathbf{n}_{j}$ is ray direction in main’s frame, $\mathbf{p}_{i}$ is an arbitrary pose position co-visible for $\mathbf{f}_{j}$. |  (_b_) Convexification of PMBA: Rotate $(\mathbf{p}_{a}-\mathbf{p}_{m})$ about $\mathbf{n}_{zj}$ by ($\pi-\alpha_{j}$) becomes $\|\mathbf{p}_{m}-\mathbf{p}_{a}\|\mathbf{R}_{m_{j}}$.
Figure 3: The geometric structure about feature $j$ and its anchors in PMBA.

A feature’s depth information is implied in the parallax between observations
from different viewpoints. For a feature $\mathbf{f}_{j}$, amongst the set of
cameras $\mathbb{T}_{j}$ to which $\mathbf{f}_{j}$ is visible, we choose a
main anchor $\mathbf{T}_{m_{j}}$ and an associate anchor $\mathbf{T}_{a_{j}}$
that form best parallax angle from their observation rays. This geometric
relationship among the feature $j$ is illustrated in Fig. 3 _(a)_. The feature
$\mathbf{f}_{j}$ can be over-parameterized by the unit observation ray vector
$\mathbf{n}_{j}$ in main-anchor frame, and the parallax angle $\theta_{j}$,
i.e.,

$\mathbf{F}_{j}=(\cos\theta_{j},\sin\theta_{j},\mathbf{n}_{j})$ (3)

The new parameterization $\mathbf{F}_{j}$ only defines the relative structure
of the feature with respect to its two anchors. The scale of the feature
$\mathbf{f}_{j}$ is implicitly defined by the relative translation of the two
anchors, computed as

$\displaystyle\mathbf{f}_{j}$
$\displaystyle=d_{j}\mathbf{R}_{m_{j}}\mathbf{n}_{j}+\mathbf{p}_{m_{j}}$ (4)
$\displaystyle=\frac{\sin(\alpha_{j}-\theta_{j})}{\sin(\theta_{j})}\|\mathbf{p}_{m_{j}}-\mathbf{p}_{a_{j}}\|\mathbf{R}_{m_{j}}\mathbf{n}_{j}+\mathbf{p}_{m_{j}}$

where

  * •

$d_{j}=\frac{\sin(\alpha_{j}-\theta_{j})}{\sin(\theta_{j})}\|\mathbf{p}_{m_{j}}-\mathbf{p}_{a_{j}}\|$
is the local depth of the feature $j$ in the main anchor frame, from sine
rule.

  * •

$\mathbf{R}_{m_{j}}$ is the rotation for main anchor frame
$\mathbf{T}_{m_{j}}$.

  * •

$\mathbf{n}_{j}\in\mathbb{R}^{3}$ is the direction of observation ray from
point $\mathbf{f}_{j}$ to point $\mathbf{p}_{m_{j}}$, local in main anchor
frame $\mathbf{T}_{m_{j}}$.

  * •

$\theta_{j}\in(0,\pi)$ is the parallax angle between the vector
$\mathbf{f}_{j}-\mathbf{p}_{m_{j}}$ and the vector
$\mathbf{f}_{j}-\mathbf{p}_{a_{j}}$.

  * •

$\alpha_{j}=\arccos(\frac{(\mathbf{p}_{m_{j}}-\mathbf{p}_{a_{j}})^{\intercal}\mathbf{R}_{m_{j}}\mathbf{n}_{j}}{\|\mathbf{p}_{m_{j}}-\mathbf{p}_{a_{j}}\|})$
is the angle between vector $(\mathbf{p}_{m_{j}}-\mathbf{p}_{a_{j}})$ and
vector $\mathbf{R}_{m_{j}}\mathbf{n}_{j}$.

###### Remark 1

In the original PBA parameterization [1], ray direction $\mathbf{n}_{j}$ was
defined by an elevation and azimuth angle in the global frame, camera’s
orientation $\\{\mathbf{R}_{i}\\}$ in Euler angles. Expressing direction in
sinusoids of angles is a potential source of singularity. In PMBA, both
$\mathbf{n}_{j}$ and $\mathbf{R}_{i}$ are in the manifold domain. Moreoever,
$\mathbf{n}_{j}$ is defined in $\mathbf{T}_{m_{j}}$’s local frame, for ease of
multi-camera system application.

###  III-B State retraction in manifold

Figure 4: Retraction of ray $\mathbf{n}$ in main anchor

Optimization in manifold follows a 3 step procedure [16]: lift a manifold
variable to its tangent space, solve a normal equation to obtain the Euclidean
increment, and retract back to manifold. We adopt method in [17][18] for pose
retraction. For feature’s ray direction, we give a natural definition of
uncertainty as a normally distributed rotational perturbation to the
directional vector as shown in Fig. 4. The rotation’s axis constitutes a plane
normal to the ray passing through the observing camera, and is the tangent
space, summarized in the following equation:

$\tilde{\mathbf{n}}_{j}=\mathrm{Exp}(\mathbf{A}_{\mathbf{n}_{j}}{\delta\mathbf{n}_{j}})\mathbf{n}_{j},\qquad\delta\mathbf{n}_{j}\in\mathcal{N}(0,\Sigma).$
(5)

where $\delta\mathbf{n}_{j}\in\mathbb{R}^{2}$,
$\mathbf{A}_{\mathbf{n}_{j}}\in\mathbb{R}^{3\times 2}$ and
$[\mathbf{A}_{\mathbf{n}_{j}}\>\mathbf{n}_{j}]\in\mathbb{SO}(3)$. The optimal
perturbation is the increment for retraction $\oplus$:

$\mathbf{F}_{j}\oplus\delta\mathbf{F}_{j}=(\cos(\theta_{j}+\delta\theta_{j}),\sin(\theta_{j}+\delta\theta_{j}),\mathrm{Exp}(\mathbf{A}_{\mathbf{n}_{j}}\delta\mathbf{n}_{j})\mathbf{n}_{j}).$
(6)

where the total increment
$\delta\mathbf{F}_{j}=\begin{bmatrix}\delta\theta_{j},\delta\mathbf{n}_{j}\end{bmatrix}\in\mathbb{R}^{3}$
has same dimensionality as conventional parameterization.

###  III-C Error function and optimization formulation

After determining the main anchor $\mathbf{T}_{m_{j}}$ and the associated
$\mathbf{T}_{a_{j}}$ for each feature $j$, we can rewrite the nonlinear least
squares problem (1) using the new feature parametrization

$\min_{\mathcal{X}}\|f(\mathcal{X})\|^{2}=\min_{\mathbf{T},\mathbf{F}}\sum_{i\in\mathbb{T}_{j},j}\|e_{ij}(\frac{\mathbf{R}_{i}^{\intercal}\mathbf{N}_{j,i}}{\|\mathbf{R}_{i}^{\intercal}\mathbf{N}_{j,i}\|})\|^{2},\\\
$ (7)

where $\mathbf{F}=\\{\mathbf{F}_{j}\\}_{j=1,\cdots,N}$ and
$\mathcal{X}=(\mathbf{T},\mathbf{F})$. We now give ray $\mathbf{N}_{j,i}$ a
new definition (with abuse of notation): the original ray vector scaled up by
a factor of $\sin(\theta_{j})$, for convenience of mathematical manipulation,
i.e.,

$\displaystyle\mathbf{N}_{j,i}:=$
$\displaystyle\sin(\theta_{j})(\mathbf{f}_{j}-\mathbf{p}_{j})$ (8)
$\displaystyle=$
$\displaystyle\sin(\alpha_{j}-\theta_{j})\|\mathbf{p}_{m_{j}}-\mathbf{p}_{a_{j}}\|\mathbf{R}_{m_{j}}\mathbf{n}_{j}$
$\displaystyle+\sin(\theta_{j})(\mathbf{p}_{m_{j}}-\mathbf{p}_{i}).$

We also introduce a ray direction based error function, different from the
conventional pixel-based error function (2) (shown in Fig. 2), i.e.,

$\mathbf{e}_{ij}(\breve{\mathbf{N}}_{j,i}^{(l)}):=\breve{\mathbf{f}}^{(l)}_{m_{j,i}}-\breve{\mathbf{N}}_{j,i}^{(l)}\in\mathbb{R}^{3},$
(9)

where
$\breve{\mathbf{f}}^{(l)}{m_{j,i}}=\frac{\mathbf{K}^{-1}\mathbf{u}_{m_{j,i}}}{\|\mathbf{K}^{-1}\mathbf{u}_{m_{j,i}}\|}\in\mathbb{R}^{3}$
is the measured directional vector for the feature $j$ in the pose
$\mathbf{T}_{i}$. From now on, we use $\breve{\mathbf{f}}^{(l)}_{j,i}$ to
refer to $\breve{\mathbf{f}}^{(l)}{m_{j,i}}$ for simplicity.
We further simplify (7) by moving measurement to global frame

$\min_{\mathcal{X}}\|f(\mathcal{X})\|^{2}=\min_{\mathbf{T},\mathbf{F}}\sum_{i\in\mathbb{T}_{j},j}\|\breve{\mathbf{N}}_{j,i}-\mathbf{R}_{i}\breve{\mathbf{f}}_{j,i}^{(l)}\|^{2}.\\\
$ (10)

###### Remark 2

The error function (9) is globally continuous and its derivative is bounded,
unlike the commonly used error function (2). Its dimensionality is extended to
3D from 2D, meaning observation direction is also taken into consideration
during optimization.

Specifically,

  * •

The error equation (9) implies the residual
$\|\mathbf{e}_{ij}\|=2\sin(\frac{\beta}{2})$, where $\beta$ is the angle
between the estimated and measured ray direction. Thus, the error equation is
bounded.

  * •

In contrast to conventional 2D cost functions, our error function (9) operates
in 3D and thus can handle the case when the feature point lies behind the
observing camera.

###  III-D Theoretical analysis on behaviour of information matrix

Consider the Hessian matrix of the problem (7)

$\mathbf{H}=\mathbf{J}^{\intercal}\mathbf{J}=\begin{bmatrix}\mathbf{H}_{\mathbf{T}\mathbf{T}}&\mathbf{H}_{\mathbf{T}\mathbf{F}}\\\
\mathbf{H}^{\intercal}_{\mathbf{T}\mathbf{F}}&\mathbf{H}_{\mathbf{F}\mathbf{F}}\\\
\end{bmatrix},$ (11)

where $\mathbf{J}:=\frac{\partial
f(\mathcal{X}\oplus\Delta\mathcal{X})}{\partial\Delta\mathcal{X}}|_{\Delta\mathcal{X}=\mathbf{0}}$
and
$\mathcal{X}\oplus\Delta\mathcal{X}:=(\mathbf{T}\oplus\Delta\mathbf{T},\mathbf{F}\oplus\Delta\mathbf{F})$.
Like the Hessian matrix in conventional BA, $\mathbf{H}_{\mathbf{FF}}$ is
block diagonal. With the Schur’s complement method, the dominant computation
in each Newton method’s iteration is about solving the following normal
equation:

$(\mathbf{H}_{\mathbf{TT}}-\mathbf{H}_{\mathbf{TF}}\mathbf{H}^{-1}_{\mathbf{FF}}\mathbf{H}^{\intercal}_{\mathbf{\mathbf{TF}}})\Delta\mathbf{T}=-\mathbf{C}f(\mathcal{X}),$
(12)

where
$\mathbf{C}=\begin{bmatrix}\mathbf{I}&\mathbf{H}_{\mathbf{TF}}\mathbf{H}^{-1}_{\mathbf{FF}}\end{bmatrix}$.
In conventional BA, existence of problematic features makes the matrix
$\mathbf{H}_{\mathbf{TT}}-\mathbf{H}_{\mathbf{TF}}\mathbf{H}^{-1}_{\mathbf{FF}}\mathbf{H}^{\intercal}_{\mathbf{\mathbf{TF}}}$
and the block matrix $\mathbf{H}_{\mathbf{FF}}$ (with slight abuse of
notation) ill-conditioned at the neighborhood of global minimum. The global
minimum locates at a “long flat valley” [1] such that solvers fail or require
long iterations to converge, see Fig. 8 _(a)_ for illustration.

In comparison, PMBA’s formulation (10), thanks to the re-defined retraction
(6) and the error function (9), has an uncluttered Hessian, can therefore
fully avoid the ill-conditioned cases caused by “problematic” features.

###### Theorem 1

Under the formulation (10), $\mathbf{H}_{\mathbf{FF}}$ is consistently non-
singular for any $\mathcal{X}$ and $\mathbf{H}_{\mathbf{FF}}\geq\mathbf{I}$.

###### Proof:

See Appendix -A. ∎

Theorem 1 completely suppresses all ill-conditioned $\mathbf{H}_{\mathbf{FF}}$
such that achieving convergence becomes much eaiser. As a result, DL can be
safely used for efficiency. One can also appreciate Theorem 1 from an
Information Theory perspective: the Hessian matrix at global minimum is the
inverse of the covariance matrix (up to a scale) and thus the uncertainty of
the parallax angle $\theta_{j}$ and the direction $\mathbf{n}_{j}$ is
uniformly bounded.

###### Remark 3

The original PBA [1] cannot guarantee non-singularity in
$\mathbf{H}_{\mathbf{FF}}$ due to use of standard addition retraction for
feature, Euler angles for orientation and the error function (2).

###### Remark 4

Although the matrices $\mathbf{H}_{\mathbf{TT}}$ and
$\mathbf{H}_{\mathbf{TF}}$ are denser, compared to those in XYZ or IDP,
$\mathbf{H}_{\mathbf{TT}}-\mathbf{H}_{\mathbf{TF}}\mathbf{H}^{-1}_{\mathbf{FF}}\mathbf{H}^{\intercal}_{\mathbf{\mathbf{TF}}}$
shows same sparsity. Thus the computational time for each iteration in PMBA is
comparable to conventional BA, see [1] for proof.

##  IV Global Initialization

In this section, we derive a novel initialization strategy. We do this in two
steps: An orientations and parallax feature initialization step that involves
cheap rotation averaging and anchor selection, without the need of expensive
triangulation; then a translation-averaging method using a simplified convex
pose-graph optimization. We prove that a near-optimal solution can be obtained
by this strategy. This process is illustrated in Fig. 5.

Figure 5:  Full Global Initialization + PMBA pipeline.

###  IV-A Orientation and feature initialization

Following the approach in [19][13][20], we first compute an initial guess for
orientation $\\{\mathbf{R}_{i}\\}_{i=1,\cdots,M}$. For each pair of pose
$\mathbf{T}_{i}$ and $\mathbf{T}_{k}$ with common features above a threshold,
we extract its Epipolar Geometry (EG)
$(\tilde{\mathbf{R}}_{i,k},\tilde{\mathbf{T}}_{i,k})$ by Kneip’s 5-point
algorithm [21]. We then use the state-of-the-art chordal initialization [22]
to accurately compute $\\{\bar{\mathbf{R}}_{i}\\}$. We now feed
$\\{\bar{\mathbf{R}}_{i}\\}$ into OpenGV’s two-pt ransac module [23], to
obtain translation directions $\\{\bar{\mathbf{T}}_{i,k}\\}_{i=1,\cdots,M}$.

Having obtained accurate estimates for orientations and EG-pairs, we are ready
to perform feature initialization. The default anchor selection strategy was
given in [1]. We use the same algorithm for anchor selection, with the small
change that co-visible pose scanned in pick anchors have to be part of an EG-
pair. This step ensures best as-can-be parallax angle be given to each feature
point. We stress that any problematic features corresponding to low parallax
angles do stay in the state and do not affect convergence under PMBA. Good
features together with problem ones work together to shape the final solution.

###### Remark 5

PMBA parameterization does not involve scale calculation, the selection
algorithm in [1] utilizes this property and only makes use of camera rotations
to compute feature values in a fast and accurate way, we thus completely avoid
unreliable/expensive linear triangulation.

###  IV-B Position initialization

After orientation and feature initialization, we can perform position
initialization. We do this by approximating the original non-linear ray
$\mathbf{N}_{j,i}$ function (10) with a linear relation of positions, helped
with a rotation trick, as illustrated in Fig. 3 _(b)_. Now we give
$\mathbf{N}_{j,i}$ a new formulation:

$\displaystyle\bar{\mathbf{N}}_{j,i}=$
$\displaystyle\sin(\bar{\alpha}_{j}-\bar{\theta}_{j})\exp(\bar{\mathbf{n}}_{zj}(\pi-\bar{\alpha}_{j}))(\mathbf{p}_{a}-\mathbf{p}_{m})$
(13)
$\displaystyle+\sin(\bar{\theta_{j}})(\mathbf{p}_{m_{j}}-\mathbf{p}_{i}),$

where

  * •

$\mathbf{n}_{zj}=\frac{\mathbf{p}_{a}-\mathbf{p}_{m}}{\|\mathbf{p}_{a}-\mathbf{p}_{m}\|}\times(\mathbf{R}_{m_{j}}\mathbf{n}_{j})$
is the rotation axis from the vector
$\frac{\mathbf{p}_{a}-\mathbf{p}_{m}}{\|\mathbf{p}_{a}-\mathbf{p}_{m}\|}$ to
the vector $\mathbf{R}_{m_{j}}\mathbf{n}_{j}$.

  * •

$\alpha_{j}=\arccos(-\frac{\mathbf{p}_{a}-\mathbf{p}_{m}}{\|\mathbf{p}_{a}-\mathbf{p}_{m}\|}\cdot(\mathbf{R}_{m_{j}}\mathbf{n}_{j}))$
is the angle of rotation from vector
$-\frac{\mathbf{p}_{a}-\mathbf{p}_{m}}{\|\mathbf{p}_{a}-\mathbf{p}_{m}\|}$ to
the vector $\mathbf{R}_{m_{j}}\mathbf{n}_{j}$.

  * •

Both $\mathbf{n}_{zj}$ and $\alpha_{j}$ are locally observable.

  * •

$\|\mathbf{p}_{m}-\mathbf{p}_{a}\|\mathbf{R}_{m_{j}}\mathbf{n}_{j}\equiv\mathrm{Exp}(\mathbf{n}_{zj}(\pi-\alpha_{j}))(\mathbf{p}_{a}-\mathbf{p}_{m})$
is rotation of vector $(\mathbf{p}_{a}-\mathbf{p}_{m})$ about axis
$\mathbf{n}_{zj}$ by $\pi-\alpha_{j}$ angle.

Inspired by the translation averaging method in [20], we obtain a “position
only” convex cost function, after substituting $\bar{\mathbf{N}}_{j,i}$ into
the original formulation (10) optimizationm:

$\min_{\\{\mathbf{p}\\}}h(\mathbf{p},\bar{\mathbf{R}},\bar{\mathbf{F}}):=\min_{\\{\mathbf{p}\\}}\sum_{i\in\mathbb{T}_{j},j}\|\breve{\bar{\mathbf{N}}}_{j,i}-\bar{\mathbf{R}}_{i}\breve{\mathbf{f}}_{j,i}^{(l)}\|^{2}.\\\
$ (14)

###### Remark 6

Considering (14) is still a nonlinear problem, an initial guess for the
problem (14) is needed. Since $\bar{\mathbf{N}}_{j,i}$ is linear to positions,
we further simplify (14) to a linearly constrained Quadratic Programming
(QPLC) problem: to minimize the cross-product between ray $N_{j,i}$ and
$\mathbf{R}_{i}\breve{\mathbf{f}}_{j,i}$, with a linear constraint to ensure
local observation ray $\mathbf{R}_{i}{\mathbf{N}}_{j,i}$ lies in front of the
camera, as shown in Fig. 2 _(b)_ , i.e.,

$\min_{\\{\mathbf{p}\\}}\sum_{i\in\mathbb{T}_{j},j}\|S(\bar{\mathbf{R}}_{i}\breve{\mathbf{f}}_{j,i}^{(l)})\bar{\mathbf{N}}_{j,i}\|^{2},\quad
z(\bar{R}_{i}\bar{\mathbf{N}}_{j,i})>=0.$ (15)

###  IV-C Theoretical analysis

###### Theorem 2

With accurate initial estimate for orientation, the formulation (14) can
always converge to a near-optimal solution for both problem (10) and (14).
Furthermore, the problem (14) is convex when EG pairs are noise-free.

###### Proof:

See Appendix -B. ∎

Theorem 2 proves the correctness and robustness of the proposed initialization
in theory. Moreover, (14) is a pose-graph problem with much reduced size than
(10) and the expensive feature retraction operation is also not needed.

###### Remark 7

Here we do not claim the proposed global initialization is the best one but it
is very compatible to PMBA. Note that the proposed method is friendly to
robust methods such as pseudo Huber, $L^{1}$-norm or outlier detection
technique. Further, this convexified model is still formulated in a
probabilistic framework, different from the “Linear Global Translation
Estimation” reported in [12].

##  V Evaluation on PMBA performance

###  V-A Simulation

We demonstrate PMBA’s ability to handle problem features by running a simple
simulation test: 4 poses and 10 features. One of the problem features is a far
feature, another is a singular feature that would cause singularity in
original PBA, as shown in Fig. 6 _(a)_. We run 4 iterations for the BAs under
comparison: DL for PMBA and XYZ-BA; and LM for PBA. At each iteration, we
collect the Hessian’s condition number, and at the end report the error
between optimization results and ground truth. The results are listed in Table
III. PMBA has normal condition numbers and gave good optimized estimates, PBA
and XYZ-BA show consistently large condition numbers and high final state
error. This confirms our prediction that PMBA has well-behaved information
matrix during optimization.

|
---|---
|  (_a_) Simulation with
---
problem features
|  (_b_) Comparison of the error
---
from PMBA, PBA and XYZ-BA
Figure 6: Compare three BA forms in a simulated scene with problem features TABLE III: Comparison of $\mathbf{H}_{\mathbf{FF}}$’s condition number during optimization and final state error for PMBA, PBA and XYZ-BA Convergence Properties | PMBA | PBA | XYZ-BA
---|---|---|---
Iter-0 $cond(\mathbf{H}_{\mathbf{FF}})$ | 9.74 | 1.46E+4 | 1.22E+94
Iter-1 $cond(\mathbf{H}_{\mathbf{FF}})$ | 5.68 | 1.46E+4 | 1.22E+94
Iter-2 $cond(\mathbf{H}_{\mathbf{FF}})$ | 8.80 | 1.46E+4 | 1.22E+94
Iter-3 $cond(\mathbf{H}_{\mathbf{FF}})$ | 5.74 | 1.46E+4 | 3.53E+95
Final $\chi_{error}^{2}$ | 2.58E-3 | 5.37E-2 | 3.43E-2

###  V-B Large dataset test

We conducted a series of real datasets to compare performance of the proposed
PMBA (10) and original PBA, IDP and XYZ, aiming to address following
questions:

  * •

Robustness. With ill-conditioned scenario disappearing, can DL be safely used
in PMBA?

  * •

Efficiency. If DL were safely applied for PMBA formulation, how fast can the
optimization process be?

  * •

Accuracy. Since the PMBA formulation employs a different error function (9).
Is the global minimum accurate?

All methods are tested against 6 very challenging datasets, which are also
accessible from OpenSLAM111https://svn.openslam.org/data/svn/ParallaxBA/. In
particular,

  * •

Fake-pile is collected by the Google tango tablet in normal lab environment
[24] with a fake bridge pile in the middle, showing close and far features.

  * •

Malaga [25] is collected using an electric car equipped camera facing the
road, rich in collinear features.

  * •

Village and College are aerial photogrammetric datasets. The low feature to
observation ratio implies the existence of many small parallax features

  * •

Usyd-Mainquad-2 and Victoria-cottage are collected at University of Sydney
campus, show in Fig. 7.

|
---|---
(_a_) Images | (_b_) PMBA output pose and map
Figure 7: Usyd-mainquad dataset TABLE IV: Comparison of convergence performance for PMBA, PBA, XYZ-BA, IDP-BA Dataset | Test-type |  | # Pose
---
/ # Feat
/ # Obsv
| # Equation solving
---
/ # Iteration
Initial Chi2 | Final Chi2 | Time[sec]
Fake-pile | PMBA | 135 | 9 / 9 | 6.6E+6 | 1.7E+2 | 0.7
| PBA | /12,741 | 23 / 23 | 6.6E+6 | 1.7E+2 | 1.9
| IDP | /53,878 | 104 / 102 | 6.6E+6 | 1.7E+2 | 6.0
| XYZ |  | 116 / 108 | 6.6E+6 | 1.2E+3 | 4.7
Malaga | PMBA | 170 | 44 / 31 | 3.8E+7 | 9.1E+3 | 21.6
| PBA | /305,719 | 64 / 47 | 3.8E+7 | 9.1E+3 | 35.4
| IDP | /779,268 | 230 / 170 | 3.1E+7 | 5.8E+5 | 93.8
| XYZ |  | 110 / 85 | 3.8E+7 | 3.3E+5 | 39.0
Village | PMBA | 90 | 12 / 12 | 1.0E+10 | 3.3E+4 | 31.8
| PBA | /305,719 | 13 / 13 | 1.0E+10 | 3.3E+4 | 36.0
| IDP | /779,268 | 19 / 19 | 1.0E+10 | 3.3E+4 | 35.2
| XYZ |  | 18 / 18 | 1.0E+10 | 3.3E+4 | 26.3
College | PMBA | 468 | 33 / 33 | 3.0E+11 | 1.1E+6 | 334.4
| PBA | /1,236,502 | 31 / 31 | 3.0E+11 | 1.1E+6 | 370.5
| IDP | /3,107,524 | 34 / 34 | 3.0E+11 | 1.1E+6 | 255.3
| XYZ |  | 295 / 193 | 3.0E+11 | 1.0E+7 | 1361.0
Victoria | PMBA | 400 | 19 / 16 | 6.2E+8 | 1.1E+6 | 70.5
cottage | PBA | /153,632 | 88 / 66 | 6.2E+8 | 1.2E+6 | 301.4
| IDP | /890,057 | 49/48 | 6.2E+8 | 1.1E+6 | 157.9
| XYZ |  | 47 / 44 | 6.2E+8 | 1.2E+6 | 124.3
Usyd | PMBA | 424 | 25 / 25 | 2.4E+9 | 2.4E+6 | 214.5
-Mainquad | PBA | /227,615 | 101 / 57 | 2.4E+9 | 3.6E+6 | 642.6
| IDP | /1,607,082 | 301 / 191 | 2.2E+9 | 4.6E+6 | 1994.7
| XYZ |  | 76 / 58 | 2.4E+9 | 2.8E+6 | 423.7
|
---|---
(_a_) Malaga | (_b_) Usyd-Mainquad-2
|
(_c_) Victoria-cottage | (_d_) Fake-pile
Figure 8: Convergence plots for PMBA, PBA, IDP and XYZ

We use the initialization method from [1] to set all BAs from the same
starting point. We find that PBA, IDP and XYZ show unstable behaviour under
DL. PMBA, in comparison, has always worked well with DL. This can be explained
by our Theorem 1 that Hessian in PMBA does not exhibit singularity yet others
can, we therefore list DL results for PMBA and LM for other BA’s.

We use Ceres-solver as the optimization engine and test all BAs on an Intel-i7
with one thread. We use ray direction cost function for PMBA, and compute its
corresponding uv-based Chi2 error at each iteration step with current
estimate, to compare with other BAs on a common error metric. This scheme is
not fair for PMBA, yet is the only convincing way to evaluate performance
amongst all methods. Despite of this treatment, we found PMBA the best
performer in all tests, consistent to our expectation. Selected convergence
plots are shown in Fig. 8, other details are summarized in Table IV.

Further, in the Malaga dataset that contains numerous problematic features
(Fig. 1), we observe that the PMBA estimates and Ground Truth are very close,
yet conventional BA gives significant error. This is also seen in Table IV,
conventional BA’s converge to a local minimum, whereas both PBA and PMBA can
converge to their respective global minimums. Fig. 8 demonstrates the error
function (9) is practical, consistent with the claim in [26]. In conclusion,
these experiments all give positive answers to the raised questions.

###  V-C Evaluation of convexified initialization

In this subsection, we use selected datasets from the “Bundle Adjustment in
the Large” (BAL) datebase222http://grail.cs.washington.edu/projects/bal/ [27]
and the datasets in Section V-B to verify our initialization strategy. We
implement a SfM pipeline according to the procedure in Fig. 5. The QPLC stage
was implemented with matlab toolbox quadprog, the rest with Ceres in C++. We
present results for following datasets:

  * •

Ladybug-1370: 1370 images, captured at a regular rate using a Ladybug camera
mounted on a moving vehicle.

  * •

Trafalgar-126: 126 out-of-order internet images.

  * •

Venice-427: 427 out-of-order internet images.

  * •

College: 468 arial photogrammetric images.

All these datasets include either collinear or far features, exposing
challenges for conventional BA. Since camera calibration is beyond the scope
of this activity, we apply the reported optimal camera settings from BAL and
PBA websites and only test undistorted versions of these data. We stress that
our initial pose and feature values are purely generated from the Rotation
averaging and Translation averaging method described in Section IV, without
using the initial values provided by [27]. We are able to form good initial
values at QPLC stage, shown in Fig. 9 as 1st image in each row. And, our
convex pose-graph stage has a very large convergence region such that
imperfect outputs from the QPLC stage can gradually converge to a pose-graph
with a topology similar to that of Ground Truth. This is especially obvious in
“Ladybug-1370” and “Venice-427”. Moreover, in “Ladybug-1370”, BAL’s optimal
trajectory (in red) contains an erroneous camera pose, shown as red dot at top
right corner of the red trajectory, our method did not encounter this stray
pose at all.

|  |  |
---|---|---|---
|  |  |
|  |  |
|  |  |
Figure 9:  A demonstration of initialization to full PMBA pipeline: column 1
is QPLC results, column 2 shows selected iteration results in Convex
initialization, column 3 is a typical iteration result in full-PMBA and column
4 shows the final map. Row 1: BAL-Ladybug-1370; row 2: BAL-Venice-427, row 3:
BAL-Trafalgar-126, row 4: College aerial dataset

##  VI Conclusion

In this work, we proposed a new bundle adjustment formulation (PMBA) which
utilizes parallax angle based feature parametrization on manifold and
observation-ray based objective function. We proved that under the new
formulation the ill-conditioned cases due to problematic features can be
theoretically avoided without any manual intervention, which results in much
better convergence and robustness properties.

Furthermore, motivated by the strong local observability hidden in the visual
SLAM problem, we derived a novel global initialization process for PMBA. We
use a simplified model that can guarantee a near-optimal solution to bootstrap
the original problem. Experimental results show that the proposed
initialization can provide accurate estimates and is a viable global
initialization strategy for many challenging situations including sequential
and out-of-order images.

The promising results of the global initialization plus PMBA pipeline using
publicly available datasets demonstrate that the proposed technique can deal
with different challenging data. In the future, we are planning to integrate
the proposed pipeline with efficient visual SLAM front-end to develop a robust
and efficient SfM system.

###  -A The Proof of Theorem 1

Consider the feature $j$ and the corresponding sub-block matrix
$\mathbf{H}_{\mathbf{FF}_{j}}$ in
$\mathbf{H}_{\mathbf{FF}}=blkdiag(\mathbf{H}_{\mathbf{FF}_{1}},\cdots,\mathbf{H}_{\mathbf{FF}_{n}})$.
Denoting
$\mathbf{J}_{i,j}=\frac{\partial\mathbf{e}_{i,j}}{\partial\mathbf{F}_{j}}$ for
($i\in\mathbb{T}_{j}$), we have

$\displaystyle\mathbf{H}_{\mathbf{FF}_{j}}\geq\mathbf{J}_{m_{j},j}^{\intercal}\mathbf{J}_{m_{j},j}+\mathbf{J}_{a_{j},j}^{\intercal}\mathbf{J}_{a_{j},j}.$
(16)

On the one hand,

$\displaystyle\mathbf{J}_{m_{j},j}^{\intercal}\mathbf{J}_{m_{j},j}$
$\displaystyle=\begin{bmatrix}0&\mathbf{0}\\\
\mathbf{0}&(S(\mathbf{n}_{j})\mathbf{A}_{\mathbf{n}_{j}})^{\intercal}(S(\mathbf{n}_{j})\mathbf{A}_{\mathbf{n}_{j}})\end{bmatrix}$
(17) $\displaystyle=\begin{bmatrix}0&\mathbf{0}\\\
\mathbf{0}&\mathbf{I}_{2}\end{bmatrix}.$

Denoting $\mathbf{a}_{j}=\mathbf{p}_{m_{j}}-\mathbf{p}_{a_{j}}$ and
$\mathbf{n}_{jw}=\mathbf{R}_{m_{j}}\mathbf{n}_{f}$, we have

$\displaystyle\mathbf{N}_{j,m_{a}}=$
$\displaystyle\cos\theta_{j}\|\mathbf{a}_{j}\times\mathbf{n}_{jw}\|\mathbf{n}_{jw}$
(18)
$\displaystyle+\sin\theta_{j}(\mathbf{a}_{j}-(\mathbf{a}_{j}\cdot\mathbf{n}_{jw})\mathbf{n}_{jw}).$

Note that
$\mathbf{n}_{jw}\perp(\mathbf{a}_{j}-(\mathbf{a}_{j}\cdot\mathbf{n}_{jw})\mathbf{n}_{jw})$
and
$\|(\mathbf{a}_{j}-(\mathbf{a}_{j}\cdot\mathbf{n}_{jw})\mathbf{n}_{jw})\|=\|\mathbf{a}_{j}\times\mathbf{n}_{jw}\|:=\sin\gamma$,
thus we have

$\displaystyle\mathbf{J}_{a_{j},j}^{\intercal}\mathbf{J}_{a_{j},j}$
$\displaystyle=\begin{bmatrix}1&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}\end{bmatrix}.$ (19)

Therefore, $\mathbf{H}_{\mathbf{F}\mathbf{F}_{j}}\geq\mathbf{I}_{3}$ and
$\mathbf{H}_{\mathbf{FF}}\geq\mathbf{I}$.

###  -B The Proof of Theorem 2

Consider the following function

$h_{\mathbf{V}}(\mathbf{x}):=\|\breve{\mathbf{x}}-\mathbf{V}\|^{2},$ (20)

where $\mathbf{x}\in\mathbb{R}^{3}$, $\mathbf{V}\in\mathbb{R}^{3}$
($\|\mathbf{V}\|=1$). It is a fact that

$h_{\mathbf{V}}(\mathbf{x}+\lambda\Delta\mathbf{x})\leq\max\\{h_{\mathbf{V}}(\mathbf{x}),h_{\mathbf{V}}(\mathbf{x}+\Delta\mathbf{x})\\}$
(21)

for any $\Delta\mathbf{x}\in\mathbb{R}^{3}$ and any $\lambda\in(0,1)$.

Considering the problem (14) and the linearity of $\bar{\mathbf{N}}_{j,i}$
w.r.t. $\mathbf{p}$, (14) can be rewritten as

$\displaystyle\min_{\\{\mathbf{p}\\}}h(\mathbf{p},\bar{\mathbf{R}},\bar{\mathbf{F}})$
$\displaystyle=\min_{\\{\mathbf{p}\\}}\sum_{i\in\mathbb{T}_{j},j}\|\frac{\bar{\mathbf{A}}_{i}\mathbf{p}}{\|\bar{\mathbf{A}}_{i}\mathbf{p}\|}-\bar{\mathbf{V}}_{ij}\|^{2}.$
(22)
$\displaystyle=\min_{\\{\mathbf{p}\\}}\sum_{i\in\mathbb{T}_{j},j}h_{\mathbf{V}_{i}}(\bar{\mathbf{A}}_{i}\mathbf{p}),$

where $\bar{\mathbf{V}}_{ij}:=\bar{\mathbf{R}}_{i}\mathbf{v}_{ij}^{(l)}$ is a
directional vector.

Denoting the global minimum of the problem (14) as $\bar{\mathbf{p}}$, we have

$\displaystyle
h(\bar{\mathbf{p}}+\lambda\Delta\mathbf{p},\bar{\mathbf{R}},\bar{\mathbf{F}})$
(23) $\displaystyle=$
$\displaystyle\sum_{i\in\mathbb{T}_{j},j}h_{\mathbf{V}_{i}}(\bar{\mathbf{A}}_{i}(\bar{\mathbf{p}}+\lambda\Delta\mathbf{p}))$
$\displaystyle\small{(using(\ref{eq::lemma}))}\leq$
$\displaystyle\sum_{i\in\mathbb{T}_{j},j}\max\\{h_{\mathbf{V}_{i}}(\bar{\mathbf{A}}_{i}\bar{\mathbf{p}}),h_{\mathbf{V}_{i}}(\bar{\mathbf{A}}_{i}(\bar{\mathbf{p}}+\Delta\mathbf{p}))\\}$
$\displaystyle\leq$ $\displaystyle
h(\bar{\mathbf{p}},\bar{\mathbf{R}},\bar{\mathbf{F}})+h(\bar{\mathbf{p}}+\Delta\mathbf{p},\bar{\mathbf{R}},\bar{\mathbf{F}})$

for any $\Delta\mathbf{p}\in\mathbb{R}^{3M}$ and $\lambda\in(0,1)$. The
inequality above indicates a fact: if we perform optimization for the problem
(14), i.e., $\min_{\mathbf{p}}h(\mathbf{p},\bar{\mathbf{R}},\bar{\mathbf{F}})$
from any initial guess $\mathbf{p}\in\mathbb{R}^{3M}$, the converged value
$\mathbf{p}_{op}$ after optimization will be a near-optimal solution, i.e.,
$h(\mathbf{p},\bar{\mathbf{R}},\bar{\mathbf{F}})\leq
2h(\bar{\mathbf{p}},\bar{\mathbf{R}},\bar{\mathbf{F}})$. When
$\bar{\mathbf{R}}$ is close to the optimal estimate, $\mathbf{p}_{op}$ will be
also a near-optimal solution of the problem (10) clearly. Under noise-free
condition, $\mathbf{p}_{op}$ is an exact solution due to $0\leq
h(\mathbf{p},\bar{\mathbf{R}},\bar{\mathbf{F}})\leq
2h(\bar{\mathbf{p}},\bar{\mathbf{R}},\bar{\mathbf{F}})\leq 0$ thus the problem
(14) is convex.

## References

  * [1] L. Zhao, S. Huang, Y. Sun, L. Yan, and G. Dissanayake, “Parallaxba: bundle adjustment using parallax angle feature parametrization,” _The International Journal of Robotics Research_ , vol. 34, no. 4-5, pp. 493–516, 2015\.
  * [2] S. Agarwal, K. Mierle, and Others, “Ceres solver,” http://ceres-solver.org.
  * [3] R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard.
  * [4] F. Dellaert, “Factor graphs and gtsam: A hands-on introduction,” Georgia Institute of Technology, Tech. Rep., 2012.
  * [5] M. J. M. M. Mur-Artal, Raúl and J. D. Tardós, “ORB-SLAM: a versatile and accurate monocular SLAM system,” _IEEE Transactions on Robotics_ , vol. 31, no. 5, pp. 1147–1163.
  * [6] H. Zhang, K. Hasith, and H. Wang, “A hybrid feature parametrization for improving stereo-slam consistency,” in _2017 13th IEEE International Conference on Control Automation (ICCA)_ , 2017, pp. 1021–1026.
  * [7] L. Carlone, Z. Kira, C. Beall, and Others, “Eliminating conditionally independent sets in factor graphs: A unifying perspective based on smart factors,” in _2014 IEEE International Conference on Robotics and Automation (ICRA)_ , 2014, pp. 4290–4297.
  * [8] C. Lee and K. Yoon, “Exploiting feature confidence for forward motion estimation,” _CoRR_ , vol. abs/1704.07145, 2017.
  * [9] L. Zhao, S. Huang, L. Yan, and G. Dissanayake, “Parallax angle parametrization for monocular slam,” in _2011 IEEE International Conference on Robotics and Automation_ , 2011, pp. 3117–3124.
  * [10] C. Wu _et al._ , “Visualsfm: A visual structure from motion system.”
  * [11] N. Snavely, S. Seitz, and R. Szeliski, “Photo tourism: Exploring image collections in 3d,” in _ACM Transactions on Graphics (Proceedings of SIGGRAPH 2006)_ , 2006.
  * [12] Z. Cui, N. Jiang, and P. Tan, “Linear global translation estimation from feature tracks,” _CoRR_ , vol. abs/1503.01832, 2015.
  * [13] N. Jiang, Z. Cui, and P. Tan, “A global linear method for camera pose registration,” in _2013 IEEE International Conference on Computer Vision_ , 2013, pp. 481–488.
  * [14] C. Tang, O. Wang, and P. Tan, “Globalslam: Initialization-robust monocular visual slam,” _arXiv preprint arXiv:1708.04814_ , 2017.
  * [15] Z. Cui and P. Tan, “Global structure-from-motion by similarity averaging,” in _2015 IEEE International Conference on Computer Vision (ICCV)_ , 2015, pp. 864–872.
  * [16] S. T. Smith, “Trust-region methods on riemannian manifolds,” _Foundations of Computational Mathematics_ , no. 3, pp. 303–330, 2007.
  * [17] T. Zhang, K. Wu, J. Song, S. Huang, and G. Dissanayake, “Convergence and consistency analysis for a 3-d invariant-ekf slam,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 2, pp. 733–740, 2017.
  * [18] T. Zhang, K. Wu, D. Su, S. Huang, and G. Dissanayake, “An invariant-ekf VINS algorithm for improving consistency,” in _accepted by 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , Sep 2017.
  * [19] D. Crandall, A. Owens, N. Snavely, and D. Huttenlocher, “Discrete-continuous optimization for large-scale structure from motion,” in _CVPR 2011_ , 2011, pp. 3001–3008.
  * [20] K. Wilson and N. Snavely, _Robust Global Translations with 1DSfM_. Cham: Springer International Publishing, 2014, pp. 61–75.
  * [21] L. Kneip, R. Siegwart, and M. Pollefeys, _Finding the Exact Rotation between Two Images Independently of the Translation_. Berlin, Heidelberg: Springer Berlin Heidelberg, 2012, pp. 696–709.
  * [22] L. Carlone, R. Tron, K. Daniilidis, and F. Dellaert, “Initialization techniques for 3d slam: A survey on rotation estimation and its use in pose graph optimization,” in _2015 IEEE International Conference on Robotics and Automation (ICRA)_ , 2015, pp. 4597–4604.
  * [23] L. Kneip and P. Furgale, “Opengv: A unified and generalized approach to real-time calibrated geometric vision,” in _2014 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2014.
  * [24] L. Liu, Y. Wang, L. Zhao, and S. Huang, “Evaluation of different slam algorithms using google tangle data,” in _2017 IEEE Conference on Industrial Electronics and Applications_ , 2017.
  * [25] J.-L. Blanco, F.-A. Moreno, and J. Gonzalez, “A collection of outdoor robotic datasets with centimeter-accuracy ground truth,” _Autonomous Robots_ , vol. 27, no. 4, p. 327, Aug 2009.
  * [26] S. Im, H. Ha, F. Rameau, H.-G. Jeon, G. Choe, and I. S. Kweon.
  * [27] S. Agarwal, N. Snavely, and Others, “Bundle adjustment in the large,” http://grail.cs.washington.edu/projects/bal/.