ECTLO: Effective Continuous-time Odometry Using Range Image for LiDAR with Small FoV

A range image is an index table $\mathcal{I}:\mathbb{R}^{2}\rightarrow\mathbb{R}^{3}$ that reserves the spatial relationship of 3D point set within single 2D image. Given a point $(x,y,z)^{\top}$ relative to range image origin, its correspondence can be indexed in previous image by spherical projection $\Pi:\mathbb{R}^{3}\rightarrow\mathbb{R}^{2}$

where $r=\sqrt{x^{2}+y^{2}+z^{2}}$. The image center correspondence to the current LiDAR center. $w$ and $h$ are the width and height of image. The pixel length of $w$ and $h$ is proportional to angular resolution $\beta$ and FoV, where $w=\beta f_{h}$ and $h=\beta f_{v}$. Note that $f_{h}$ and $f_{v}$ are the user-defined FoV rather than the original LiDAR’s, which keeps the historical points out of the sensor’s FoV to facilitate robust registration.

The generation of initial range image $\mathcal{I}_{\textrm{map}}$ is just the process that project points onto an empty image. Fig. 1 shows the single scan projection results of different LiDARs. For Livox Mid-40, its single scan is too sparse to register, several consecutive scans at the static position are used for initialization. If the projection pixel is already occupied by another point, the collision is resolved by selecting the nearest points. In our proposed approach, we only maintain single range image $\mathcal{I}_{\textrm{map}}$. This means that the memory consumption of map is fixed when the FoV and angular resolution of the range image are determined.

Range Image $\mathcal{I}_{\textrm{map}}$ is a robocentric map representation, where the projection origin is the last successful registered LiDAR position. Once current scan can register with previous $\mathcal{I}_{\textrm{map}}$, we transfer map points to the local scan coordinate. Then, all the previous map points and current scan points are projected onto a new image. We choose the nearest points when pixels conflict. This new image $\mathcal{I}_{\textrm{map}}$ is the local map for future registration.

However, occlusion is inevitable when keeping historical map points in range image representation even with complicated conflict-resolving solutions. This phenomenon makes our $\mathcal{I}_{\textrm{map}}$ noisier after continuously updating. Since the odometry output of current scan is affected by the local map, it may lead to the inferior pose optimization results. We handle this issue by applying an efficient GMM-based registration method.

The fundamental problem of LiDAR odometry is to find a series of discrete motion parameters properly describing the LiDAR movement. Once the raw scan $\mathcal{P}$ is obtained, the transformation between the current scan $\mathcal{P}$ and previous map $\mathcal{Q}$ is estimated. $\mathbf{p}_{1},\mathbf{p}_{2},\cdots,\mathbf{p}_{M}$ and $\mathbf{q}_{1},\mathbf{q}_{2},\cdots,\mathbf{q}_{N}$ are points in $\mathcal{P}$ and $\mathcal{Q}$, respectively.

We make use of a scan-to-map method, where map $\mathcal{Q}$ stores in $\mathcal{I}_{\textrm{map}}$. With the continuous updating of the range map, it becomes quite noisy. Conventional ICP methods aim to find the exact closest point in the data association procedure, which is susceptible to noise data. To this end, we introduce an efficient GMM-based method FilterReg [18] into our pipeline. To achieve better accuracy in optimization, we adopt point-to-plane criteria. The original FilterReg estimates the current state $\mathbf{s}$ of LiDAR using the EM algorithm, as follows:

E step: for each point in scan $\mathcal{P}$, compute

M step: minimize the following objective function

where $c=\frac{w_{i}}{1-w_{i}}\frac{J}{M}$, and $w_{i}$ is the parameter that accounts for the ratio of outliers. $\mathcal{N}(\cdot)$ is Gaussian distribution. Gaussian kernel $\Sigma_{xyz}=\textup{diag}(\sigma^{2},\sigma^{2},\sigma^{2})$ is the fixed parameter, and the variance $\sigma$ decides the weight of each map points $\mathbf{q}_{j}$ and its normal vector $\mathbf{n}_{\mathbf{q}_{j}}$. $\mathbf{p}_{i}(\mathbf{s})$ transfers scan points from local coordinate into world frame, which may extend to continuous-time form. Practically, it is inefficient to employ all map points in E step. Thus, we select the neighborhood points of $\mathbf{p}_{i}(\mathbf{s})$ in a window for approximation, where $J$ is the size of valid neighborhoods. The implementation details are following.

