# Point-Line LIVO Using Patch-Based Gradient Optimization for Degenerate Scenes

Tong Shi $^{ID}$, Kun Qian $^{ID}$, Member, IEEE, Yixin Fang $^{ID}$, Yun Zhang $^{ID}$, and Hai Yu $^{ID}$

Abstract—Simultaneous localization and mapping based on 3-D light detection and ranging (LiDAR) tends to degenerate in structural-less environments, leading to a distinct reduction in localization accuracy and mapping precision. This article proposes a point-line LiDAR-visual-inertial odometry (PL-LIVO) based on the system implementation of FAST-LIVO for robust localization in LiDAR-degenerate scenes. The key idea is integrating both points and lines into the proposed direct visual odometry subsystem (PL-DVO). By minimizing the patch-based gradient residuals for state optimization, PL-DVO provides additional constraints complementary to LiDAR. Furthermore, a LiDAR map assisted visual features depth extraction (LM-VDE) method is proposed to recover 3-D positions of visual features by mapping them onto the 3-D planes of the LiDAR map. This method is independent of the single scan's density and notable for superior generalization across various LiDAR sensors. Extensive experiments on both public datasets and our datasets demonstrate that PL-LIVO ensures robust localization and outperforms other state-of-the-art systems in LiDAR degenerate scenes.

Index Terms—SLAM, localization, mapping, lidar-inertial-visual odometry, patch-based gradient optimization.

### I. INTRODUCTION

SIMULTANEOUS localization and mapping (SLAM) stands as a crucial technique for real-time localization and 3-D reconstruction in unknown environments. 3-D light detection and ranging (LiDAR) based SLAM [^1], [^2], [^3] systems have been widely employed in automatic robots, due to their ability to provide accurate state estimation and dense 3-D map. However, for some real-world applications such as inspection robots working on construction sites [^4] and automatic vehicles driving through tunnels [^5], LiDAR-based SLAM tends to degenerate in unconstrained directions (e.g., the direction of the tunnel extension). Consequently, the significant drop in localization

![Image](images/d4eb67289a3b59e64759b2105959f71550c6d891d09c496ed493992ef756a150.jpg)

LiDAR-degenerate environments in staircase motion trajectory of the IMU frame

<div style="text-align: center;">Fig. 1. localization and mapping result of PL-LIVO under the stairs_02 sequence. In this sequence, there was a significant exposure change when the operator entered indoors from outside. Moreover, challenging degeneration occurred when the operator was moving upstairs and downstairs, attributed to the limited FoV of the LiDAR scan.</div>

accuracy disqualifies the system from practical application. This problem becomes more serious when the LiDAR has a small field of view (FoV) [^6].

To improve the system performance in challenging scenes, many studies [^7], [^8] fuse the additional Inertial Measurement Unit (IMU) measurements which provide high-frequency motion data. Furthermore, by integrating visual point features of the image, several LiDAR-inertial-visual odometry (LIVO) systems [^9], [^10], [^11] demonstrate their robust pose estimation capabilities in LiDAR-degenerate scenes. However, there are still challenges for current LIVO systems to handle degenerate environments: (1) the visual subsystems of previous methods rely heavily on the brightness invariance assumption, which is prone to be disrupted by significant exposure change and camera lens flare; (2) abundant line features in man-made environments can provide effective constraints, but remain underutilized; (3) most existing LiDAR-assisted visual feature depth extraction methods require the high density of LiDAR scans. These methods adapt poorly to relatively sparse scans from multi-line spinning LiDAR sensors.

To overcome the aforementioned limitations, we propose PL-LIVO, a point-line LIVO modified from FAST-LIVO [^11], using patch-based gradient optimization for degenerate scenes. The key idea is integrating points and lines in the direct visual odometry subsystem to enhance pose estimation. Specifically,

---

a novel patch-based gradient residual is conducted to overcome the frequent exposure change, and a novel LiDAR-assisted depth extraction method for visual features is proposed to improve the generalization across various LiDAR sensors. The detailed contributions are listed below:

1) We propose a Direct Visual Odometry subsystem integrating both Points and Lines (PL-DVO) by minimizing the patch-based gradient residual. This residual holds higher adaptability to illumination changes based on the gradient invariance assumption. A rectangular patch pattern aligned with line direction is designed for line features, enabling searching more informative pixels and establishing more accurate patch associations between different images.

2) We propose a novel LiDAR Map assisted Visual features Depth Extraction (LM-VDE) method by mapping 2-D visual features onto the 3-D planes of the LiDAR map, rather than directly fitting 3-D positions with the LiDAR scan points. Thus LM-VDE is independent of the scan's density and generalizes better across different LiDAR sensors.

3) Extensive experiments are conducted on general datasets (e.g., NTU VIRAL [^12] Dataset) and degenerate datasets (collected in degenerate scenes by R3LIVE [^10], FAST-LIVO [^11], and our platform as Fig. 5 shows). Results show that our system achieves competitive performance in general scenes and outperforms other state-of-the-art (SOTA) methods in LiDAR-degraded scenes.

### II. RELATED WORKS

### A. LiDAR-Visual-Inertial Odometry

The integration of multiple sensors in LiDAR-visual-inertial SLAM systems mitigates individual sensor degradation. LVI-SAM [^9] incorporates depth information from LiDAR measurements to visual points and utilizes the pose estimated from the visual-inertial subsystem to initialize LiDAR scan matching. R2LIVE [^13] integrates data from LiDAR, IMU, and camera within a filter-based framework. These two systems employ KLT optical flow to track feature points and conduct re-projection errors for the visual constraints. R3LIVE [^10] enhances the system's state estimation by minimizing the photometric error between tracked points and the map. FAST-LIVO [^11] reuses LiDAR points as visual features to enhance real-time performance.

Since line features are less sensitive to large viewpoint changes [^14] and can provide more geometric constraints [^15], many studies focus on integrating line features into the visual subsystem to introduce additional information. Huang et al. [^16] utilized line features in LiDAR-visual odometry by constructing point-to-line re-projection residual on the image. They proposed a method for line depth extraction by minimizing 3-D point-to-line distance and 2-D re-projection distance. Cao et al. [^17] built a spherical coordinate system to associate the geometric features from the visual and LiDAR subsystems, thus enabling the reconstruction of line segment depths. However, these depth extraction methods rely on the high density of LiDAR scans. In contrast, our proposed LM-VDE employs the 3-D planes stored in the LiDAR maps, requiring limited steps, and performing better adaptation to relatively sparse LiDAR sensors (e.g., 16-line spinning LiDAR).

![Image](images/4aa82c8393779d2e5fe3402a939b56a85f795d994b2f1fa1cf1c785474876e94.jpg)

<div style="text-align: center;">Fig. 2. System overview of the proposed PL-LIVO. This system comprises 4 basic modules: Proprocessing, Data Association, Feature Generation, and Map Manager. The measurements are fused based on an iterated error-state Kalman filter.</div>

### B. Direct Visual Odometry

