Geometric Structure Aided Visual Inertial Localization

Visual Localization is a crucial building block for autonomous navigation, as it provides essential information for other functionalities [1]. In recent years, significant progress has been made for metric visual localization that aims to recover 6-DoF camera poses. Based on the map representation, the prevalent pipelines can be categorized into visual structure- and geometric structure-based methods.

Among them, localizing based on the visual structure is the most prevailing paradigm[2, 3, 4, 5]. Generally, the visual structure is represented by a bipartite graph between landmarks and keyframes, which can be built from Structure-from-Motion (SfM) [6] or Simultaneous Localization and Mapping (SLAM) [7]. Global and local features along with geometric information (e.g., camera poses, landmark positions) are also preserved. During the localization phase, it retrieves the candidate visual frames in the map and search for correspondences via feature matching. Camera pose can then be recovered in a Perspective-n-Point (PnP) scheme. Localization based on the visual structure can resolve a kidnapped localization problem, and the map storage is typically lightweight. However, it suffers from the camera viewpoint and visual appearance variances [5].

Recently, geometric structure-based methods raise increasing concerns [8, 9, 10, 11, 12, 13, 14]. This track of methods quantize the prior map with dense structure components (e.g., pointcloud, voxels, surfels). Generally, they do not focus on a kidnapped problem, and instead regard the visual localization problem as a registration problem between local visual structure and global dense map. The intuition behind is that, compared to visual information, structural information can be more invariant against view-point or illumination change. However, in these systems, how to bootstrap the localization without a relatively accurate initial guess has not been well considered [8]. In addition, how to efficiently introduce geometric information from dense structure is also an open problem to explore [14].

In this work, we propose a visual-inertial localization system with a hybrid map representation, which takes the advantage of visual and geometric structure and temporal visual information. Based on this representation, we can easily resolve a kidnapped localization problem without dealing with how to initialize camera poses cross different modalities. In addition, with the aid of prior geometric structure, we could efficiently generate temporal landmarks, and this process does not require exhaustive inter-frame matching in traditional vision pipelines [6, 7], which would be more time-consuming. The landmark positions can then be recovered and further optimized with geometric constraints. Another interesting issue is the trade-off between accuracy and time cost. Unlike a typical SLAM system, optimizing the states related to historical frames is less meaningful for a localization problem, while with the instant localization we are able to achieve a relatively accurate state estimation result. In this way, how to minimize the computational overhead in batch optimization is also an interesting question to think over. Therefore, in this work, we propose a windowed optimization scheme with linearized term as prior, which improves the efficiency with a comparative localization accuracy compared to other methods. The experimental results on a public dataset show that our method can provide an average accuracy around 1.7 cm in a challenging indoor environment. We summarize our contributions as follows:

• •

  We present a visual-inertial localization system based on a hybrid map representation, which takes the advantage from both visual structure and geometric structure.

• •

  We propose a map-feature association module for temporal landmark generation, the computational cost of which is reduced and bounded.

• •

  We propose an efficient batch optimization scheme with camera poses constrained by the prior terms from the instant localization.

• •

  Experimental results show that our system achieves a competitive localization performance while balances the computational cost.

The proposed visual-inertial localization framework is presented in Fig. 1. In the following sections, we briefly introduce different modules along with some preliminaries.

A solution of associating local features with map components is proposed in GMMLoc [14], named as projection method here. We briefly review this association scheme. GMMLoc first projects all the map components to the image coordinate as 2D distributions by applying non-linear transforms on 3D gaussian distributions. The local features are then associated with map components in the 2D space. To filter out occluded components, it sorts the projected components by depth, and compare the depth of each component and its neighborhoods. The drawbacks of the projection method can be three-fold: 1) when applied to generic camera models, the projection function needs to be linearized, yielding a bad approximation when the camera distortion is large; 2) the computational time is highly dependent on the number of visible components; 3) sorting operation is sequential thus is difficult to be parallelized.

To resolve these issues, in this work, we propose an improved solution that is more invariant to camera model and presents reduced and bounded computational cost. This method is inspired by techniques widely used in volumetric dense mapping, such as ray-casting, thus we name it ray-casting method here. An comparative illustration of these two methods is shown in Fig. 2.