Just like the closet point searching in ICP, the E step is to compute the correspondence of scan point $\mathbf{p}_{i}$. The form of $\mathbf{m}^{0}_{\mathbf{p}_{i}},\mathbf{m}^{1}_{\mathbf{p}_{i}},\mathbf{n}_{\mathbf{p}_{i}}$ are Gaussian Transform. Obviously, the bottleneck of this EM algorithm is how to efficiently compute thousands of the above independent items in E step. FilterReg [18] employs a customized permutohedral lattice filter to enable efficient computation without compromising accuracy. Since the point registration problem occurs in 3D space rather than the general N-dimensional space, we utilize a more efficient 2D Gaussian filter on the range image.

The advantage of range image is to retain the 3D spatial relationship within a 2D index table, where its neighbor exists in adjacent pixels. Therefore, the Gaussian Transform can be efficiently computed by image filter methods like Gaussian Blur or Bilateral Filter. If the spherical projection result of $\mathbf{p}_{i}$ is $(u_{i},v_{i})$, the related map points affecting the correspondence are in adjacent pixels. Hereby, Gaussian filter is employed to compute the filter-based correspondence $\mathbf{m}^{0}_{\mathbf{p}_{i}},\mathbf{m}^{1}_{\mathbf{p}_{i}},\mathbf{n}_{\mathbf{p}_{i}}$ in a pre-defined window $\mathcal{W}_{i}$ with center $(u_{i},v_{i})$.

During scan registration, map points $\mathcal{Q}$ are fixed so that their normal vectors do not change. By making use of point-to-plane criteria, we convert the previous map $\mathcal{I}_{\textrm{map}}$ into two temporal index tables, vertex map $\mathcal{I}_{\textrm{v}}$ storing 3D position information, and normal map $\mathcal{I}_{\textrm{n}}$ saving their normal vector. We estimate normals through Eigendecomposition within a $\mathcal{W}{n}$ window on the range image. To select valid planar points, we consider those surface curvatures [5] that fall below the threshold $\delta{\sigma}$. In cases where the angle between the normal vector and the point exceeds $90^{\circ}$, we flip the normal direction. It is important to note that we only need to calculate the normals once before the EM procedure.

A scan is the accumulated point cloud during a period of time $t\in\left[t_{b},t_{e}\right)$. $t_{b}$ is the starting timestamp of a scan, and $t_{e}$ is the end timestamp. Motion distortion occurs due to assuming that all points are sampled simultaneously. To address this issue, we employ a continuous-time model to compensate the motion distortions in filter registration.

The continuous movement of LiDAR within a scan is parameterized by the beginning of scan $\mathbf{T}_{\textrm{WL}_{b}}$ and end of scan $\mathbf{T}_{\textrm{WL}_{e}}$. Therefore, the estimation target is the state $\mathbf{s}=[\mathbf{T}_{\textrm{WL}_{b}},\mathbf{T}_{\textrm{WL}_{e}}]$, where $\mathbf{s}$ is in $\mathbb{R}^{12}$ through the minimal representation of Lie algebra. Given a raw point measurement $\mathbf{p}_{i}\in\mathbb{R}^{3}$ at timestamp $t_{i}\in\left[t_{b},t_{e}\right)$, its corrected position $\mathbf{p}_{i}(\mathbf{s})$ in world coordinate is computed by