Direct methods [^18], [^19], [^20] are among the most widely used approaches for estimating pose in visual odometry. Unlike feature-based methods that require extracting salient feature points and generating robust feature descriptors, direct methods select the gradient pixels and construct photometric errors, performing more robustly in low-texture scenes. DVL-SLAM [^21] projects LiDAR scan onto the image and selects those with high-gradient to construct photometric residuals between image frames. PL-SVO [^22] extends the direct method to line features by distributing points uniformly along each line segment, thus obtaining additional constraints in the structured scenes. Fang et al. [^23] utilized depth measurements from the LiDAR scan to recover the positions of visual points and visual lines. Different from these visual-like systems, FAST-LIVO [^11] tightly incorporates image-to-map alignment, LiDAR scan registration, and IMU measurements, improving the system's robustness in challenging environments.

These methods rely on the strict assumption of brightness invariance, which will be compromised by frequent exposure changes and camera lens flare (e.g., indoor-outdoor scenes). Some studies mitigate this issue by online photometric calibration [^24], [^25], but these methods introduce additional computation costs. Differently, we design a patch-based gradient optimization method that is established on a less strict assumption of gradient invariance, thereby enhancing the robustness of the method in environments with varying illumination. Moreover, a rectangular patch is designed for the visual line and aligned with the line direction, hence the corresponding patches between frames can be associated along the line.

### III. SYSTEM OVERVIEW

Fig. 2 depicts the system overview of our proposed PL-LIVO. This system comprises a backbone LiDAR-inertial odometry (blue and gray components) and a direct visual odometry subsystem PL-DVO (red component). Our proposed LM-VDE (green component) method is employed to extract all visual features' depth. We illustrated the details of PL-DVO and LM-VDE in Fig. 4. The data of IMU, LiDAR, and camera are tightly coupled at the observation level.

---

![Image](images/cc755ab5294900883353d7a854b4df47f118acd3e5bb7ddb9d970c9dc00b9b0d.jpg)

![Image](images/06233836366bbcba4324ca15f15b8b014266747eb00c1863668fe639a6722f66.jpg)

<div style="text-align: center;">Fig. 3. Image-based Data Alignment. As (a) shows, the LiDAR scan is treated as a points stream (green and blue points from different scans). Points within the interval of two consecutive images (the red dotted box) are gathered as a new scan. As (b) shows, an aligned unit is generated with the new scan and the current image.</div>

Initially, the input data from LiDAR and camera are fed into the Preprocessing module for data alignment and feature extraction, resulting in an aligned unit for the following steps. Concurrently, the IMU data is gathered for state propagation. Then, the aligned unit is utilized to construct observation residuals in the Data Association module. In detail, the point-to-plane residual is calculated for each LiDAR scan point, while the patch-based gradient residual is computed for each visual feature within the camera's FoV. In particular, the lines' patch pattern is shaped as a rectangle aligned with the line direction. All residuals are weighted by different factors and subsequently employed to update the state in an error-state Kalman filter framework. Furthermore, the Feature Generation module recovers 3-D coordinates of all extracted 2-D visual features by our proposed LM-VDE method. Additionally, the current LiDAR scan points are transformed into the world frame. These new features are then registered to the Visual Map and the LiDAR map respectively for further feature association and residual computation. Old features will be removed to limit memory usage.

### IV. METHODOLOGY

### A. Preprocessing

The Preprocessing module operates on the input data and generates aligned units for subsequent modules. Unlike most current methods [^11], [^13] that update state when receiving each data from LiDAR or camera, we conduct an image-based data alignment that supports joint optimization simultaneously. As Fig. 3(a) shows, the LiDAR scan is treated as a stream of points (green points from the last scan and blue points from the current scan). Points within the interval of two consecutive images (in the red dotted box) are collected as a new LiDAR scan, and then down-sampled and undistorted to the time of the current image. The new LiDAR scan and current image are packed as an aligned unit as Fig. 3(b) shows. Besides, point and line features on the image are detected by [^26], [^27] and the gradient magnitude of the pixels on the image is extracted by the Sobel operator for further computing gradient residuals.

 $$ \mathcal{M}\triangleq S O(3)\times\mathbb{R}^{15} $$

 $$ \begin{aligned}\boldsymbol{x}&\triangleq\left[^{W}\boldsymbol{q}_{b}^{T}\quad^{W}\boldsymbol{v}_{b}^{T}\quad^{W}\boldsymbol{p}_{b}^{T}\quad\boldsymbol{b}_{a}^{T}\quad\boldsymbol{b}_{g}^{T}\quad^{W}\boldsymbol{g}^{T}\right]^{T}\in\mathcal{M}\\\delta\boldsymbol{x}&\triangleq\left[^{W}\delta\boldsymbol{\theta}_{b}^{T}\quad^{W}\delta\boldsymbol{v}_{b}^{T}\quad^{W}\delta\boldsymbol{p}_{b}^{T}\quad\delta\boldsymbol{b}_{a}^{T}\quad\delta\boldsymbol{b}_{g}^{T}\quad^{W}\delta\boldsymbol{g}^{T}\right]^{T}\in\mathbb{R}^{18}\\\boldsymbol{u}&\triangleq\left[\boldsymbol{a}_{m}^{T}\quad\boldsymbol{\omega}_{m}^{T}\right]^{T}\\\boldsymbol{w}&\triangleq\left[\boldsymbol{n}_{a}^{T}\quad\boldsymbol{n}_{a}^{T}\quad\boldsymbol{n}_{ba}^{T}\quad\boldsymbol{n}_{b,v}^{T}\right]^{T}\tag{1}\end{aligned} $$

The IMU data corresponding to the aligned unit (within the red dotted box in Fig. 3(a)) is gathered for state $\pmb{x}$, error state $\delta\pmb{x}$, IMU input $\pmb{u}$, and noise $\pmb{w}$ as (1). $\pmb{W}$ and $\pmb{b}$ are the world frame and the IMU (body) frame, respectively. $\pmb{w}_{q_b}^{'},\pmb{w}_{b_b}^{'},\pmb{w}_{b_b}^{'}$ and $\pmb{w}_{p_b}^{'}$ denote the body rotation, velocity, and position, while $\pmb{b}_a$ and $\pmb{b}_g$ represent the IMU biases. $\pmb{w}_{g}$ signifies the gravity in the world frame. The terms in $\delta\pmb{x}$ are error states relative to those in $\pmb{x}$. $\pmb{w}$ consists of measurement noises of IMU input $\pmb{u}$ and random walk noises of IMU biases.

Using the IMU measurements as input, the nominal state $\hat{x}$, the error state $\delta x$, and the corresponding covariance matrix $\hat{P}$ is propagated using a discrete-time kinematic model as (2). $f(\hat{x}_k, u_k)$, $F_k$, $C_k$, and $Q_k$ are detailed in [^28]. This propagation imposes a prior normal distribution of $x_k$ as (3).

 $$ \begin{aligned}\widehat{\boldsymbol{x}}_{k+1}&=f\left(\widehat{\boldsymbol{x}}_{k},\boldsymbol{u}_{k}\right)\\\delta\boldsymbol{x}_{k+1}&=\left(I+\boldsymbol{F}_{k}\Delta t\right)\delta\boldsymbol{x}_{k}+\left(\boldsymbol{C}_{k}\Delta t\right)\boldsymbol{w}\\\widehat{\boldsymbol{P}}_{k+1}&=\left(I+\boldsymbol{F}_{k}\Delta t\right)\widehat{\boldsymbol{P}}_{k}\left(I+\boldsymbol{F}_{k}\Delta t\right)^{T}\\&\quad+\left(\boldsymbol{C}_{k}\Delta t\right)\boldsymbol{Q}_{k}\left(\boldsymbol{C}_{k}\Delta t\right)^{T}\end{aligned} $$

 $$ \boldsymbol{x}_{k}-\widehat{\boldsymbol{x}}_{k}=\delta\boldsymbol{x}_{k}\sim\mathcal{N}\left(\mathbf{0},\widehat{\boldsymbol{P}}_{k}\right) $$

