Robust Photogeometric Localization over Time for Map-Centric Loop Closure

TL;DR

This paper introduces a robust photogeometric localization method for map-centric SLAM that combines LiDAR and camera data, effectively validating loop closures and improving accuracy and robustness over traditional methods.

Contribution

It presents a tightly coupled photogeometric localization approach that enhances loop closure validation and outlier rejection in map-centric SLAM.

Findings

More accurate than conventional ICP methods

Robust to incorrect initial pose guesses

Effective outlier rejection using visual evidence

Abstract

Map-centric SLAM is emerging as an alternative of conventional graph-based SLAM for its accuracy and efficiency in long-term mapping problems. However, in map-centric SLAM, the process of loop closure differs from that of conventional SLAM and the result of incorrect loop closure is more destructive and is not reversible. In this paper, we present a tightly coupled photogeometric metric localization for the loop closure problem in map-centric SLAM. In particular, our method combines complementary constraints from LiDAR and camera sensors, and validates loop closure candidates with sequential observations. The proposed method provides a visual evidence-based outlier rejection where failures caused by either place recognition or localization outliers can be effectively removed. We demonstrate the proposed method is not only more accurate than the conventional global ICP methods but is also robust to incorrect initial pose guesses.

Tables

Table 1. TABLE I: Localization Accuracy Comparison.

Initial	(a)	(b)	(c)	(d)	(e)	(f)
Guess	Sparse ICP	Visual+ICP	SHOT+ICP	FPFH+ICP	PhotogeoSeq	PhotogeoSeq+
Easy	0.04(0.01)	0.05(0.01)	0.15(0.008)	0.30(0.03)	0.09(0.004)	0.05(0.003)
Medium	0.40(0.19)	0.12(0.11)	0.13(0.005)	1.64(0.30)	0.09(0.004)	0.04(0.004)
Hard	2.52(2.42)	1.49(0.71)	0.17(0.005)	13.8(0.10)	0.10(0.004)	0.04(0.004)
Time (sec)	0.17	0.63	1.05	3.25	0.75$\times i_n$	2.04$\times i_n$

Equations

$$\text{T}(\tau ):=\left[\begin{array}{cc}\text{R}{}_{\tau }& \text{t}{}_{\tau }\\ \text{0}& 1\end{array}\right].$$

$$\text{T}(\tau )=\text{T}{}_k\text{e}{}^{\alpha [{\mathbf{\xi }}_r]{}_{\times }}.$$

$$\begin{array}{r}{}^{L_r}{\text{T}}_{L_s}={}^w{\text{T}}_L({\tau }_r{)}^{-1}{}^w{\text{T}}_L({\tau }_s),\end{array}$$

$$\begin{array}{rl}{}^{L_r}{\bar{\text{T}}}_{L_s}={\mathbf{e}}^{[\mathbf{\xi }{]}_{\times }}{}^{L_r}{\text{T}}_{L_s}& :=\left[\begin{array}{cc}\text{R}& \text{t}\\ \text{0}& 1\end{array}\right],\end{array}$$

$$\begin{array}{rl}{}^{C_r}{\text{T}}_{C_s}& ={}^C{\text{T}}_L{}^{L_r}{\text{T}}_{L_s}{}^L{\text{T}}_C:=\left[\begin{array}{cc}{\text{R}}^{\prime }& {\text{t}}^{\prime }\\ \text{0}& 1\end{array}\right],\end{array}$$

$${}^{L_{r}}{\textbf{T}}_{L_{s}}^{i}={{}^{{\color[rgb]{0,0,0}{L}}_{r}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{r_{i}}}}{{}^{{\color[rgb]{0,0,0}{L}}_{r_{i}}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{s_{i}}}}{{}^{{\color[rgb]{0,0,0}{L}}_{s_{i}}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{s}}},$$

$${}^w{\text{T}}_C={}^w{\text{T}}_L(\tau +\delta \tau ){}^L{\text{T}}_C,$$

$${}^L{\text{T}}_C={\mathbf{e}}^{[{\mathbf{\xi }}_{cali}{]}_{\times }}{}^L{\bar{\text{T}}}_C,$$

$$\text{e}{}_I={\text{n}}^{\top }({\text{p}}_r-(\text{R}{\text{p}}_s+\text{t})),$$

$${\mathbf{r}}_I=\frac{1}{2}\sum _{v=1}^V\text{e}{{}_{I_v}}^{\top }{\mathbf{\Sigma }}_{I_v}^{-1}\text{e}{}_{I_v},$$

$$\text{e}{}_c={\text{u}}_s^{\top }[{\text{t}}^{\prime }{]}_{\times }{\text{R}}^{\prime }{\text{u}}_r.$$

