JVLDLoc: a Joint Optimization of
Visual-LiDAR Constraints and Direction Priors for Localization in Driving Scenario

First we briefly review some of the major visual or LiDAR SLAM in literature. ORB-SLAM[4, 16] completely uses ORB features, realizing a full V-SLAM system including loop detection, relocalization, and map fusion. IMLS-SLAM[6] estimates pose by minimizing the distance from the current point to the surface represented by the implicit moving least square method (IMLS). Then we categorize visual-LiDAR SLAM into two major genres according to whether or not to use both multi-modal residuals in the optimization stage.

The common directions of parallel 3-D lines are known as vanishing points (VPs). Because the VPs offer direction information independent of the present robot proposition[3], Many academics have researched VPs to help improving orientation. [15] apply the orthogonal constraint between multiple VPs, known as the Manhattan world constraint, to calcute the full rotation. [13] on longer need Manhattan world assumption but they will extract all valid VPs from an image while our method indeed only use one VP. [21] also combine VP error in a pose estimator, but it have to generate VPs of three directions before refinement.

We process visual and LiDAR data and compute their features and decide whether to generate a new keyframe. Note that we need to transform point cloud to camera coordinate by given extrinsic: $T_{CL}$. If there is no initial pose from other odometry, we can still handle it by launching a LiDAR odometry. It is modified from the approach in IMLS-SLAM[6].The main changes compared to [6] can be found in supplemental material.

A new keyframe $\mathcal{K}_{n}$ will trigger a visual-LiDAR graph optimization.The corresponding joint energy function consists of visual reprojection error and point to IMLS surface error.

Our backend maintains a covisiblity graph where keyframes that observe same 3D point are connected. Denote keyframe launching local mapping as $\mathcal{K}_{n}$, sort all the keyframes connected to $\mathcal{K}_{n}$ at covisiblity graph (abbreviated as $\mathbf{K}_{n}^{cov}$) including itself by their timestamps: $\mathbf{K}_{local}=\left\{...\mathcal{K}_{i}...\mathcal{K}_{n}\right\}$. $\mathcal{P}_{local}$ refers to all 3D visual map points seen by $\mathbf{K}_{local}$. Defining $\mathcal{X}_{i}=\{(\textrm{x}^{j},\textbf{{X}}^{j})|j=0,1...\}$ as the set of correspondences between feature points at keyframe $\mathcal{K}_{i}$ and 3D visual map points from $\mathcal{P}_{local}$. Then do the following steps:

• 1)

  For all keyframes in $\mathbf{K}_{local}$, according to visual correspondences from $\mathcal{X}_{k}$, we can get visual reprojection error term like [16]: $\textbf{e}_{V}(i,j)=\|\textrm{x}^{j}_{(\cdot)}-\pi_{(\cdot)}(\textrm{R}_{iw}\textbf{{X}}^{j}+\textrm{t}_{iw})\|^{2}_{\sum}$.

• 2)

  For current keyframe $\mathcal{K}_{n}$ which has a global frame index $f_{n}$, we find the first keyframe whose frame index met $f_{n}-f_{i}\leq\textbf{N}$, where N is the size of local scan map of each keyframe. Denote this keyframe as $\mathcal{K}_{start}$. Now list all corresponding scan from $\mathcal{K}_{start}$ to $\mathcal{K}_{n-1}$ : $\left\{\mathcal{S}_{start},...,\mathcal{S}_{n-1}\right\}$. We use each keyframe’s current pose to transform these scans to world frame and accumulate to be a whole point cloud model named $\mathbf{S}_{model}$(See Fig. 3 (a)).

• 3)

  Sort every point in $\mathcal{S}_{n}$ by their computed nine feature values. Then transform $\mathcal{S}_{n}$ to world coordinate to find correspondences in its $\mathbf{S}_{model}$. we use the same scan sampling strategy as Section V in [6] to get sampled point cloud $\widetilde{\mathcal{S}}_{n}$. we compute for each point the distance to the IMLS surface in $\mathbf{S}_{model}$:

  $$I^{n}(\textbf{p}_{x})=\\ \frac{\sum_{\textbf{p}_{i}\in\mathbf{S}_{model}}W_{i}(\textbf{p}_{x})((\textbf{p}_{x}-\textbf{p}_{i})\cdot\vec{n_{c}})}{\sum_{\textbf{p}_{i}\in\mathbf{S}_{model}}W_{i}(\textbf{p}_{x})}$$

  (2)

  where $\textbf{p}_{x}\in\widetilde{\mathcal{S}}_{n}$ but in world coordinate, $\textbf{p}_{i}$ is the neighbor of $\textbf{p}_{x}$ from model scan and $\vec{n_{c}}$ is the normal of $\textbf{p}_{c}$ that is nearest neighbor of $\textbf{p}_{x}$. And $\textbf{p}_{c}$ is from $\mathcal{S}_{c}$ of $\mathcal{K}_{c}$. $W_{i}(\textbf{p}_{x})=e^{-\|\textbf{p}_{x}-\textbf{p}_{i}\|^{2}/h^{2}}$ is the same weight function as [6].Next we can obtain the projection of $\textbf{p}_{x}$ at IMLS surface, $\textbf{p}_{y}$: $\textbf{p}_{y}=\textbf{p}_{x}-I^{n}(\textbf{p}_{x})\vec{n_{c}}$.