where $\boldsymbol{\tau}\in\mathbb{R}^{6}$ is the tangent space of manifold $\mathrm{SE(3)}$, and $\mathbf{T}_{\mathrm{WL}_{i}}$ is the LiDAR origin at timestamp $t_{i}$ in world coordinate. Each 3D point $(x,y,z)^{\top}$ is represented in homogeneous coordinates as $\mathbf{p}_{i}=(x,y,z,1)^{\top}$. As the estimated state $\mathbf{s}$ is updated iteratively until the convergence, the point $\mathbf{p}_{i}(\mathbf{s})$ is recomputed with the newly updated pose at each iteration.

Ideally, the beginning pose of current scan should be consistent with the end pose of previous one. Thus, the new state is initialized by its previous pose as follows

where the first state $\mathbf{s}_{0}$ is set to identity. However, the optimization may diverge on the noisy data with strict consistency constraints. Therefore, we impose two soft constraints, the location consistency $E_{\textrm{loc}}(\mathbf{s})=\mathbf{r}_{\textup{loc}}^{\top}\mathbf{r}_{\textup{loc}}$ and velocity consistency $E_{\textrm{vel}}(\mathbf{s})=\mathbf{r}_{\textup{vel}}^{\top}\mathbf{r}_{\textup{vel}}$, where the position difference is defined as $\mathbf{r}_{\textup{loc}}=\mathbf{T}_{\mathrm{WL}_{b}^{n}}\ominus\mathbf{T}_{\mathrm{WL}_{e}^{n-1}}$ and velocity difference between the consecutive scans is computed by $\mathbf{r}_{\textup{vel}}=(\mathbf{T}_{\mathrm{WL}_{e}^{n}}\ominus\mathbf{T}_{\mathrm{WL}_{b}^{n}})-(\mathbf{T}_{\mathrm{WL}_{e}^{n-1}}\ominus\mathbf{T}_{\mathrm{WL}_{b}^{n-1}})$. Since handheld devices exhibit complex and irregular movement compared to driving scenarios, their scan data contain significant noise. Thus we apply the state constraint in the $\mathrm{SE}(3)$ space, rather than solely the translation component [14].

With the above proposed motion constraints, we reformulate the previous M step in Eqn. 4 into the continuous-time form,

where $\lambda_{l}$ and $\lambda_{v}$ are two regularization coefficients to balance the above the energy terms.

The optimization in the M step involves a least square minimization problem, which can be reformulated into the generalized form $\sum c_{i}\mathbf{r}{i}^{\top}\mathbf{r}{i}$. Here, $c_{i}$ represents the weight coefficient, and $\mathbf{r}_{i}$ denotes the residual of the energy term. In our proposed approach, there are three kinds of residual, including $\mathbf{r}_{\textup{reg}}^{i}=\mathbf{n}_{\mathbf{p}_{i}}^{\top}(\mathbf{p}_{i}(\mathbf{s})-\mathbf{m}^{1}_{\mathbf{p}_{i}}/\mathbf{m}^{0}_{\mathbf{p}_{i}})$, $\mathbf{r}_{\textup{loc}}$ and $\mathbf{r}_{\textup{vel}}$. By the first order Taylor expansion, the residual around $\mathbf{s}$ is approximated as follows

where $\frac{\partial\mathbf{r}_{i}}{\partial\mathbf{s}}$ is the Jacobian, and $\Delta\mathbf{s}=\begin{bmatrix}\Delta\boldsymbol{\xi}_{b}&\Delta\boldsymbol{\xi}_{e}\end{bmatrix}$ is the increment. In Gauss-Newton (GN) algorithm, the optimal increment minimizing $E_{\textrm{ct-reg}}(\mathbf{s})$ is found by solving the linear equation $\mathbf{H}\Delta\mathbf{s}=-\mathbf{b}$, where the Hessian matrix $\mathbf{H}=\sum c_{i}\left(\frac{\partial\mathbf{r}_{i}}{\partial\mathbf{s}}\right)^{\top}\frac{\partial\mathbf{r}_{i}}{\partial\mathbf{s}}$ and $\mathbf{b}=\sum c_{i}\left(\frac{\partial\mathbf{r}_{i}}{\partial\mathbf{s}}\right)^{\top}\mathbf{r}_{i}$.