As a prerequisite, we extend the GMM into a voxelized representation in the mapping stage. To this end, we first adopt the voxel-hashing method in Voxblox [19] to divide the Euclidean space into a set of voxels $\left\{V_{t}\right\}$, with a given resolution $\delta_{v}=0.1m$. Each voxel $V_{t}$ represents a region $R_{V_{t}}=\left\{\mathbf{x}|\mathbf{x}\in\mathbb{R}^{3},l<\mathbf{c}^{T}\mathbf{x}<u\right\}$, defined by a box function with parameters $l,u,\mathbf{c}$. Then for each Gaussian component, we locate their intersected voxels via the cumulative likelihood:

$$p(V_{t}|\mathcal{G}_{j})=\iiint_{\mathcal{R}_{V_{t}}}\exp(-\frac{1}{2}\|\mathbf{p}-\boldsymbol{\mu}_{j}\|_{\boldsymbol{\Sigma}})\,d\mathbf{p}.$$

(4)

We adopt a numerical method [20] to compute this integral as there is no analytical solution. If $p(V_{t}|\mathcal{G}_{j})>\delta_{l}=0.1$, the component $\mathcal{G}_{j}$ is considered intersected with the voxel $V_{t}$, and therefore in our system a component can be associated with multiple intersected voxels.

With this voxelized map representation, we then associate the unmatched local features with components in the map. Given a localized frame, with camera pose $[\mathbf{R}_{WC_{k}},{{}^{W}\mathbf{t}_{WC_{k}}}]$ and the local features $\left\{\mathbf{u}_{ki}\right\}_{i=1\dots N}$, for each candidate feature $\mathbf{u}_{ki}$, we can define a ray parameterized by $\mathbf{r}_{ki}=\left[{}^{W}\mathbf{v}_{ki},{{}^{W}\mathbf{t}_{WC_{k}}}\right]$, with the bearing vector under world coordinate ${}^{W}\mathbf{v}_{ki}$, which is given by: ${{}^{W}\mathbf{v}_{ki}}=\mathbf{R}_{WC_{k}}\pi^{-1}(\mathbf{u}_{ki})$, and ${{}^{W}\mathbf{t}_{WC_{k}}}$ the origin of the ray. Then we can parameterize the position of landmark observed by $\mathbf{u}_{ki}$ as ${}^{W}\mathbf{p}_{i}={{}^{W}\mathbf{t}_{WC_{k}}}+\lambda{{}^{W}\mathbf{v}_{ki}}$, where $\lambda$ is the depth along the ray. To find out proper map association for $\mathbf{u}_{ki}$, we cast the ray into the voxelized map. For each voxel along the ray, we examine the components intersected with it and the metric is defined by Mahalanobious distance between a line and a component ${dist}\left(\mathbf{r}_{ki},\mathcal{G}_{j}\right)$, which is explained as follows: decomposing the covariance of $\mathcal{G}_{j}$ using SVD, we have:

$$\boldsymbol{\Sigma}_{j}=\mathbf{R}_{j}\mathbf{S}_{j}\mathbf{R}_{j}^{T},\mathbf{S}_{j}=\text{diag}(\lambda_{1},\lambda_{2},\lambda_{3}),\mathbf{R}_{j}=[\mathbf{e}_{j1},\mathbf{e}_{j2},\mathbf{e}_{j3}].$$

Applying the whitening transform, the ray parameters become [21]:

	$\displaystyle{\mathbf{t}^{\prime}_{WC_{i}}}$	$\displaystyle=\mathbf{S}_{j}^{-\frac{1}{2}}\mathbf{R}_{j}^{T}({{}^{W}\mathbf{t}_{WC_{i}}}-{{}^{W}\boldsymbol{\mu}_{j}}),$		(5)
	$\displaystyle{\mathbf{v}^{\prime}_{ki}}$	$\displaystyle=\mathbf{S}_{j}^{-\frac{1}{2}}\mathbf{R}_{j}^{T}\cdot{{}^{W}\mathbf{v}_{kj}},$

with which the distance metric is given by:

$\displaystyle{dist}\left(\mathbf{r}_{ki},\mathcal{G}_{j}\right)$

$\displaystyle=\frac{1}{\|\mathbf{v}_{ki}^{\prime}\|}\|\mathbf{t}_{WC_{i}}^{\prime}\times\mathbf{v}_{ki}^{\prime}\|.$

(6)