### B. Patch-Based Gradient Residuals for Visual Features

For each visual feature within the image FoV, its 3-D position is projected from the visual map onto the current image to establish a 2-D feature association with the corresponding feature on the reference image. Subsequently, the patch association around the 2-D feature is performed by extracting a patch P on the current image and calculating the aligned patch Q on the reference image. The pixel-wise differences between the associated patches are computed as the visual residual. Unlike conventional methods that gather the pixels on the grayscale image and calculate photometric residuals, we collect the pixels on the gradient image as Fig. 4(b) shows, which implies that PL-DVO operates under the gradient invariance assumption. This choice is motivated by the fact that exposure changes alter the brightness of the entire image, but have little effect on the relative brightness between the adjacent pixels. Therefore, the gradient of pixels is more stable when illumination changes.

Fig. 4(b) depicts the image patch patterns of visual points and lines. The point patch is a common $2a \times 2a$ square shape while the line patch is shaped as an $a \times 4a$ rectangle whose 4a side is aligned with the line direction. This line patch enables selecting more pixels with informative gradients and helps to associate the corresponding pixels between frames.

Given the ground truth state $x_k$, the visual map $\mathcal{M}_V$ and the current gradient image $I_k$, the patch-based gradient residuals for visual features can be calculated as Algorithm 1 outlines.

For each visual point $\mathcal{P}_i$ (lines 2–8) within the FoV of $\mathbf{I}_k$, we can get its pixel coordinate on $\mathbf{I}_k$ and extract its patch $\mathbf{P}_i$ (line 4). As Fig. 4(a) shows, the associated patch $\mathbf{Q}_i$ on the reference image is calculated through an affine transformation $\mathbf{A}_i$ to align with $\mathbf{P}_i$ (line 5). The pixel-wise difference between the aligned patches is defined as the visual point residual $\mathbf{r}_P$ which should be zero and can be calculated as (4):

 $$ \mathbf{r}_{\mathcal{P}}\left(\boldsymbol{x}_{k},\mathcal{P}_{i}\right)=\mathbf{P}_{i}-\mathbf{A}_{i}\mathbf{Q}_{i}=\mathbf{D}^{\mathbf{p}}\left(\boldsymbol{\pi}\left({}^{C}\boldsymbol{T}_{W}\mathcal{P}_{i}\right)\right)-\mathbf{A}_{i}\mathbf{Q}_{i} $$

---

![Image](images/ec00d8803fc1c7d38d8458c9d876e75fd54d99fe48332f01b78555bfe6abd237.jpg)

<div style="text-align: center;">New Detected Visual Points/Lines on Current Image  Projected Visual Points/Lines on Current Image from Visual Map</div>

<div style="text-align: center;">Fig. 4. Overview of the proposed PL-DVO. The proposed patch-based gradient residual for points and lines is constructed as (a). The utilized patch pattern is designed as (b). (c-f) depict the proposed LM-VDE method for extracting the depth of visual features.</div>

```pseudo
// Algorithm 1: Patch-Based Gradient Residuals.

```

Data: visual local map $\mathcal{M}_{V}$, gradient image $\mathbf{I}_{k}$, ground truth state $x_{k}$

Result: visual point residuals $\{\mathbf{r}_{P}\}$, visual line residuals $\{\mathbf{r}_{L}\}$

1 $\longrightarrow$ visual point residual $\longrightarrow$;

2 for each point feature $\mathcal{P}_{i}$ in $\mathcal{M}_{V}$ do

3 $\left|\begin{array}{c} if \mathcal{P}_{i} \text{ within the } \text{FoV of } \mathbf{I}_{k} \text{ then } \\ \text{compute square gradient patch } \mathbf{P}_{i} ; \\ \text{calculate associated reference patch } \mathbf{Q}_{i} \text{ by } \text{affine transformation } \mathbf{A}_{i}, \text{ and compute point } \\ \text{patch residual } \mathbf{r}_{P} (4); \end{array}\right.$

4 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$

5 $\left|\begin{array}{c} \text{end} \\ \text{for each line feature } \mathcal{L}_{i} \text{ in } \mathcal{M}_{V} \text{ do } \\ \text{if } \mathcal{L}_{i} \text{ within the } \text{FoV of } \mathbf{I}_{k} \text{ then } \\ \text{for each sample point } l_{m} \text{ on } \mathcal{L} \text{ do } \\ \text{compute rectangle gradient patch } \mathbf{P}_{m} \\ \text{aligned with line direction } (5); \\ \text{calculate associated reference patch } \mathbf{Q}_{m} \text{ by } \text{line direction and scale, and compute point-on-line patch residual } \mathbf{r}_{l} (5); \end{array}\right.$

6 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$

7 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$

8 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$

9 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{for each line feature } \mathcal{L}_{i} \text{ in } \mathcal{M}_{V} \text{ do } \\ \text{if } \mathcal{L}_{i} \text{ within the } \text{FoV of } \mathbf{I}_{k} \text{ then } \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

10 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$

11 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{for each sample point } l_{m} \text{ on } \mathcal{L} \text{ do } \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

12 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{for each sample point } l_{m} \text{ on } \mathcal{L} \text{ do } \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

13 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

14 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

15 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

16 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

17 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

18 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

19 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$

where $\pi(\bullet)$ is the camera projection model and $\mathbf{D}^{\mathbf{P}}$ denotes the operation to extract the neighbor pixels within the $2a \times 2a$ square on the gradient image. ${}^{C}T_{W}$ represents the transformation from the world to the camera frame, which is computed from the extrinsic ${}^{C}T_{b}$ and the rotation and position state.

For each visual line $\mathcal{L}_i$ (lines 10-19) within the camera FoV, n points $\{l_m\}$ has been sampled on $\mathcal{L}_i$ when it was extracted (see Section. IV-D). We extract the neighbor pixels of each $l_m$ within an $a \times 4a$ rectangle on the gradient image (this operation is denoted as $\mathbf{D}^1$) to form a line patch $\mathbf{P}_m$ (line 13). As Fig. 4(b) shows, the long side of the rectangle is aligned with the line direction to cover more pixels with distinct gradients. As depicted in Fig. 4(a), this also enables calculating the associated reference patch $\mathbf{Q}_m$ by orienting it along the line direction without an affine transformation (line 14). To further improve the patch alignment, a scaling factor $s_m$ that describes the depth ratio of $l_m$ between the current frame and the reference frame should be employed to scale the $\mathbf{Q}_m$. We define the pixel-wise difference between each pair of $\mathbf{P}_m$ and $\mathbf{Q}_m$ as $\mathbf{r}_1$ which will then be accumulated as the line patch residual $\mathbf{r}_L$ (line 16). $\mathbf{r}_1$, $\mathbf{r}_L$ should be zero and can be calculated as (5), (6):

 $$ 0=\mathbf{r}_{1}\left(\boldsymbol{x}_{k},\boldsymbol{l}_{m}\right)=\mathbf{D}^{1}\left(\boldsymbol{\pi}^{\left(C\right)}T_{W}\boldsymbol{l}_{m}\right))-\boldsymbol{s}_{m}\mathbf{Q}_{m} $$

 $$ 0=\mathbf{r}_{\mathcal{L}}\left(\boldsymbol{x}_{k},L_{i}\right)=\sum_{m=1}^{n}\mathbf{r}_{1}\left(\boldsymbol{x}_{k},l_{m}\right) $$