Since $\mathbf{T}_{\mathrm{WL}_{b}}$ and $\mathbf{T}_{\mathrm{WL}_{e}}$ are represented in $\mathrm{SE(3)}$, the new state $\mathbf{s}$ for the next E step is updated through $\mathbf{s}=\mathbf{s}^{\textrm{old}}\oplus\Delta\mathbf{s}$. Specifically, the two poses are updated as follows

The EM procedure will terminate when either the maximum increment falls below the threshold $\delta_{m}$ or the number of iterations exceeds 15.

The computational cost in M step is mainly dominated in calculating the Jacobian. To this end, we derive the analytic Jacobians of each residual with respect to the beginning pose $\mathbf{T}_{\textrm{WL}_{b}}$ and the end pose $\mathbf{T}_{\textrm{WL}_{e}}$ for efficient optimization. The analytic Jacobian of registration term is computed by

As in [30], $\mathbf{J}_{r}(\cdot)$ and $\mathbf{J}_{l}(\cdot)$ are the right- and left- Jacobians, respectively. $\left[\cdot\right]_{\times}$ denotes the skew symmetric matrix.

Similarly, the analytic Jacobians of motion constraints can be derived as below

The testing data is adopted from the previous studies [9] with a Livox Mid-40. Since the dataset does not have ground truth for validation, we select two sequences ‘loop_hku_main’ (389m) and ‘loop_hku_zym’ (290m) whose beginning and end positions are the same.

Fig. 4 plots the trajectories of each valid method. It can be clearly seen that our proposed ECTLO approach achieves the lowest drift even without the extra loop closure module, which obtains the high reconstruction quality by taking advantage of the accurate continuous-time filter registration. Livox-mapping is the official odometry package from Livox, however, its mapping results have the obvious drifts. It is worthy of mentioning that BALM totally fails to obtain the correct results with this small FoV LiDAR.

To examine the gap between LiDAR-only odometry and sensor fusion methods, we compare our proposed LiDAR only approach against the state-of-the-art LiDAR-inertial odometry FAST-LIO2 [11] in a large-scale outdoor sequence ‘hkust_campus_01’ (1524m) from R3LIVE [31] collected by Livox Avia. The end-to-end drift of ECTLO is 4.72m in contrast to 0.31m drift of FAST-LIO2⁴⁴4https://github.com/hku-mars/FAST_LIO. LOAM_Livox cannot generalize to Livox Avia, and Livox_mapping has large drift. Moreover, we plot the mapping result in Fig. 5.

The Hilti SLAM Challenge Dataset [32] is a multi-sensor collection containing challenging featureless areas and varying illumination conditions. We evaluate different odometry by computing absolute trajectory error (ATE) [33] using Livox Mid-70 data. Meanwhile, SVO2 [34] and hdl_graph_slam [35] are two baselines of VIO and multi-line spinning LiDAR odometry, respectively. The Mid-70 Data in Lab sequence is invalid at the beginning where the sensor is too close to the wall. Thus, we select other five sequences with ground truth for evaluation.

Table II shows the ATE on Hilti SLAM Challenge dataset, where LOAM_Livox and BALM fail to track on all sequences. Sequence ‘RPG Area’ and ‘Basement1’ have the degenerated scenarios, where data is recorded in front of a textureless wall for a long time. It is challenging for both camera and LiDAR with small FoV. It can be observed that ECTLO outperforms SVO using the visual-inertial sensor and Livox_mapping using Livox Mid-70 at a large margin. Our result is even close to the method using 360^∘ scanning LiDAR Ouster OS0-64.

Although FAST-LIO2 using Livox Mid-70 and IMU has better performance at outdoor sequences, it drifts significantly in the indoor basement even with an extra sensor. This reflects that our robocentric image representation is a more appropriate structure for LiDAR with small FoV in narrow space. Overall, our ECTLO achieves the lowest average ATE on the Hilti Dataset with single small FoV LiDAR.