For enhancing the efficiency, $\mathbf{S}_{j}^{-\frac{1}{2}}\mathbf{R}_{j}^{T}$ in Eq. 5 can be pre-computed for each component $\mathcal{G}_{j}$ by the Cholesky decomposition: $\boldsymbol{\Sigma}_{j}^{-1}=\mathbf{LL}^{T}=(\mathbf{S}_{j}^{-1/2}\mathbf{R}_{j}^{T})^{T}(\mathbf{S}_{j}^{-1/2}\mathbf{R}_{j}^{T})$.

With this metric, for each component associated with the same voxel, we adopt $\mathcal{X}^{2}$-test for the component that has the closest distance to the optical ray, which gives a threshold $\delta_{r}=2.8$. And if the distance is within $\delta_{r}$, we associate the component with the feature $\mathbf{u}_{ki}$. Finally, with the association, the depth $\lambda$ in the landmark position parameterization can be trivially recovered by: $\lambda={\mathbf{e}_{1}^{T}(\boldsymbol{\mu}-\mathbf{t}_{WC})}/{\mathbf{e}_{1}^{T}\mathbf{v}}$. We only perform this operation for local features that are not associated with landmarks in the map. We also sort them by detection score and only keep the top-100 keypoints to bound the computational cost. These strategies are kept the same for comparing the projection method and the ray-casting method in Sec. V. Unlike the projection method, time complexity of the ray-casting method is mainly dependent on number of local features for querying, hence is more bounded. ‘’ We name these initialized points as seeds, which are not immediately used for localization. Instead we further verified them in the Landmark Update step, with subsequent visual information along with localized camera poses. Similar to the matching scheme in Sec. II-D, we match the correspondences with the localized camera pose as prior. Briefly, if a seed is observed with sufficient parallax over multiple frames, it will be activated as a landmark for the future localization. In the implementation, the minimum parallax is set to $4$-px and the minimum number of observation is set to $4$. Seeds not visible in the current window will be cleared after each update.

Although we can already recover the global camera pose with the instant localization, some of the states are not able to be properly estimated, especially when the inertial measurements are incorporated. For example, we find that in practice, $\mathbf{b}^{a}$ is difficult to converge without batch optimization, yielding a biased state prediction. Here we proposed a batched yet efficient optimization structure, which can be solved within $10$ms in average.

Firstly, inspired by SVO [22], we decoupled the full Bundle Adjustment (BA) into motion- and structure-only BA, and Fig. 3 shows the factor graph representation of our motion-only BA pipeline. For the motion-only BA, we can optimize the camera pose via minimizing

$$E=\sum_{k\in\mathcal{F}}E_{\text{inertial}}(\mathbf{x}_{k},\mathbf{x}_{k-1})+\sum_{k\in\mathcal{F}}\sum_{i\in\mathcal{V}_{k}}E_{\text{proj}}(\boldsymbol{\xi}_{k},{{}^{W}\mathbf{p}_{i}}),$$

(7)

where $\mathcal{F}$ is the keyframes in the current window, and we set $\left|\mathcal{F}\right|=15$. Although the decoupled scheme accelerate the optimization, we found that iteratively re-linearizing the visual factors can be less effective in computation, while the the instant localization can already provide a relatively accurate estimation. As a consequence, we propose to marginalize the visual factors as a pose prior instead of using the original visual factors. After the pose optimization described in Sec. II-D, we have the normal equation: $\mathbf{H}\delta\hat{\mathbf{x}}_{k}=\mathbf{b}$. By reordering the states, this normal equation can be re-written as:

$$\left[\begin{matrix}\mathbf{H}_{tt}&\mathbf{H}_{tr}\\ \mathbf{H}_{rt}&\mathbf{H}_{rr}\\ \end{matrix}\right]\left[\begin{matrix}\delta\hat{\boldsymbol{\xi}}_{k}\\ \delta\hat{\mathbf{x}}_{r}\\ \end{matrix}\right]=\left[\begin{matrix}\mathbf{b}_{t}\\ \mathbf{b}_{r}\\ \end{matrix}\right],$$

(8)

where $t$ denotes the block of related to the camera pose update $\delta\boldsymbol{\xi}_{k}$ and $r$ the block of other variables. The hessian block $\mathbf{H}_{xx}$ can be decomposed by $\mathbf{H}_{xx}=\mathbf{H}_{i}+\mathbf{H}_{p}$, where $\mathbf{H}_{i}$ and $\mathbf{H}_{p}$ are the hessians related to inertial and visual factors, respectively. We have $\mathbf{H}_{p}=\sum_{i\in\mathcal{V}_{k}}\mathbf{H}_{pi}$ and $\mathbf{H}_{pi}$ is given by:

$$E_{\text{proj}}(\boldsymbol{\xi}_{k}\boxplus{\delta\boldsymbol{\xi}_{k}},{{}^{W}\mathbf{p}_{i}})\approx\mathbf{J}_{pi}^{T}\boldsymbol{\Sigma}_{pi}^{-1}\delta\boldsymbol{\xi}+\delta\boldsymbol{\xi}^{T}\underbrace{\mathbf{J}_{pi}^{T}\boldsymbol{\Sigma}_{pi}^{-1}\mathbf{J}_{pi}}_{\mathbf{H}_{pi}}\delta\boldsymbol{\xi},$$

where $\mathbf{J}_{pi}$ is the jacobian of $\mathbf{e}_{\text{proj}}(\boldsymbol{\xi}_{k},{{}^{W}\mathbf{p}_{i}})$ with respect to $\boldsymbol{\xi}_{k}$. Marginalizing out the visual factors simply forms a distribution on the camera pose update $\delta\boldsymbol{\xi}_{k}\sim\mathcal{N}(\delta\hat{\boldsymbol{\xi}}_{k},\boldsymbol{\Sigma}_{\delta\boldsymbol{\xi}_{k}}),\boldsymbol{\Sigma}_{\delta\boldsymbol{\xi}_{k}}=\mathbf{H}_{p}^{-1}$.

With this prior information, we can formulate the prior pose constraints as:

	$\displaystyle\mathbf{e}_{\text{rot}}$	$\displaystyle=\log(\hat{\mathbf{R}}_{WB_{k}}\mathbf{R}_{B_{k}W}\exp(\delta\hat{\boldsymbol{\phi}}^{\land}))^{\lor},$		(9)
	$\displaystyle\mathbf{e}_{\text{trans}}$	$\displaystyle=({{}^{B_{k}}\mathbf{t}_{B_{k}W}}-{{}^{B_{k}}\hat{\mathbf{t}}_{B_{k}W}})-\delta\hat{\mathbf{t}}.$

Assuming the pose estimation reaches the local optimal, we have the update of state $\delta\hat{\boldsymbol{\xi}}_{k}\rightarrow\mathbf{0}$, thus the prior error state can be formulated as

$$\mathbf{e}_{\text{pose}}(\boldsymbol{\xi}_{k})=[\log(\hat{\mathbf{R}}_{WB_{k}}\mathbf{R}_{B_{k}W})^{\lor},{{}^{B_{k}}\mathbf{t}_{B_{k}W}}-{{}^{B_{k}}\hat{\mathbf{t}}_{B_{k}W}}].$$

Then the total objective function Eq. 7 is changed into:

$$E=\sum_{k\in\mathcal{F}}E_{\text{inertial}}(\mathbf{x}_{k},\mathbf{x}_{k-1})+\sum_{k\in\mathcal{F}}\rho(\|\mathbf{e}_{\text{pose}}\left(\boldsymbol{\xi}_{k}\right)\|_{\boldsymbol{\Sigma}_{\delta\boldsymbol{\xi}_{k}}}),$$

This problem can be solved by the Levenberg-Marquarelt method. For the structure-only optimization, similar to GMMLoc [14], we re-triangulate the temporal landmarks with geometric constraints.

The proposed optimization structure is quite similar to Sliding Window Filter (SWF) commonly used in batched state estimation. A little bit different is that we fix the states relevant to the oldest frame, instead of introducing a prior term from marginalization, same as in [17]. As in the localization system, the global observations are available for most of the time, we believe such information loss is acceptable. Therefore, we choose this strategy that is much easier for implementation.

In this work, we have presented a visual inertial localization system based on a hybrid map representation. To increase the localization robustness, we have proposed an association method to efficiently create temporal landmarks. In the windowed optimization, we have proposed a strategy that reduce the computational time of full BA from about $50$ms to under $10$ms. The experimental results on EuRoC MAV dataset show that how our method can provide a $1-3$ cm localization accuracy with less computational resources in indoor scenarios. In the future, we would like to further improve the robustness and accuracy at the system level. Although our optimization module is already very efficient, we believe that solving this problem incrementally would be more suitable in practice, as the measurements typically comes in order. In addition, as the current feature extraction module is relatively tiaccording to the ame-consuming, distillation from the current model can be another interesting problem to deal with.