Both residuals will be optimized on a three-level gradient image pyramid to update the state as detailed in Section. IV-C.

### C. LiDAR Observation and State Updates

Our LiDAR map employs VoxelMap [^29] that stores a collection of plane features in voxels. Given the ground truth state $\boldsymbol{x}_k$, the LiDAR local map $\mathcal{M}_L$, and the down-sampled LiDAR scan $\{\mathbf{p}_i\}$, we search for valid point-to-plane matches and calculate the LiDAR observation residual $\mathbf{r}_L(\boldsymbol{x}_k, \mathbf{p}_i)$ following [^29]. $\mathbf{r}_L$ is subject to a normal distribution $\mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_L)$.

By combining the prior state distribution (3) with observation distributions of LiDAR points and visual features (4) (6), we derive the maximum a posteriori estimation for $\mathbf{x}_k$ as (7):

 $$ \begin{aligned}\min_{\boldsymbol{x}_{k}\in\mathcal{M}}&\left(\left\|\boldsymbol{x}_{k}-\widehat{\boldsymbol{x}}_{k}\right\|_{\bar{P}_{k}}^{2}+\sum_{i=1}^{m_{L}}\left\|\mathbf{r}_{\mathbf{L}}\left(\boldsymbol{x}_{k},\mathbf{p}_{i}\right)\right\|_{\boldsymbol{\Sigma}_{\mathbf{L}}}^{2}\right.\\&\left.+\sum_{i=1}^{m_{p}}\left\|\mathbf{r}_{\mathcal{P}}\left(\boldsymbol{x}_{k},\mathcal{P}_{i}\right)\right\|_{\boldsymbol{\Sigma}_{\mathcal{P}}}^{2}+\sum_{i=1}^{m_{l}}\left\|\mathbf{r}_{\mathcal{L}}\left(\boldsymbol{x}_{k},\mathcal{L}_{i}\right)\right\|_{\boldsymbol{\Sigma}_{\mathcal{L}}}^{2}\right)\end{aligned} $$

where  $\|\boldsymbol{x}\|_{\hat{\boldsymbol{\Sigma}}}^{2} = \boldsymbol{x}^{T} \boldsymbol{\Sigma}^{-1} \boldsymbol{x}$.  $\boldsymbol{\Sigma}_{\mathcal{P}}$ and  $\boldsymbol{\Sigma}_{\mathcal{L}}$ are two pre-defined factors to weigh  $\boldsymbol{r}_{\mathcal{P}}$ and  $\boldsymbol{r}_{\mathcal{L}}$ during the optimization.  $m_{L}$ LiDAR

---

```pseudo
// Algorithm 2: LM-VDE: LiDAR Map Assisted Visual Features Depth Extraction.

```

Data: LiDAR local map $\mathcal{M}_L$, down-sampled LiDAR scan $\mathbf{L}_k$, detected points $\{p_i\}$ and lines $\{e_1e_{2_i}\}$

Result: 3-D points $\{\mathcal{P}_i\}$ and 3-D lines $\{\mathcal{L}_i\}$

1 Let $e_1$, $e_2$ be start point, end point of $e_1e_2$;

2 Project $\mathbf{L}_k$ onto the image;

3 —— point depth extraction ——;

4 for each p in $\{p_i\}$ do

5 search on the image for the nearest LiDAR scan point $\mathbf{p}_L$ in $\mathbf{L}_k$;

6 use $\mathbf{p}_L$ index the plane features $\pi_{\mathcal{P}}$ from $\mathcal{M}_L$;

7 in camera frame: intersect the projective line and $\pi_{\mathcal{P}}$ to solve depth $\mathbf{z}$ (9);

8 in camera frame: recover 3-D coordinate $\mathcal{P}^C$ along the projective line using $\mathbf{z}$ (8);

9 transform $\mathcal{P}^C$ to world frame $\mathcal{P}$;

10 add $\mathcal{P}$ to $\{\mathcal{P}_i\}$;

11 end

12 —— line depth extraction ——;

13 for each $e_1e_2$ in $\{e_1e_2\}_i$ do

14 sample n points $\{e_m\}$ on $e_1e_2$;

15 for each $e_m$ do

16 repeat 5 - 6, index $\pi_{\mathcal{L}_m}$;

17 add $\pi_{\mathcal{L}_m}$ to $\{\pi_{\mathcal{L}_m}\}$;

18 end

19 perform RANSAC on $\{\pi_{\mathcal{L}_m}\}$ and fit $\pi_{\mathcal{L}}$;

20 repeat 7 - 9 on $e_1$, $e_1$ and recover their 3-D coordinate $\mathbf{E}_1$, $\mathbf{E}_2$ in world frame;

21 parameterize $\mathcal{L}$ with endpoint $\mathbf{E}_1$, $\mathbf{E}_2$;

22 add $\mathcal{L}$ to $\{\mathcal{L}_i\}$;

23 end

points, $m_p$ visual points, and $m_l$ visual lines are fused to optimize the state $\mathbf{x}_k$.

This optimization is solved by an iterated error-state Kalman filter, starting from the top pyramid level with the lowest resolution and progressing to subsequent levels. Since the propagated state provides the initial pose for visual patch alignment, all levels undergo only a single iteration, except for the bottom level, which undergoes two iterations using the high-resolution image. During each iteration, residuals from LiDAR and visual observations are computed using the progressively optimized state to perform the current optimization.

### D. LM-VDE and Map Update

A local visual map $\mathbf{M}_V$ is dynamically updated by inserting new visual features that enter the image and removing the features that exit the image. The depth of the new detected visual features is extracted by our proposed LM-VDE method. This method leverages the down-sampled LiDAR scan $\mathbf{L}_k$ to index 3-D planes from the local LiDAR map $\mathbf{M}_L$, then maps the 2-D visual features on the 3-D planes. The visual points and lines that lie on the 3-D planes feature stable backgrounds and maintain relatively stable gradient distribution across different viewpoints, making them suitable for constructing residuals. We discuss LM-VDE in Algorithm 2 and detail it in the following paragraphs.

For each detected point p (lines 4-11), as Fig. 4(f) shows, its 3-D coordinate $\mathcal{P}^C$ in the camera frame can be calculated along the projective line using (8). The depth z is ambiguous and needs to be solved. Initially, we project $L_k$ onto the image and perform the nearest neighbor search to get the nearest scan point $\mathbf{p}_L$ as Fig. 4(c)–(d). $\mathbf{p}_L$ is then employed to index the leaf voxel from the LiDAR map $\mathcal{M}_L$ as Fig. 4(e) (lines 5–6). Subsequently, if the minimum eigenvalue of all contained points in the leaf voxel is larger than a threshold, indicating a satisfactory plane $\pi_P$ stored in it, we can formulate the relation that $\mathcal{P}$ lies on $\pi_P$ as (9) to solve z (Fig. 4(f) and line 7). At last, $\mathcal{P}^C$ is recovered by (8) and transformed to the world frame $\mathcal{P}$ using $\mathbf{x}_k$ (lines 8–10).

 $$ \mathcal{P}^{\mathrm{C}}=\begin{bmatrix}z\left(p_{x}-c_{x}\right)/f_{x}\\ z\left(p_{y}-c_{y}\right)/f_{y}\\ z\end{bmatrix} $$

 $$ \left(^{W}\boldsymbol{T}_{C}\mathcal{P}^{\mathrm{C}}-\mathbf{c}\right)\cdot\mathbf{n}^{T}=0 $$