$${\text{u}}_s^{\prime }=\mathcal{W}({\text{u}}_r)=\mathbf{\pi }(\text{K}({\text{R}}^{\prime }\text{p}({\text{u}}_r,{\mathcal{D}}_r)+{\text{ t}}^{\prime }),$$

$$\delta\textbf{u}^{*}=\underset{\delta\textbf{u}}{\mathrm{argmin}}{\sum_{\Delta\textbf{u}\in\mathcal{P}}{\Bigl{\|}\mathcal{I}_{s}(\textbf{u}_{s}^{\prime}+\delta\textbf{u}+\Delta\textbf{u})-\mathcal{I}(\Delta\textbf{u})}\Bigl{\|}^{2}},$$

$$\mathcal{I}(\Delta \text{u})={\mathcal{I}}_r({\mathcal{W}}^{-1}({\text{u}}_s^{\prime }+\Delta \text{u})),$$

$${\mathbf{r}}_P=\frac{1}{2}\sum _{j=1}^J{\text{e}}_{c_j}^{\top }{\mathbf{\Sigma }}_{c_j}^{-1}\text{e}{}_{c_j},$$

$$\begin{array}{rl}\mathbf{\Sigma }=& {\mathbf{J}}_s^{\top }{\sigma }_p{\mathbf{J}}_s+{\mathbf{J}}_r^{\top }{\sigma }_p{\mathbf{J}}_r+{\mathbf{J}}_{{\tau }_1}^{\top }{\sigma }_{\tau }{\mathbf{J}}_{\tau 1}\\ & +{\mathbf{J}}_{{\tau }_2}^{\top }{\sigma }_{\tau }{\mathbf{J}}_{{\tau }_2}+2{\mathbf{J}}_{{\mathbf{\xi }}_{cali}}^{\top }{\mathbf{\Sigma }}_{cali}{\mathbf{J}}_{{\mathbf{\xi }}_{cali}},\end{array}$$

$${\mathbf{J}}_r=\frac{\partial \text{e}}{\partial {\text{u}}_r},{\mathbf{J}}_s=\frac{\partial \text{e}}{\partial {\text{u}}_s},{\mathbf{J}}_{\tau }=\frac{\partial \text{e}}{\partial \tau },{\mathbf{J}}_{{\mathbf{\xi }}_{cali}}=\frac{\partial \text{e}}{\partial {\mathbf{\xi }}_{cali}}.$$

$$\begin{array}{rl}\text{A}& ={\mathfrak{I}}_c^{-T}{\mathbf{\Sigma }}_c^{-1}{\mathfrak{I}}_c^{-1}+{\mathfrak{I}}_k^{-T}{\mathbf{\Sigma }}_k^{-1}{\mathfrak{I}}_k^{-1},\\ \text{b}& ={\mathfrak{I}}_c^{-T}{\mathbf{\Sigma }}_c^{-1}{\mathbf{\xi }}_c+{\mathfrak{I}}_k^{-T}{\mathbf{\Sigma }}_k^{-1}{\mathbf{\xi }}_k,\\ \mathbf{\xi }& ={\text{A}}^{-1}\text{b},\end{array}$$

$${\text{T}}^{\ast }\leftarrow {\text{e}}^{[\mathbf{\xi }{]}_{\times }}{\text{T}}^{\ast },$$

$${\mathbf{\Sigma }}_c=({\mathfrak{I}}_c^{-T}{\mathbf{\Sigma }}_c^{-1}{\mathfrak{I}}_c^{-1}+{\mathfrak{I}}_k^{-T}{\mathbf{\Sigma }}_k^{-1}{\mathfrak{I}}_k^{-1}{)}^{-1}.$$

$$\sum _{l=1}^L\sum _{j=1}^{J_i}\text{e}{}_{jl}({\mathbf{\xi }}_{jl}{)}^{\top }{\mathbf{\Sigma }}_{jl}({\mathbf{\xi }}_{jl}{)}^{-1}\text{e}{}_{jl}({\mathbf{\xi }}_{jl})<{\chi }_{\delta ,0.95}^2,$$

Full text

I Introduction

Simultaneous Localization And Mapping (SLAM) is a key enabling component of autonomous robots and driverless technologies. Although SLAM techniques have reached a mature level, with the recent advent of low cost multi-modal sensors, 3D dense LiDAR-based SLAM is still in its infancy.

Map-centric approaches [1, 2], which have demonstrated their accuracy and effectiveness by fusion-based mapping and deformation-based loop closure, provide an alternative solution to the dominant trajectory-centric SLAM [3, 4].

However, in the map-centric approach, the effect of incorrect loop closure is more destructive and irreversible compared to the conventional graph-based methods. The map-centric approach fuses all the measurements on-the-fly to the map, where the fused map is difficult to be detached and corrected once the loop is closed. Therefore, the accuracy and robustness of loop closure in map-centric approaches [1, 2] become crucial.

Previous map-centric approaches either did not have a proper method to detect false positives [2] in a loop closure or they are not robust enough as well as being limited to only the RGB-D sensor model [1]. The current state-of-the-art solution in false positive loop closure detection or localization failure detection [5] requires a global pose graph optimization for each validity test which is a huge penalty when it comes to scalability issues. Furthermore, their method is also dedicated for trajectory-centric systems such as graph-based SLAM, and as a result, is not applicable to map-centric methods.

To overcome this problem, we propose a metric localization, namely PhotogeoSeq+, which combines geometric and photometric constraints over time for the robust metric localization of the map-centric loop closure. The specific contributions of this work can be outlined as follows. First, with the tightly coupled geometric and photometric constraints, our method robustly estimates 6 Degree of Freedom (DoF) alignment even when the initial guess for registration is completely incorrect or unknown.

Second, instead of making a loop closure decision based on only a single 6 DoF alignment estimation, the proposed method observes the stability of the alignment over time and provides a reliability metric to reject or accept a loop closure hypothesis without a global trajectory optimization. For this purpose, we demonstrate that utilizing the visual features are beneficial for detecting a localization failure.

This paper is organized as follows. Section II reviews related works. Section III describes the overall procedure of the proposed method. In Section IV, we describe the proposed photogeometric metric localization method (PhotogeoSeq+), and detail our visual evidence-based outlier rejection in Section V. Results demonstrating the advantages of our method over current state-of-the-art global ICP algorithms are given in Section VI. Finally we summarize and present future directions in Section VII.

II Related Work

SeqSLAM [6] proposed a place recognition architecture that utilizes the coherent sequential 2D visual information, where it searches whether a similar sequence of the features between the two paths are observed over time. Lynen et al. [7] extended SeqSLAM as a 2D probability density estimation problem in votes versus travel distance space. Their work finds chunks of revisited places more efficiently than SeqSLAM by the distance normalization. However, as their method purely depends on 2D visual input, it is not suitable for a 6 DoF pose estimation problem. Furthermore, if the trajectory is only overlapped a short distance, such as at intersections, it is not likely to have much coherency which increases the risk of failure [8].

Latif et al. [9] proposed a graph-based loop closure detection and correction system, where they utilize a residual of the graph optimization as a prior of a loop closure failure. Their method clusters topologically related loop closure hypotheses over time and rejects any hypothesis that increases the residual of graph optimization. They have proved that the localization could be reversely utilized for testing whether the input place recognition is false positive, whereas the conventional algorithms showed single directional hierarchical flow from place recognition to localization. Although, they were able to increase robustness to false positive loop closures, their method requires a graph-optimization to test each hypothesis, which is a huge drawback for a long-term mapping system.

On the other hand, the modality of the loop closure system is one of the key components that increases the performance and accuracy of a loop closure. In early works, a single sensor was utilized for the loop closure detection problem, where 3D or visual descriptors were solely used [8, 9]. However, with the single sensor approach the robustness is rather limited because of the innate degeneracies in each configuration. Thus, even the state-of-the-art works have a higher false positive rate than desired and work well only within limited scenarios.

There are few works that introduce multi-modality for the place recognition or localization problem. Early works in this category [10, 11, 12], combined 3D and 2D descriptors for a place recognition problem. While the combined complementary descriptors are capable of handling degenerate configurations such as where there are no visual patterns or where the scene is geometrically flat, rejection of incorrect place recognition or alignment error is difficult. Furthermore, the constraints from 2D recognition are not tightly integrated in the pose optimization of the metric localization step [1, 10]. Also, when it comes to a separate sensing system such as an independent LiDAR and camera system on a dynamically moving platform [3], the quality of the localization is affected by the spatio-temporal differences in observations, which was not properly considered in the previous works [10]. Even in the cases where the integrated intensity from LiDAR is utilized instead of 2D visual features, difficulties still exist because of the LiDAR intensity calibration and intensity differences according to the incident angle [13].

III Overview of the system

The proposed metric localization method (PhotogeoSeq+) consists of place recognition, metric localization, and pose fusion modules as depicted in Fig. 1. Given a motion-undistorted local point cloud from LiDAR and corresponding camera images by the sliding window-based local trajectory optimization [2] as shown Fig. 2, the place recognition module finds a possible revisit of a place and triggers the localization step to estimate an alignment between the current location and the reference location. Once the localization module is triggered, our method continuously estimates the alignment between the new current location and its corresponding reference location at different places until the uncertainty of the estimated alignment reaches a certain threshold.

IV Alignment Estimation

In this section, we elaborate on our alignment estimation method by photogeometric constraints.

IV-A Continuous-Time Trajectory Representation

In the proposed method, the synchronization problem of the multi-modal sensors such as different frame-rates or unsynchronized clocks are required to be properly addressed. To cope with this problem, our method proposes to use the continuous-time trajectory representation [2]. In the continuous-time trajectory representation, the trajectory is modeled as a function of time. An exact system pose $\textbf{T}(\tau)\in{SE(3)}$ at an arbitrary query time $\tau\in{\mathbb{R}}$ can be interpolated from a set of temporally nearby poses. Let $\textbf{T}(\tau)$ be composed of translational component $\textbf{t}{{}_{\tau}}\in{\mathbb{R}^{3}}$ and rotational component $\textbf{R}{{}_{\tau}}\in{SO(3)}$ as,

[TABLE]

Then, utilizing the linear continuous-time trajectory representation, its value can be evaluated by an interpolation from two poses $\textbf{T}{{}_{k}},\textbf{T}{{}_{k+1}}$ where their timestamps satisfy $\tau_{k}<\tau<{\tau}_{{k+1}}$. Given poses and timestamps, their interpolation at $\tau$ is given by,

[TABLE]

where the relative pose $[\boldsymbol{\xi}_{r}]_{\times}\in\mathfrak{s}\mathfrak{e}(3)$ is defined by $log(\textbf{T}{{}_{{k}}^{-1}}\textbf{T}{{}_{{k+1}}})$ and the exponential mapping $\textbf{e}{{}^{\alpha[\boldsymbol{\xi}]{{}_{\times}}}}$ linearly interpolates the relative pose on the manifold with the interpolation ratio $\alpha=({\tau-{\tau}_{{k}}})/({\tau_{k+1}-{\tau}_{{k}}})$. $[\cdot]_{\times}$ represents skew-symmetric matrix conversion of a vector.

IV-B Alignment Between Two Places

Given a continuous-time trajectory and two matching places found by a loop closure detection module as depicted in Fig. 2 (b), we define an initial source to reference transformation from the drifted raw trajectory as,

[TABLE]

where ${}^{w}{\textbf{T}_{L}}(\tau)$ is a continuous-time trajectory [3] that represents the LiDAR frame $[L]$ in the world frame $[w]$ at source and reference time $\tau_{s}$, $\tau_{r}$ as shown in Fig. 3. Due to drifts in trajectory, the initial alignment ${{}^{L_{r}}\textbf{T}_{L_{s}}}$ is highly likely to be misaligned. Thus, we define and find its correction by,

[TABLE]

where $[{\boldsymbol{\xi}}]_{\times}\in{\mathfrak{se}(3)}$ is the compensation to the initial alignment which can be found by visual and geometrical sensor observations. Additionally, for the joint optimization by the LiDAR and visual observations, we define a corresponding relative camera transformation as,

[TABLE]

where ${{}^{L}\textbf{T}_{C}}$ is a pre-calibrated camera frame [C] to LiDAR frame extrinsics.

IV-C Single Alignment Estimation from Place Pairs

As we already mentioned, an alignment estimation based on a single location has a high probability of failure. We propose to aggregate the estimations of multiple alignments at different locations to the very first place where the loop closure was first detected. This approach will develop a more robust and reliable failure detection and outlier rejection.

Equation (4) represents the displacement between two matched local frames. As our objective is to estimate a single alignment, we transform the local displacements into the first place as,

[TABLE]

where {{}^{{\color[rgb]{0,0,0}{L}}_{r}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{r_{i}}}}, {{}^{{\color[rgb]{0,0,0}{L}}_{s}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{s_{i}}}} are the relative transformation from the first frame and $i_{th}$ frame which are extracted from the drifted trajectory as described in Fig. 3. It is important to note that we assume that the trajectory utilized to extract {{}^{{\color[rgb]{0,0,0}{L}}_{r}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{r_{i}}}}, {{}^{{\color[rgb]{0,0,0}{L}}_{s_{i}}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{s}}} is locally rigid and accurate enough, which implies that {{}^{{\color[rgb]{0,0,0}{L}}_{r}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{r_{i}}}}, {{}^{{\color[rgb]{0,0,0}{L}}_{s_{i}}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{s}}} do not embed any uncertainty because of the local trajectory drift within small regions.

IV-D Spatio-Temporal Uncertainty in Visual Measurements

To account for uncertainties from various sources during the optimization, we define the uncertainty model of the system. Two types of uncertainties are defined. The first uncertainty is from the temporal difference between the two sensing modalities. As we estimate the location of the camera based on the LiDAR trajectory, the temporal uncertainty affects the camera location estimation as,

[TABLE]

where $\delta\tau\sim\ \mathcal{N}(\mu_{\sigma},\sigma_{\tau}^{2})$ is the estimated temporal difference with mean $\mu_{\sigma}$ and variance $\sigma_{\tau}^{2}$ [14]. Even with a small temporal uncertainty, a large local motion amplifies the uncertainty of the camera pose.

The second uncertainty originates from the extrinsic calibration between the two sensors,

[TABLE]

where ${\boldsymbol{\xi}_{cali}}\sim\ \mathcal{N}(\textbf{0},\boldsymbol{\Sigma}_{cali})$ is an extrinsic calibration error and ${{}^{L}\bar{\textbf{T}}_{C}}$ is the true extrinsics. The visualization of these uncertainties are shown in Fig. 4. The uncertainties modelled here will be propagated through the cost function of visual constraints and used as weights in non-linear least squares in the next section as well as a statistic test for localization outlier rejection.

IV-E Geometric Constraints

We apply surfel-based point-to-plane ICP with multi-resolutional voxelization[15] for the geometrical alignment between the source and the reference point cloud as,

[TABLE]

[TABLE]

where the pair of the matched surfel centroids $\textbf{p}_{r}$, $\textbf{p}_{s}$ are acquired by a nearest neighbour method in the surface normal and centroid space. The original point cloud to calculate these surfels are extracted around the pose ${}^{w}\textbf{T}_{L}(\tau)$ at $\tau_{s}$, $\tau_{r}$ and represented in the local LiDAR coordinates. The covariance is defined by a M-estimator weight and the Eigenvalue of the voxel [2].

IV-F Photometric Constraints

The sparse surfel constraint is computationally efficient because of the lower number of surfels after a spatial compression but this often fails when the initial guess on the pose is incorrect or the scene is geometrically degenerate such as in a corridor or wide open area. To cope with both problems, we tightly combine Photometric constraints with ICP. If there are distinctive visual patterns in the scene, the visual patterns generate complementary constraints which are not effected by the structure of the scene.

Our proposed method combines two different types of visual features: semi-direct features [16] and indirect features [17], as a complementary constraint to the geometric constraint. Both visual features are fed into the epipolar constraints between two camera poses. Thus, given a set of 2D features $\textbf{u}_{s}$, $\textbf{u}_{r}$ the epipolar constraint is given as,

[TABLE]

The visual constraint by (11) can estimate the pose up to scale only. However, combined with the ICP constraint, full 6 DoF can be estimated as well as preventing the optimization from being stuck at local minima because of incorrect surfel matching caused by an incorrect initial guess on alignment.

The indirect 2D features between the source image and the reference image are found by a global feature descriptor method [17] to cope with the large view difference problem. As the feature detection and matching stage are independent of the time lag and initial guess on the true alignment, even a few matches impose strong orientational constraints. This will guide the pose estimation to the roughly correct direction so that ICP can find the proper matchings of surfels. Therefore, Equation (11) largely reduces the chance of getting stuck in local minima because of surfel false matchings.

Also, to estimate more features widely spread over the image, the semi-direct features are matched by projecting the reference image feature points onto the source and refining as depicted in Fig. 5. It starts by defining the warping function $\mathcal{W}(\textbf{u}_{r})$ as,

[TABLE]

where $\textbf{u}_{r}$, $\textbf{u}_{s}^{\prime}\in R^{2}$ are the feature location on the reference image and its projection on the source image, K is the intrinsic matrix and $\mathcal{D}_{r}$ is the depth map of the scene, and $\pi$ is the camera projection. $\textbf{p}(\textbf{u}_{r},\mathcal{D}_{r})\in\mathbb{R}^{3}$ is the reconstructed 3D positions of the 2D features. The depth map $\mathcal{D}_{r}$ is generated from a dense surfel map by the LiDAR point cloud [2] by rendering at ${{}^{w}\textbf{T}_{L}}(\tau){{}^{L}\textbf{T}_{C}}$ where $\tau$ follows the reference image time stamp.

As the depth is loosely coupled to the image, the 2D geometry of the projected reference patch is not accurately aligned to the source patch in general as shown in the Fig. 6. Thus, for each image patch a refinement $\delta\textbf{u}^{*}$ is found so that it reduces the intensity difference of two patches as described in Fig. 5 and Fig. 6,

[TABLE]

where $\mathcal{I}_{s}$, $\mathcal{I}$ are the source and reference images defined at $\textbf{u}_{s}^{\prime}$ respectively, $\mathcal{P}$ is the image patch around $\textbf{u}_{s}^{\prime}$. The warped reference image $\mathcal{I}$ is defined as,

[TABLE]

where $\mathcal{W}^{-1}(\textbf{u}_{s}^{\prime})$ is the inverse projection from the source to the reference. Thus, the additional matched points are given from the reference feature location and the corrected source location as $\textbf{u}_{r},\textbf{u}_{s}=\textbf{u}_{s}^{\prime}+\delta\textbf{u}$. Note that the semi-direct method does not directly relate the intensity to the pose estimation problem. The semi-direct method is utilized to find more matched points that are uniformly distributed over the image. This semi-direct method is inspired by SVO [16] but we added frequency domain refinement to cope with considerably large $\delta\textbf{u}$ [18].

The semi-direct and indirect features are also complementary to each other. The semi-direct method finds features only from the reference side and warps them to the source side. Although more features are found evenly over the entire image area, it is not functional until the given pose is close enough to the true pose. However, the matching of indirect features are given regardless of the current pose, therefore, they are valid over all pose estimation stages. Furthermore, the indirect features are usually detected at a far distance where the shape of features are less distorted, thus it is easy to become a rank deficient constraint. Finally, the cost function of the two visual constraints are defined as,

[TABLE]

where ${e}_{c_{j}}$ is the error of $j_{th}$ feature pair and the covariances $\boldsymbol{\Sigma}_{c_{j}}\in\mathbb{R}$ are utilized to consider spatio-temporal uncertainty caused by heterogeneous sensory system which is defined as,

[TABLE]

where $\mathbf{J}_{s}$, $\mathbf{J}_{r}$, $\mathbf{J}_{\tau_{1,2}}$, $\mathbf{J}_{\boldsymbol{\xi}_{cali}}$ are the Jacobian of the cost function with respect to the uncertainty parameters on matching corruption, $\tau$ and $\boldsymbol{\xi}_{cali}$. They propagate the uncertainty to the optimization parameter space and are defined as follows,

[TABLE]

The amount of the pixel level uncertainty $\sigma_{p}$ is calculated from a predefined ground truth dataset by back projecting source inlier features to the destination. The spatiotemporal uncertainties $\sigma_{\tau}$, $\boldsymbol{\Sigma}_{cali}$ are approximated by the Hessian during the on-the-fly extrinsic, temporal calibration [14] by using a continuous-time bundle adjustment.

IV-G Joint Optimization

Given a photogeometric cost function $\mathbf{F}(\boldsymbol{\xi})=\alpha\mathbf{r}_{I}+\beta\mathbf{r}_{P}$, we utilize the Gauss-Newton method to find the compensation $\boldsymbol{\xi}$ and update {{}^{{\color[rgb]{0,0,0}{L}}_{r_{i}}}\textbf{T}_{{\color[rgb]{0,0,0}{L}}_{s_{i}}}} iteratively. Since the cost function is a multi objective problem, a proper handling of scale difference is required [19]. The scaling factors $\alpha,\beta$ are found by normalization. Also, for soft outlier rejection, we applied an M-estimator with t-distribution based weights.

V Fusing Alignment Estimations

Generally, single alignment estimation is prone to error due to various inevitable reasons. In our proposed system, to ensure robust localization, we propose to use sequential alignment estimation through different parts of the map instead of taking a single place recognition result for a loop closure. In this section we describe our probabilistic and temporal approach on fusing estimated poses. Also, an effective way to detect a localization failure or false positive will be discussed as well.

V-A Pose Fusion Model

Suppose that we have multiple alignment estimations and their covariances. As the estimated poses ${}^{L_{r}}{\textbf{T}}_{L_{s}}^{i}$ are the alignments between the first source and reference frame, the estimated alignment should not change over time under the assumption that they are locally rigid. Thus, we model this problem as estimating the same pose over time by (6).

The Bayesian fusion offers a closed form solution on the vector fusion problem [2]. However, as the pose vector sits on a manifold, directly applying the Bayesian fusion on the poses causes the convergence to be sub-optimal. Thus, we introduce the $SE(3)$ pose fusion approach proposed in [20] with a modification to convert the original batch fusion problem to a sequential fusion problem suitable for real-time application.

Let $\textbf{T}_{k},\boldsymbol{\Sigma}_{k},\textbf{T}_{c},\boldsymbol{\Sigma}_{c}$, respectively be $k_{th}$ pose measurement and the current pose estimation up to the point with their uncertainties. The uncertainty of the current estimation is approximated by the Hessian as $\boldsymbol{\Sigma}_{k}=\sigma^{2}(\textbf{J}^{\top}\textbf{J})^{-1}$. Applying the Gauss-Newton method with the two poses, we have,

[TABLE]

where $\boldsymbol{\mathfrak{I}}^{-1}$ is the Baker-Cambell-Hausdorff approximation [20] which is calculated from $\textbf{T}_{k},\textbf{T}_{c}$. Refer to [20] for a detailed derivation of the above. The next current pose estimation is found by iteratively updating the best guess from the current poses as,

[TABLE]

where the pose $\textbf{T}^{*}$ becomes the new $\textbf{T}_{c}$ at the end of the iteration for the next fusion. The covariance of the next current pose estimation is decided as follows at the end of the iteration,

[TABLE]

Note that the accuracy of the sequential fusion is identical with the batch fusion method but sequential fusion reduces unnecessary redundant fusions. To decide whether or not to collect more evidence for the loop closure, we inspect the eigenvalues of the current pose estimation covariance and stop estimating the alignment when eigenvalues are sufficiently small such that $\sum\lambda(\boldsymbol{\Sigma}_{c})_{i}<\theta_{th}$ for $i=[1,...,6]$, where $\theta_{th}$ is a sufficiently conservative threshold and $\lambda(\boldsymbol{\Sigma}_{c})_{i}$ $i_{th}$ eigenvalue of $\boldsymbol{\Sigma}_{c}$.

V-B Outlier Rejection

The basic assumption for the pose fusion is that the uncertainty of the estimated pose is consistent and well represents the true error. A pose with a large error is fine to be fused as long as its covariance value is high. In such a case, the incorrect pose merely changes the fused pose when the system has the strong evidence that the previous pose is correct. But in general it is not true because of blindness of ICP oriented covariance with respect to translation and possibility of degeneracy in 2D scenes. Thus, pose outliers where covariance does not properly represent its estimation quality should be filtered prior to the fusion.

Under the local rigidity assumption, any newly estimated alignment should not be increasing the residual of previously constructed constraints. The outlier rejection stage utilizes this rigidity assumption to detect and properly handle any outliers before fusing them to our final alignment estimation.

For this stage, only the epipolar constraint is utilized as its residuals proportionally increases when the input pose is incorrect. Note that the surfel-based geometric feature matching does not follow this property. The matched surfels are found in normal and centroid spaces, therefore, what it implies is an abstracted spatial alignment rather than feature level correspondence. Thus, given a new pose to be tested, a statistic test using the epipolar constraint is carried out as follows,

[TABLE]

where $\textbf{e}{{}_{jl}}$ is residual of $j_{th}$ epipolar constraint of $l_{th}$ pose estimation with respect to the pose $\boldsymbol{\xi}_{jl}$ and $\delta$ is degree of freedom. The pose $\boldsymbol{\xi}_{jl}$ should be reversely calculated from the given pose estimation using the relationship between the first pose and $l_{th}$ pose as in (6). This test checks if the residuals from each previous pose estimations are within 95% $\chi^{2}$ confidence area considering uncertainties caused by the time lag and extrinsic calibration error.

VI Experiment

In this section, we demonstrate the accuracy and robustness of the proposed method by comparing it to ground truth and other 3D point cloud based global localization methods.

VI-A Implementation Details

For the experiments, a hand-held 3D spinning LiDAR is utilized to collect LiDAR and color images as shown in Fig. 2 (a). The device consists of a spinning Hokuyo UTM-30LX laser, an encoder, a Microstrain 3DM-GX3 IMU, and a Grasshopper3 2.8 MP color camera[2].

For an initial seeding of the loop closure, a bag-of-word based place voting method is utilized. Once an initial pair of images are given, point clouds around the images are extracted for the ICP as shown Fig. 2 (b). To prevent the point cloud in the reference place and the current place to be mixed, a temporal selection is utilized with $\pm$5 seconds range. Also, for the efficiency in processing the point cloud, a 10$m$ spatial selection is made. Additional matched place pairs are added by either the place recognition module or selecting the nearest image frame with a similar angle when an inlier pose exists. For the multi-resolutional voxels, voxels size with 0.3$m$, 0.8$m$, 1.5$m$ are utilized.

To reduce computational complexity, sub-sampled images with size 960$\times$540 are utilized with 21$\times$21 patch sizes for the semi-direct features. Considering intensity changes caused by largely different views and time differences, the intensities of the patches are compared after a normalization. For both the global and projected features, outliers are initially filtered using a geometrical constraint with the Essential matrix. Although, Essential matrix based filtering works reasonably well, it fails to detect outliers when 2D features are degenerate such as in co-planer cases. Thus, upon detection of an inlier 6 DoF localization that minimizes both the geometrical and visual difference within the expected uncertainty, all of the past 2D matches, that are used as evidence in (21), are double checked and filtered by a $\chi^{2}$ test. The localization estimation is terminated early when the uncertainty is sufficiently small.

VI-B Experimental Setup

As we mentioned earlier the initial guess on the true alignment is the dominant factor that often leads the pose optimization to failure. It is fair to assume the initial guess is unknown because of possible drift to a large extent. This is rather similar to the global point cloud registration problem. Thus, we compare our method with the state-of-the-art global ICP registration libraries with different initial guesses.

For the evaluation of the localization performance, we have collected an indoor and outdoor mixed point cloud dataset that includes multiple loops, a continuous-time trajectory and color images. The collected dataset spans 60$\times$15$m^{2}$ with 331$m$ in continuous-time trajectory length and 377 seconds recording time. To demonstrate the algorithm under different challenging situations, we generated 10 datasets from the map shown in Fig. 9 by making multiple traverses and picking a different place recognition starting position. The datasets include challenging scenes where the loop closure sequence starts at geometrically degenerate scenes. The ground truth of the alignment is acquired from the globally optimized trajectory. To fairly estimate the robustness against the initial guess, initial poses are randomly generated within a certain amount of uncertainty (easy, medium and hard in the Table I) and then a 6 DoF alignment is estimated 50 times each at different loop closure location.

VI-C Localization

The Root Mean Square Error (RMSE) between the ground truth and the estimated alignments is listed in Table I. Sparse surfel-based point-to-plain ICP is utilized for the sparse ICP (a) which was most vulnerable to incorrect initial guess because of incorrect surfel matchings. The method that combines ICP with visual features (b) showed improved rotation estimation ability but initial guesses with a large error (medium and hard in Table I) caused occasional failures.

For the comparison with the state-of-the-art global ICP registration libraries, PCL (c) and Open3D (d) are utilized. For both methods, the initial alignments are achieved by 3D feature matchings such as using Signature of Histograms of OrienTations (SHOT) [21] (c) or Fast Point Feature Histograms (FPFH) [22] (d) and refined with dense point cloud in multi-resolution levels, which makes the alignment estimation less affected by the initial guess. These approaches are similar to method (b) except that multi-modal complementary constraints are jointly optimized in our case.

For both methods (c), (d) the results give reasonably accurate estimations for the places with many geometrical features and enough overlaps. However, in case of (d) the accuracy drops quickly and the estimation diverges especially when the localization occurs in geometrically degenerate scenes or the overlap is not sufficient which causes the failure in estimating a proper initial guess. On the other hand, the result in (c) shows a constant level of error over different initial guesses which was due to the robustness in the SHOT-based initial pose guess.

Our proposed method PhotogeoSeq (e) combines the method Visual+ICP (b) with the proposed sequential fusion and pose outlier rejection. Its robustness against the initial guess outperforms the state-of-the-art algorithms. However, further improvement was achieved in the translation estimation as given in the proposed PhotogeoSeq+ (f) by adding semi-direct features to (e). The improvement was due to the increased number of the widely spread visual features.

Our proposed sequential methods (e), (f) utilizes wider area for the localization than the methods (a)-(d). However, rather than utilizing the wide area at a single alignment estimation, the proposed method (e), (f) segments the map into frame level and runs independent registrations. This provides a way of conducting graph-SLAM like loop closure to the map-centric SLAM where the map is continuous and non-rigid rather than discrete like graph-based SLAM.

Fig. 7 displays how the continuous map is spatio-temporally segmented into the frame level for the sequential localization.

The processing time is given at the last row of Table I in seconds. Note that as the localization occurs over different places, sufficient time will be given for processing each localization. Note that the alignment estimation process in the proposed methods (e), (f) will be executed at each $i_{th}$ locations, which is why the processing time is multiplied by the number of matched places $i_{n}$.

Considering the actual local trajectories are drifting over time, the length of the sequential fusion $i_{n}$ should be limited. Otherwise, the transformed alignments which are found far from the initial loop closure location will be different from the alignment found on the initial location. The maximum length of $i_{n}$ depends on the accuracy of the trajectory estimation and can be found by propagating the unit uncertainty according to the travel time.

VI-D Outlier Handling

The pose estimation uncertainty and the error of the fused pose in the sequential fusion is depicted in Fig. 9 along with the visual evidence test statistics where the color in the $x$ axis label represents the status of the current fusion (black: need more evidence, blue: inlier pose estimation, red: outlier pose estimation).

The uncertainty and the actual error monotonically decrease whenever a new valid pose estimation is added. Occasional localization failures or false positive place recognitions (numbers in red) are filtered by the localization outlier rejection method. Once the uncertainty reaches to a threshold, the result of the localization is utilized for a map deformation as a loop closure constraint [2]. An example of the deformation graph and the comparison of the registration is depicted in Fig. 9.

When the uncertainty of each translation or orientation is high, the pose estimation is not reflected to the fused pose. However, as discussed in Section V.B the measured uncertainty could be incorrect for various reasons. The adverse effect on the rotation estimation such as in the fusion sequence 2, 3 can be caused by incorrect uncertainty.

Detected inlier features are added to the visual evidence pool and increased number of evidences are used for each sequence in the alignment validity test. The inlier rate is calculated by (inlier evidence)$/$(total evidence) where any pose estimations with a large error such as the fusion sequence 6 drastically drop the inlier rate.

VII Conclusion

In this paper, we proposed a robust metric localization method (PhotogeoSeq+) that tightly couples spatio-temporal and visual information from a multi-modal sensory set to improve the robustness and accuracy of a loop closure for continuous-time map-centric SLAM. Based on our experiments, we demonstrated the proposed method is superior to the state-of-the-art global ICP methods in terms of accuracy and failure detection. This is especially the case when the loop closure is detected for scenes that lack geometric features, as the proposed metric localization method has four times more accurate translation estimation compared to state-of-the-art. Furthermore, with the proposed sequential approach where it observes if the estimated alignment is constant over different places, we were able to reject both localization or place recognition failures and achieve robust localization regardless of the initial guess. Our approach is especially beneficial with the on-the-fly loop closure scheme of the map-centric approach where faultless metric localization is required. Finally, comparison with the state-of-the-art loop closure failure detection method reveals the benefits of our proposed method as it does not require a global trajectory optimization. In our future work, handling local non-rigidity and advanced map deformation that counts multiple alignments will be covered.

Acknowledgments

The authors gratefully acknowledge funding of the project by the CSIRO and QUT. Special thanks goes to Renaud Dube, Pavel Vechersky for the implementation.