• 4)

  Now we have found a association between $\mathcal{K}_{n}$ and $\mathcal{K}_{c}$. Denote ${}^{c}\textbf{p}_{y}=T_{cw}\textbf{p}_{y}$. Finally, we can get point to IMLS surface error:

  $$\textbf{e}_{L}(n,j)=\|(T_{cw}T_{nw}^{-1}\textbf{p}_{x}^{j}-^{c}\textbf{p}_{y}^{j})\cdot\vec{n_{c}}^{j}\|^{2}$$

  (3)

  where $\textbf{p}_{x}^{j}$ is the $j_{th}$ point from $\widetilde{\mathcal{S}}_{n}$.

  All the correspondences $(\textbf{p}_{x}^{j},^{c}\textbf{p}_{y}^{j},\vec{n_{c}}^{j})$ for each $\textbf{p}_{x}^{j}$ and keyframe index $n$ and $c$ in Eq. 3 will be saved in $\mathbf{L}=\{(n,c,\textbf{p}_{x}^{j},^{c}\textbf{p}_{y}^{j},\vec{n_{c}}^{j})\}$. The previous saved correspondences in $\mathbf{L}$ will be taken out to build more costraints within $\mathbf{K}_{local}$, see supplemental material for detail.

• 5)

  Our proposed local joint energy function is weighted sum of above two types of constraints:

  $$\begin{split}\textbf{E}_{local}=\sum_{i\in\mathbf{K}_{local}}\alpha\sum_{j\in\mathcal{X}_{i}}\rho(\textbf{e}_{V}(i,j))+\beta\sum_{j\in\widetilde{\mathcal{S}}_{n}}\textbf{e}_{L}(n,j)\end{split}$$

  (4)

  where $\rho$ is the robust Huber cost function, $\alpha$ and $\beta$ are weight hyper parameters to balance those two kinds of measurements. The poses of keyframes in $\mathbf{K}_{local}$ are obtained by minimizing Eq. 4.

After local mapping of the final keyframe, we will launch the global mapping.

To prove that JVLDLoc will reduce global drift, we visualize the output point cloud map with color in Fig. 5. Qualitively, our method greatly improves the global consistency of the map. For more mapping visualization, please turn to supplemental material. We use RMSE of APE[9] to quantify the drift.

Table 1 includes results test on KITTI odometry when fed with diferent prior map. We have set four baselines as input odometry including ORB-SLAM2[16], our LiDAR odometry under different N which is the number of scans kept as local map. The other two groups of result is shown in supplemental material. Under different quality prior maps, the average accuracy of these 11 sequences can be significantly improved. Especially when input LiDAR odometry with $N=1$, the mean APE is reduced by about $79\%$. Table 2 is for the other two datasets, which also prove our method’s effect. That is because JVLDLoc directly uses differnent kinds of geometric constraints to optimize pose.

To demonstrate the effects of direction priors itself, we further compare accuracy with and without using the proposed direction priors. APE are still shown in Table 1, Table 2. It can be seen the direction priors are able to further improve localization due to smaller average RMSE on KITTI. For other two datasets, with help of direction priors, maximum of APE strongly decrease on KITTI-360 from the level when there is none direction constraint; The minimum and RMSE have again significantly reduced by $80\%$ and $28\%$ on Oxford Radar RobotCar.

In a word, although the direction priors are not very accurate as they are based on input poses, they still contribute to better localization. We believe the reason is that direction priors impose longer term geometric constraints as the direction is an infinite landmark. Fig. 7 presents intuitive trajectory comparisons.Apparently, we can get less drift after long distance under direction priors.

Besides APE, Table 3 also calculates relative transition error and rotation error for 100, 200,… and 800 meter distances with KITTI’s official evaluation code.

Compared to other visual-LiDAR methods, JVLDLoc achieve best relative translation error and APE, which indicates the superiority of our method over others using the same modality of data. LIMO-PL[10] and DVL-SLAM[19] are loosely-coupled methods, which cause quite large translation error; TVL-SLAM[5] is a SOTA visual-LiDAR SLAM on KITTI, which also incorporates visual and lidar measurements in backend optimization. TVL-SLAM VL calib refers to their online camera-lidar extrinsics calibration version. Our JVLDLoc can outperform TVL-SLAM VL calib though using raw extrinsics. LIO-SAM[17] is representative method of Lidar-inertial SLAM. We run its open source code with given parameters ¹¹1https://github.com/TixiaoShan/LIO-SAM#other-notes. Due to the unknown intrinsics of the IMU, LIO-SAM failed on 00, 02, 08, but we can get better acuracy on three metrics for all left sequences.