The term $f_x$, $f_y$, $c_x$, and $c_y$ denote camera intrinsic. c and n represent the center point and the normal of $\pi_P$.

The methodology for point depth extraction is extended to recover line depths (13–23). For each detected line $e_1e_2$ on the image, its 3-D position $\mathcal{L}$ is parametrized with the endpoints $E_1$ and $E_2$ in the world frame. As Fig. 4(c-d) shows, we uniformly sample n 2-D points $\{e_m\}$ on $e_1e_2$ and search their nearest LiDAR points. These points are utilized to index n related planes $\{\pi_{\mathcal{L}_m}\}$ from $\mathcal{M}_L$ (Fig. 4(e) and lines 14–18). Subsequently, a RANSAC is performed on $\{\pi_{\mathcal{L}_m}\}$ to fit a new plane $\pi_\mathcal{L}$ on which $\mathcal{L}$ is expected to lie (Fig. 4(f) and line 19). Similar to the last paragraph, we can solve the depth of two endpoints $e_1$ and $e_2$ by (9) and recover their 3-D coordinates in the camera from by (8). At last, we transform the endpoints to the world frame using $x_k$ to parameterize $\mathcal{L}$ (lines 20–22). Moreover, n 3-D points $\{l_m\}$ with the maximum Shi-Tomas score are sampled on $\mathcal{L}$, and these points will be utilized to construct patch-based line residuals as Section. IV-B.

Using the updated state $\boldsymbol{x}_k$, we re-transform the down-sampled LiDAR scan $\mathbf{L}_k$ to the world frame and compute their covariances following [^29]. Then, these points are inserted into the LiDAR map $\boldsymbol{M}_L$ while those oldest voxels are removed.

### V. EXPERIMENTS

In this section, we compared our proposed PL-LIVO with SOTA odometry systems in general scenes and challenging degenerate scenes. The generalization of our proposed LM-VDE was also verified on a relatively sparse 16-line spinning LiDAR sensor. All experiments were conducted on a computer equipped with an Intel i5-1135G7 CUP and a 16GB RAM. Moreover, we collected a dataset in degenerate scenes using the platform as Fig. 5 shows, which comprised a Livox Avia LiDAR sensor (with a built-in IMU) and an industrial camera. All sensors were hard-synchronized following FAST-LIVO2 [^30]. We empirically set the parameter a of the visual patch pattern to 4 pixels to adequately cover the gradient distribution near the visual features. Both factors $r_P$ and $r_L$ that weight the patch-based visual residuals during state optimization were pre-defined as 100. We employed a 3-level gradient image pyramid, performing

---

![Image](images/a75b094a5c6d1aee970ac22f8c34e0dc6f610d49ccbb69ae7302be9b62acb47e.jpg)

<div style="text-align: center;">Fig. 5. Our platform with two usage configurations: (a) handheld and (b) robot-mounted. The LiDAR and the camera are hard synchronized with homogenous PWM signals.</div>

<div style="text-align: center;">TABLEI</div>

<div style="text-align: center;">ABSOLUTE TRAJECTORY ERRORS (ATE, METERS) ON NTU-VIRAL SEQUENCES</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>FAST-LIO</td><td style='text-align: center; word-wrap: break-word;'>VoxelMap</td><td style='text-align: center; word-wrap: break-word;'>FAST-LIVO</td><td style='text-align: center; word-wrap: break-word;'>R3LIVE</td><td style='text-align: center; word-wrap: break-word;'>PL-LIVO</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>eee_01</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>0.023</td><td style='text-align: center; word-wrap: break-word;'>0.084</td><td style='text-align: center; word-wrap: break-word;'>0.147</td><td style='text-align: center; word-wrap: break-word;'>0.022</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>eee_02</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>0.079</td><td style='text-align: center; word-wrap: break-word;'>0.159</td><td style='text-align: center; word-wrap: break-word;'>0.020</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>eee_03</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.176</td><td style='text-align: center; word-wrap: break-word;'>0.120</td><td style='text-align: center; word-wrap: break-word;'>0.027</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>nya_01</td><td style='text-align: center; word-wrap: break-word;'>0.027</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.079</td><td style='text-align: center; word-wrap: break-word;'>0.111</td><td style='text-align: center; word-wrap: break-word;'>0.024</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>nya_02</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>0.098</td><td style='text-align: center; word-wrap: break-word;'>0.110</td><td style='text-align: center; word-wrap: break-word;'>0.024</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>nya_03</td><td style='text-align: center; word-wrap: break-word;'>0.038</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.109</td><td style='text-align: center; word-wrap: break-word;'>0.123</td><td style='text-align: center; word-wrap: break-word;'>0.026</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>sbs_01</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.099</td><td style='text-align: center; word-wrap: break-word;'>0.080</td><td style='text-align: center; word-wrap: break-word;'>0.029</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>sbs_02</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.033</td><td style='text-align: center; word-wrap: break-word;'>0.097</td><td style='text-align: center; word-wrap: break-word;'>0.138</td><td style='text-align: center; word-wrap: break-word;'>0.025</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>sbs_03</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.088</td><td style='text-align: center; word-wrap: break-word;'>0.146</td><td style='text-align: center; word-wrap: break-word;'>0.027</td></tr></table>

pose optimization for each aligned unit over 4 iterations to ensure convergence.

### A. Accuracy Evaluation on the NTU VIRAL Datasets

Quantitative experiments were conducted on the 9 sequences of the public dataset NTU-VIRAL [^12], which provided the ground truth of the trajectory captured by the Leica laser tracker. We compared our proposed PL-LIVO with several open-sourced SOTA odometry methods, including two LiDAR-inertial odometry, FAST-LIO [^8] and VoxelMap [^29], and two LiDAR-inertial-visual odometry, R3LIVE [^10] and FAST-LIVO [^11]. For a fair comparison, we unified the voxel size of the downsampling filter as 0.2 m for indoor scenes (nya_xx) and 0.5 m for outdoor scenes (eee_xx and sbs_xx). The grid sizes of the LiDAR map were unified as 0.4 m and 1.0 m for indoor and outdoor scenes, respectively. Other parameters in each system were set as the default values.

We recorded the trajectories of the IMU frame and calculated the absolute trajectory error using the official evaluation tools provided by NTU-VIRAL [^12]. The results are depicted in Table I. The LiDAR subsystem of PL-LIVO is the same as VoxelMap [^29], which achieved higher accuracy than FAST-LIO [^8] on 6 of 9 sequences owing to its adaptive and coarse-to-fine voxel construction. Unexpectedly, the performance of two LiDAR-inertial systems surpassed the two LiDAR-inertial-visual systems. The reason is that R3LIVE [^10] and FAST-LIVO [^11] updated the state with each received measurement (LiDAR scan or image), so the image blur would significantly influence the pose estimation without the constraints from LiDAR. In contrast, beneficial from the image-based data alignment, PL-LIVO jointly updated the state with tightly coupled constraints from both LiDAR and image. Moreover, the proposed PL-DVO enhanced the system's accuracy because visual line features introduced more effective constraints. Consequently, PL-LIVO achieved the highest accuracy on 8 out of 9 sequences.

### B. Robustness Evaluation on the Degenerate Datasets

We evaluated the robustness of PL-LIVO on several degenerate datasets including 5 public sequences and 6 our collected sequences. For all the testing sequences, the start point and the end point were the same. Thus we used the start-to-end drift error (distance between the first and the last pose) to evaluate all the candidate methods. The results are listed in Table II.

FAST-LIO [^8] and VoxelMap [^29] exhibited noticeable drift errors on all sequences because LiDAR provided insufficient constraints in degenerate directions. While FAST-LIVO [^11] and R3LIVE [^10] mitigated degradation and reduced drifts in several sequences by incorporating constraints from visual points, they still encountered significant drifts in certain sequences. For instance, R3LIVE [^10] performed poorly in LiDAR_Degenerate where the Livox Avia LiDAR sensor faced a tiled wall with repetitive texture. This is due to the low image sampling frequency (10 Hz) resulting in the significant disparity between consecutive frames. Consequently, the optical flow tracking fell into local minima, imposing erroneous visual constraints on pose estimation. FAST-LIVO [^11] yielded unsatisfactory results in degenerate_00 where the LiDAR sensor faced the ground and moved aggressively. This issue arises from the LiDAR sensor's proximity to the ground, leading to a restricted field of view and insufficient LiDAR-projected points to complement the visual features. Another factor resulting in FAST-LIVO's limited performance in degenerate_xx is the lack of hardware synchronization in these sequences. On our collected challenging dataset, both FAST-LIVO [^11] and R3LIVE [^10] exhibited suboptimal performance due to the low camera sampling frequency (10 Hz), along with aggressive motion and significant exposure variations. Compared to these methods, our proposed PL-LIVO extracts sufficient visual features (points and lines) and recovers their 3-D positions through LM-VDE. Additionally, it constructs more robust visual constraints by leveraging the gradient image pyramid. As a result, it achieved the highest accuracy in 10 out of 11 sequences. Notably, its start-to-end drift errors across all testing sequences were less than 0.1m, while other methods showcased fluctuating levels of drifts throughout different sequences. This demonstrates the robustness of PL-LIVO in handling degenerate environments.

We conducted an ablation study to verify the beneficial effect of the proposed patch-based gradient residuals and visual line features. As shown in Table II, two variants of PL-LIVO were tested: P-LIVO (only visual points) and L-LIVO (only visual lines). We employed the illumination residuals (brightness in-variation) and the proposed gradient residuals (gradient in-variation) for each variant. The result shows that those adopting gradient residuals achieved lower drifts on 9, 10, and 11 out of 11 sequences. Using the same visual residual, P-LIVO performed higher accuracy than L-LIVO on 6, 7 out of 11 sequences because the points were more common than lines and provided more visual constraints. PL-LIVO outperformed both variants on all sequences due to the complementary constraints from

---

<div style="text-align: center;">TABLE II START-TO-END DRIFT ERRORS (METERS) ON DEGENERATE SEQUENCES</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td rowspan="3"></td><td rowspan="3">Degenerate $_{{(*)}}$ Type</td><td rowspan="3">Camera Frequency</td><td rowspan="3">FAST-LIO</td><td rowspan="3">VoxelMap</td><td rowspan="3">FAST-LIVO</td><td rowspan="3">R3LIVE</td><td colspan="6">Proposed</td></tr><tr><td colspan="3">Brightness Invariance</td><td colspan="3">Gradient Invariance</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>P-LIVO</td><td style='text-align: center; word-wrap: break-word;'>L-LIVO</td><td style='text-align: center; word-wrap: break-word;'>PL-LIVO</td><td style='text-align: center; word-wrap: break-word;'>P-LIVO</td><td style='text-align: center; word-wrap: break-word;'>L-LIVO</td><td style='text-align: center; word-wrap: break-word;'>PL-LIVO</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>degenerate_00</td><td style='text-align: center; word-wrap: break-word;'>L, V2</td><td style='text-align: center; word-wrap: break-word;'>20Hz</td><td style='text-align: center; word-wrap: break-word;'>5.282</td><td style='text-align: center; word-wrap: break-word;'>4.927</td><td style='text-align: center; word-wrap: break-word;'>6.062</td><td style='text-align: center; word-wrap: break-word;'>0.076</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>3.021</td><td style='text-align: center; word-wrap: break-word;'>2.070</td><td style='text-align: center; word-wrap: break-word;'>7.324</td><td style='text-align: center; word-wrap: break-word;'>0.061</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>degenerate_01</td><td style='text-align: center; word-wrap: break-word;'>L, V2</td><td style='text-align: center; word-wrap: break-word;'>20Hz</td><td style='text-align: center; word-wrap: break-word;'>8.847</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>0.227</td><td style='text-align: center; word-wrap: break-word;'>0.100</td><td style='text-align: center; word-wrap: break-word;'>0.198</td><td style='text-align: center; word-wrap: break-word;'>0.377</td><td style='text-align: center; word-wrap: break-word;'>0.081</td><td style='text-align: center; word-wrap: break-word;'>0.917</td><td style='text-align: center; word-wrap: break-word;'>0.199</td><td style='text-align: center; word-wrap: break-word;'>0.078</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>degenerate_02</td><td style='text-align: center; word-wrap: break-word;'>L, V1</td><td style='text-align: center; word-wrap: break-word;'>30Hz</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>0.102</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>0.013</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>0.010</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>LiDAR_Degenerate</td><td style='text-align: center; word-wrap: break-word;'>L</td><td style='text-align: center; word-wrap: break-word;'>10Hz</td><td style='text-align: center; word-wrap: break-word;'>4.732</td><td style='text-align: center; word-wrap: break-word;'>3.332</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>8.037</td><td style='text-align: center; word-wrap: break-word;'>0.042</td><td style='text-align: center; word-wrap: break-word;'>0.745</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.013</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.007</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Visual_Challenge</td><td style='text-align: center; word-wrap: break-word;'>L, V1</td><td style='text-align: center; word-wrap: break-word;'>10Hz</td><td style='text-align: center; word-wrap: break-word;'>1.023</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>0.043</td><td style='text-align: center; word-wrap: break-word;'>0.057</td><td style='text-align: center; word-wrap: break-word;'>0.043</td><td style='text-align: center; word-wrap: break-word;'>0.047</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.042</td><td style='text-align: center; word-wrap: break-word;'>0.029</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>corridor_01</td><td style='text-align: center; word-wrap: break-word;'>L</td><td style='text-align: center; word-wrap: break-word;'>10Hz</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>7.890</td><td style='text-align: center; word-wrap: break-word;'>1.166</td><td style='text-align: center; word-wrap: break-word;'>0.378</td><td style='text-align: center; word-wrap: break-word;'>0.117</td><td style='text-align: center; word-wrap: break-word;'>0.086</td><td style='text-align: center; word-wrap: break-word;'>0.099</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.014</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>corridor_02</td><td style='text-align: center; word-wrap: break-word;'>L</td><td style='text-align: center; word-wrap: break-word;'>10Hz</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>3.499</td><td style='text-align: center; word-wrap: break-word;'>1.122</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>0.242</td><td style='text-align: center; word-wrap: break-word;'>5.952</td><td style='text-align: center; word-wrap: break-word;'>0.178</td><td style='text-align: center; word-wrap: break-word;'>0.083</td><td style='text-align: center; word-wrap: break-word;'>0.780</td><td style='text-align: center; word-wrap: break-word;'>0.017</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>stairs_01</td><td style='text-align: center; word-wrap: break-word;'>L, V2</td><td style='text-align: center; word-wrap: break-word;'>10Hz</td><td style='text-align: center; word-wrap: break-word;'>2.134</td><td style='text-align: center; word-wrap: break-word;'>2.312</td><td style='text-align: center; word-wrap: break-word;'>3.812</td><td style='text-align: center; word-wrap: break-word;'>3.046</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>2.609</td><td style='text-align: center; word-wrap: break-word;'>1.047</td><td style='text-align: center; word-wrap: break-word;'>0.173</td><td style='text-align: center; word-wrap: break-word;'>0.682</td><td style='text-align: center; word-wrap: break-word;'>0.046</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>stairs_02</td><td style='text-align: center; word-wrap: break-word;'>L, V2</td><td style='text-align: center; word-wrap: break-word;'>10Hz</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>4.394</td><td style='text-align: center; word-wrap: break-word;'>7.913</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>9.122</td><td style='text-align: center; word-wrap: break-word;'>2.210</td><td style='text-align: center; word-wrap: break-word;'>1.351</td><td style='text-align: center; word-wrap: break-word;'>0.522</td><td style='text-align: center; word-wrap: break-word;'>0.359</td><td style='text-align: center; word-wrap: break-word;'>0.010</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>campus_01</td><td style='text-align: center; word-wrap: break-word;'>L</td><td style='text-align: center; word-wrap: break-word;'>10Hz</td><td style='text-align: center; word-wrap: break-word;'>2.539</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>1.655</td><td style='text-align: center; word-wrap: break-word;'>3.937</td><td style='text-align: center; word-wrap: break-word;'>0.081</td><td style='text-align: center; word-wrap: break-word;'>0.198</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>0.189</td><td style='text-align: center; word-wrap: break-word;'>0.045</td><td style='text-align: center; word-wrap: break-word;'>0.024</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>campus_02</td><td style='text-align: center; word-wrap: break-word;'>L, V2</td><td style='text-align: center; word-wrap: break-word;'>10Hz</td><td style='text-align: center; word-wrap: break-word;'>2.393</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>2.946</td><td style='text-align: center; word-wrap: break-word;'>9.298</td><td style='text-align: center; word-wrap: break-word;'>2.962</td><td style='text-align: center; word-wrap: break-word;'>fail</td><td style='text-align: center; word-wrap: break-word;'>1.258</td><td style='text-align: center; word-wrap: break-word;'>0.326</td><td style='text-align: center; word-wrap: break-word;'>0.920</td><td style='text-align: center; word-wrap: break-word;'>0.053</td></tr></table>

 $^{*}$ L denotes LiDAR degeneration, V1, V2 denote camera faces a white wall or undergoes significant exposure changes, respectively.

The best results are marked in bold and the second-best results are underlined.

 $^{2}$ fail means a drift error $\geq$ 10 m.

![Image](images/cac08eb19272f0a5dd71bd98f316f0a03ef4745df61dfb7e345c69cd13a89849.jpg)

<div style="text-align: center;">Fig. 6. Localization and mapping result of PL-LIVO under the degenerate_00 sequence. In this sequence, the LiDAR experienced degradation while oriented towards the ground. Due to the constraints provided by the gradient residuals of visual points and lines, PL-LIVO achieved stable localization and accurately returned to the origin.</div>

visual points and lines, illustrating the benefit of additional line features for pose estimation.

Fig. 1 demonstrates that PL-LIVO achieved stable localization and high-precision mapping despite significant exposure changes, LiDAR degradation, and IMU fluctuations in the stairs_01 sequence. Fig. 6 demonstrates the robust localization and mapping capabilities of PL-LIVO in the degenerate_00 sequence, even with severe LiDAR degradation and a highly constrained field of view. Additionally, Fig. 6(a)–(b) depicts the 2-D visual lines and points on the gradient image, while Fig. 6(c) shows their corresponding 3-D points and lines.

![Image](images/fcb00eb47e30570cdd88eb8e6fa7bab328e4dfd625145ec2b2f40015d178a3b0.jpg)

<div style="text-align: center;">Fig. 7. Line map constructed by Huang et al. [^16] and LM-VDE.</div>

### C. Line Depth Extraction Method Evaluation

To demonstrate the generalization of our proposed LM-VDE across different LiDAR sensors, we compared LM-VDE with the depth extraction method proposed by Huang et al. [^16]. Since Livox LiDAR scans were of high density, both LM-VDE and [^16] could recover the depth of most detected lines. However, the extraction task became more challenging when dealing with relatively sparse scans from the spinning LiDAR. For this evaluation, we visualized and compared the line map constructed using both two methods on the NTU-VIRAL [^12] eee_01, nya_01, and sbs_01 sequences. The results, depicted in Fig. 7, indicate that LM-VDE recovered more lines than [^16]. This discrepancy illustrates LM-VDE's superior adaptability to relatively sparse LiDAR data, highlighting its generalization across diverse LiDAR sensors.

### D. Time Consumption Evaluation

In this section, we evaluate the average processing time of the full PL-LIVO system on our collected dataset, including indoor and outdoor environments. The average processing time

---

<div style="text-align: center;">TABLE III</div>

<div style="text-align: center;">AVERAGE PROCESSING TIME (MILLISECONDS) ON OUR COLLECTED DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'>Modules</td><td style='text-align: center; word-wrap: break-word;'>Process</td><td style='text-align: center; word-wrap: break-word;'>Time</td></tr><tr><td rowspan="3">Preprocessing</td><td style='text-align: center; word-wrap: break-word;'>Image Preprocessing</td><td style='text-align: center; word-wrap: break-word;'>11.0</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>LiDAR Scan Preprocessing</td><td style='text-align: center; word-wrap: break-word;'>1.2</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>IMU Propagation</td><td style='text-align: center; word-wrap: break-word;'>1.2</td></tr><tr><td rowspan="2">Data Association $^{(*)}$</td><td style='text-align: center; word-wrap: break-word;'>Visual Observation Model</td><td style='text-align: center; word-wrap: break-word;'>6.6</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>LiDAR Observation Model</td><td style='text-align: center; word-wrap: break-word;'>25.6</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>State Estimation $^{(*)}$</td><td style='text-align: center; word-wrap: break-word;'>IESKF Update</td><td style='text-align: center; word-wrap: break-word;'>5.6</td></tr><tr><td rowspan="2">Feature Generation and Map Manager</td><td style='text-align: center; word-wrap: break-word;'>LM-VDE and Visual Map Update</td><td style='text-align: center; word-wrap: break-word;'>4.5</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>LiDAR Map Update</td><td style='text-align: center; word-wrap: break-word;'>1.6</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>PL-LIVO</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>57.3</td></tr></table>

The module undergoes 4 iterations during optimization, and the table presents the total time consumption of the 4 iterations.

of PL-LIVO per aligned unit is 57.3 ms, whereas FAST-LIVO [^11] achieves a processing time of 49.3ms. This additional computational overhead arises from the extraction of 2-D visual features and the reconstruction of their 3-D positions. Nevertheless, our system maintains real-time performance and improves robustness in degenerate environments. We further detail the average processing time of PL-LIVO in Table III.

### VI. CONCLUSION

In this letter, a point-line LiDAR-visual-inertial odometry (PL-LIVO) is proposed that outperforms previous methods in mitigating LIDAR degeneration. The patch-based gradient optimization improves the robustness of the method against exposure changes and lens flare. A LiDAR map assisted visual features depth extraction (LM-VDE) method is also proposed to recover the 3-D coordinates of visual lines, resulting in superior adaption to relatively sparse LiDAR data as well as more line features being recovered. Comparative evaluations demonstrate that PL-LIVO achieves higher accuracy in general environments and superior robustness in challenging degenerate scenes. In practical applications, PL-LIVO can be used for inspection robots or handheld mapping equipment working in challenging environments. However, it is worth mentioning that PL-LIVO requires high-precision calibration results between the camera and LiDAR. For future works, we plan to extend our research by introducing online extrinsic calibration utilizing point and line features, thereby creating a more application-friendly system.

## REFERENCES

[^1]: J. Zhang and S. Singh, "LOAM: LiDAR odometry and mapping in real-time," in Proc. Robot.: Sci. Syst., Jul. 2014, vol. 2, pp. 1–9.

[^2]: H. Shen, Q. Zong, B. Tian, and H. Lu, "Voxel-based localization and mapping for multirobot system in GPS-denied environments," IEEE Trans. Ind. Electron., vol. 69, no. 10, pp. 10333–10342, Oct. 2022.

[^3]: Y. Fang, K. Qian, Y. Zhang, T. Shi, and H. Yu, “Segmented curved-voxel occupancy descriptor for dynamic-aware LiDAR odometry and mapping,” IEEE Trans. Geosci. Remote Sens., vol. 62, 2024, Art. no. 5701916.

[^4]: M. Helmberger, K. Morin, B. Berner, N. Kumar, G. Cioffi, and D. Scaramuzza, “The hilti SLAM challenge dataset,” IEEE Robot. Automat. Lett., vol. 7, no. 3, pp. 7518–7525, Jul. 2022.

[^5]: I. Filip, J. Pyo, M. Lee, and H. Joe, "LiDAR SLAM comparison in a featureless tunnel environment," in 2022 22nd Int. Conf. Control, Automat. Syst., Nov. 2022, pp. 1648–1653.

[^6]: H. Li, B. Tian, H. Shen, and J. Lu, “An intensity-augmented LiDAR-inertial SLAM for solid-state LiDARs in degenerated environments,” IEEE Trans. Instrum. Meas., vol. 71, 2022, Art. no. 8503610.

[^7]: T. Shan, B. Engl. J. Med.

[^8]: W. Xu and F. Zhang, "FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter," IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021.

[^9]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled LiDAR-visual-inertial odometry via smoothing and mapping," in 2021 IEEE Int. Conf. Robot. Automat., Jun. 2021, pp. 5692–5698.

[^10]: J. Lin and F. Zhang, “R $^{3}$LIVE: A robust, real-time, RGB-colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package,” in 2022 Int. Conf. Robot. Automat., May 2022, pp. 10672–10678.

[^11]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, "FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry," in 2022 IEEE/RSJ Int. Conf. Intell. Robots Syst., Oct. 2022, pp. 4003–4009.

[^12]: T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie, "NTU VIRAL: A visual-inertial-ranging-LiDAR dataset, from an aerial vehicle viewpoint," Int. J. Robot. Res., vol. 41, no. 3, pp. 270–280, 2022.

[^13]: J. Lin, C. Zheng, W. Xu, and F. Zhang, “R $^{2}$ LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping,” IEEE Robot. Automat. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021.

[^14]: A. P. Gee and W. Mayol-Cuevas, “Real-time model-based SLAM using line segments,” in Proc. Int. Symp. Vis. Comput., 2006, pp. 354–363.

[^15]: S. Lin, X. Zhang, Y. Liu, H. Wang, X. Zhang, and Y. Zhuang, "FLM PL-VIO: A robust monocular point-line visual-inertial odometry based on fast line matching," IEEE Trans. Ind. Electron., early access, Apr. 23, 2024, doi: 10.1109/TIE.2024.3379661.

[^16]: S.-S. Huang, Z.-Y. Ma, T.-J. Mu, H. Fu, and S.-M. Hu, "LiDAR-monocular visual odometry using point and line features," in 2020 IEEE Int. Conf. Robot. Automat., Jun. 2020, pp. 1091–1097.

[^17]: K. Cao et al., “Tightly-coupled LiDAR-visual SLAM based on geometric features for mobile agents,” in 2023 IEEE Int. Conf. Robot. Biomimetics, 2023, pp. 1–8.

[^18]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM," in Proc. Eur. Conf. Comput. Vis., Sep. 2014, pp. 834–849.

[^19]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in 2014 IEEE Int. Conf. Robot. Automat., 2014, pp. 15–22.

[^20]: J. Jiang, J. Yuan, X. Zhang, and X. Zhang, "DVIO: An optimization-based tightly coupled direct visual-inertial odometry," IEEE Trans. Ind. Electron., vol. 68, no. 11, pp. 11212–11222, Nov. 2021.

[^21]: Y.-S. Shin, Y. S. Park, and A. Kim, “DVL-SLAM: Sparse depth enhanced direct visual-LiDAR SLAM,” Auton. Robots, vol. 44, no. 2, pp. 115–130, Jan. 2020.

[^22]: R. Gomez-Ojeda, J. Briales, and J. Gonzalez-Jimenez, "PL-SVO: Semi-direct monocular visual odometry by combining points and line segments," in 2016 IEEE/RSJ Int. Conf. Intell. Robots Syst., Oct. 2016, pp. 4211–4216.

[^23]: B. Fang, Q. Pan, and H. Wang, “Direct monocular visual odometry based on LiDAR vision fusion*,” in 2023 WRC Symp. Adv. Robot. Automat., Aug. 2023, pp. 256–261.

[^24]: P. Bergmann, R. Wang, and D. Cremers, “Online photometric calibration of auto exposure video for realtime visual odometry and SLAM,” IEEE Robot. Automat. Lett., vol. 3, no. 2, pp. 627–634, Apr. 2018.

[^25]: Z. Feng, J. Li, L. Zhang, and C. Chen, “Online spatial and temporal calibration for monocular direct visual-inertial odometry,” Sensors, vol. 19, no. 10, May 2019, Art. no. 2273.

[^26]: J. Shi and Tomasi, “Good features to track,” in 1994 Proc. IEEE Conf. Comput. Vis. Pattern Recognit., 1994, pp. 593–600.

[^27]: I. Suárez, J. M. Buenaposada, and L. Baumela, “ELSED: Enhanced line segment drawing,” Pattern Recognit., vol. 127, Jul. 2022, Art. no. 108619.

[^28]: J. Sola, “Quaternion kinematics for the error-state Kalman filter,” 2017, arXiv:1711.02508.

[^29]: C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online LiDAR odometry,” IEEE Robot. Automat. Lett., vol. 7, no. 3, pp. 8518–8525, Jul. 2022.

[^30]: C. Zheng et al., "FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry," 2024, arXiv:2408.14035.