APGC-LVIW: LiDAR-visual-inertial-wheel SLAM with adaptive planar geometric constraints

Abstract

This paper proposes a novel multisensor fusion ground SLAM system that integrates LiDAR, visual, IMU, and wheel encoder data to enhance localization and mapping accuracy in complex environments. The proposed system consists of a LiDAR subsystem and a visual subsystem. To fully leverage the complementary advantages of LiDAR and visual sensors, the two subsystems provide each other with initial values for optimization. LiDAR supplies high-precision depth information to the visual subsystem, while the visual subsystem assists the LiDAR module in motion distortion correction, thereby enhancing the overall accuracy of state estimation. In addition, a novel plane extraction method is proposed based on adaptive dynamic distance thresholds and normal vector consistency verification. And the planar factor is introduced into the LiDAR subsystem for joint optimization. Constraining continuous planar structures (such as the ground and walls) reduces matching ambiguity, which further decreases local map noise and improves mapping accuracy. The proposed method has been extensively evaluated in complex urban campus scenarios, and the results show the effectiveness and accuracy of the algorithm.

Introduction

In recent years, simultaneous localization and mapping (SLAM) has gained widespread attention and experienced rapid advancement. SLAM has become indispensable for various robot navigation tasks due to its ability to estimate poses and reconstruct maps in real time. ¹ The mainstream methods primarily consist of LiDAR-based, vision-based, and LiDAR-vision fusion approaches.

Visual SLAM

In vision-based methods, purely visual SLAM^2–4 algorithms, which rely solely on cameras as sensors, offer advantages like being lightweight and cost-effective, and have been widely applied over the past decade. Meanwhile, due to the inability of monocular cameras to recover metric scale, a common solution is to incorporate IMU sensors. An IMU supplies high-frequency inertial measurements, while a camera captures information-rich images. These two sensors complement each other to form a visual-inertial odometry (VIO) system. According to the estimation strategy, VIO can be categorized into two categories: filter-based methods and optimization-based methods.

Filter-based methods^5–7 typically use a Kalman Filter ⁸ and its extended forms to propagate system states. State propagation is accomplished using the IMU kinematic model, while visual updates offer multi-frame constraints. Optimization-based methods^9,10 employ a sliding window optimization framework that jointly estimates system states by constructing a multi-modal constrained factor graph. Specifically, a nonlinear least squares optimization problem incorporating visual reprojection constraints, IMU pre-integration constraints is formulated within the sliding window. To address the challenge of precise picking point localization in cluttered lychee clusters, Fcaf3d-lychee ¹¹ enhances the 3D detection backbone with a visual attention mechanism, achieving exceptional precision.

However, the sensitivity of visual SLAM algorithms to illumination changes, as well as the difficulty in extracting sufficient feature points in texture-sparse environments, can still easily result in divergence or failure of localization.

LiDAR SLAM

Owing to its capability of accurately measuring point-wise distances and spatial coordinates, LiDAR offers distinct advantages in point cloud registration and high-accuracy motion estimation. Compared with visual SLAM, LiDAR SLAM directly acquires accurate depth information via LiDAR sensors. However, purely LiDAR SLAM^12,13 relies on sufficient environmental characteristics during the initialization phase, otherwise, it is prone to failure. Therefore, integrating IMU sensors to assist with distortion correction and provide initial values is a common solution. This sensor fusion approach forms a LiDAR-inertial odometry (LIO) system.

Similarly, LIO can also be categorized into filter-based methods and optimization-based methods. Although state-of-the-art filter-based SLAM methods^14–16 can achieve highly accurate and stable localization over short durations, their inability to rectify accumulated pose errors makes them unsuitable for applications requiring globally consistent mapping. While optimization-based LiDAR SLAM methods^17,18 achieve high-precision global consistency through factor graph optimization, they are highly sensitive to initialization and prone to getting trapped in local minima under aggressive motion or in feature-scarce environments.

In addition, the performance of LiDAR-based SLAM often degrades significantly in environments with insufficient geometric constraints, such as narrow corridors, tunnels, or large open flat areas.

LiDAR-visual-inertial SLAM

Recently, LiDAR-visual-inertial systems have attracted growing attention owing to their robustness when one sensor fails or is partially degenerated. ¹⁹ VIL-SLAM ²⁰ employs a loosely-coupled method without leveraging the joint optimization of LiDAR, camera, and IMU measurements. The LIMO framework ²¹ establishes cross-modal associations by leveraging LiDAR point clouds to geometrically constrain visual features, subsequently performing keyframe-based motion estimation through bundle adjustment optimization with geometric consistency constraints. In, ²⁰ a cascaded architecture is employed to integrate tightly-coupled stereo visual-inertial odometry, LiDAR odometry, and a LiDAR-based loop-closure module. LIC-Fusion ²² implements a tightly-coupled fusion strategy within the MSCKF ⁵ framework, which synchronously integrates heterogeneous sensor modalities including IMU kinematic constraints, sparse visual feature tracking, and LiDAR geometric features. The enhanced LIC-Fusion2.0 ²³ framework refines LiDAR-based pose estimation through sliding-window constrained planar feature association, where consecutive LiDAR scans are processed using plane parameterization and geometric consistency verification within a factor graph optimization architecture. R2LIVE ²⁴ establishes a unified probabilistic framework combining LiDAR, camera, and IMU measurements via manifold-based iterated Kalman filtering. ⁸ Specifically, its VIO component leverages keyframe-based sliding window optimization with geometric verification to achieve robust visual feature triangulation through maximum a posteriori estimation. R3LIVE ²⁵ employs a hierarchical architecture where LIO establishes the geometric representation of the global map, while VIO concurrently populates photometric attributes through texture rendering. These complementary subsystems maintain joint state estimation via tightly-coupled fusion of heterogeneous sensor streams. R3LIVE++ ²⁶ implements a self-adaptive radiometric compensation framework with real-time exposure parameter estimation and proactive photometric calibration, ²⁷ enabling radiometrically consistent reconstruction of environmental radiation properties across varying illumination conditions. LVI-SAM ²⁸ implements a tightly-integrated multi-modal fusion architecture within a factor graph-based optimization framework, where LiDAR geometric constraints, visual feature observations, and IMU kinematic models are jointly optimized through smoothing-based state estimation. Specifically, its visual-inertial subsystem executes sparse feature tracking while establishing cross-modal depth associations through LiDAR-derived geometric verification. Fast-LIVO ²⁹ and Fast-LIVO2 ¹ implements a tightly-coupled framework through singular error-state iterative Kalman filtering (ESIKF), which can be updated by both LiDAR and visual observations. SR-LIVO ³⁰ enhances LiDAR scan segmentation by reconstructing point clouds to align with camera timestamps. Once a relatively accurate pose estimation is obtained using LIO, it integrates visual updates, enhancing both pose accuracy and processing efficiency. The study ³¹ addressed the limitation of locating 3D cracks by introducing an automated detection system that integrates LiDAR and camera data, achieving sub-millimeter measurement precision for 3D structural health monitoring.

In addition, wheel encoder, as a low-cost high-frequency motion sensor, provides stable measurements in GNSS-denied environments. Current LiDAR-visual-inertial fusion methodologies predominantly neglect systematic integration of wheel encoders. Besides, there are a large number of planar features in actual environments, and using continuous planar structures in the scene (such as floors and walls) to construct planar constraints can significantly reduce matching ambiguity in degraded environments. To address these situation, we propose a novel optimization framework for multimodal sensors (LiDAR-Camera-IMU-Wheel) with adaptive planar constraints. The specific contributions are as follows:

• A novel multimodal sensor (LiDAR-Camera-IMU-Wheel) fusion optimization framework is proposed, which jointly optimizes visual reprojection residuals, IMU preintegration residuals, wheel encoder preintegration residuals, and LiDAR odometry residuals through factor graphs.

• A novel plane extraction method based on adaptive dynamic distance thresholds and normal vector consistency verification is proposed. Additionally, planar factors are integrated into the backend optimization process of the LiDAR subsystem. By imposing constraints derived from planar structures, local map noise is effectively suppressed, and the geometric consistency of the reconstructed environment is rigorously maintained, thereby improving overall mapping accuracy.

• Extensive validation in complex urban environments, demonstrating superior localization accuracy and system robustness compared to state-of-the-art SLAM methods.

Methodology

System overview

The overview of the proposed system is shown in Figure 1, with input sensors including LiDARs, RGBD cameras, IMUs, and wheel encoders, while GNSS is optional. The system is primarily composed of two subsystems: the visual subsystem and the LiDAR subsystem. The visual subsystem takes input camera images, IMU measurements, and wheel encoder measurements, with LiDAR data being optionally input. This subsystem achieves state estimation by jointly minimizing visual reprojection residuals, IMU preintegration ³² residuals, and wheel encoder preintegration residuals. The LiDAR subsystem extracts geometric features from point clouds and constructs a LiDAR odometry by matching the features of the current frame to a local map, thereby providing high-precision pose estimation. The back-end employs a factor graph optimization framework, utilizing iSAM2 ³³ to optimize the system states jointly. Constraints integrated into the optimization include IMU preintegration factors, LiDAR odometry factors, and loop closure factors.

Figure 1.

System overview.

Two subsystems solve the state estimation problem using optimization-based methods, where the problem is formulated as a Maximum A Posteriori (MAP) problem. During this formulation, it is assumed that the measurements from different sensors are independent and that their noise follows zero-mean Gaussian distributions. Based on these assumptions, the MAP problem is further converted into a cost minimization problem, where each cost corresponds to the residual of a specific sensor measurement:

$$\begin{array}{c}{\mathcal{X}}^{★}=\underset{\mathcal{X}}{\arg \max }p(\mathcal{X}\mid \mathbf{z})\hfill \\ \multicolumn{1}{c}{=\underset{\mathcal{X}}{\arg \max }p\left(\mathcal{X}\right)p(\mathbf{z}\mid \mathcal{X})}\\ \multicolumn{1}{c}{=\underset{\mathcal{X}}{\arg \max }p\left(\mathcal{X}\right)\underset{i=1}{\overset{n}{\Pi }}p({\mathbf{z}}_i\mid \mathcal{X})}\\ \multicolumn{1}{c}{=\underset{\mathcal{X}}{\arg \min }\{\parallel {\mathbf{r}}_p-{\mathbf{H}}_p\mathcal{X}{\parallel }^2+\sum _{i=1}^n\parallel \mathbf{r}({\mathbf{z}}_i,\mathcal{X}){\parallel }_{{\mathbf{P}}_i}^2\}.}\end{array}$$

(1)

Where ${\mathbf{z}}_i$ stands for the measurements of the $i$ th sensor. $\{{\mathbf{r}}_p,{\mathbf{H}}_p\}$ is the prior information from marginalization. $\mathbf{r}(·)$ denotes the residuals corresponding to each sensor measurement. $\parallel ·{\parallel }_{\mathbf{P}}$ represents the Mahalanobis norm.

Visual subsystem

The visual subsystem performs an optimization process within a sliding window framework, where the robot state $\mathbf{x}$ can be defined as:

$$\mathbf{x}=[\mathbf{R},\mathbf{p},\mathbf{v},\mathbf{b}].$$

(2)

Where $\mathbf{R}\in \mathrm{SO}\left(3\right)$ is the rotation matrix, $\mathbf{p}\in {\mathbb{R}}^3$ is the position vector, $\mathbf{v}$ is the speed, and $\mathbf{b}$ is the IMU bias. The transformation $\mathbf{T}\in \mathrm{SE}\left(3\right)$ from $\mathbf{B}$ to $\mathbf{W}$ is represented as $\mathbf{T}=[\mathbf{R}\mid \mathbf{p}]$ .

Improvements in depth estimation and initialization

The processing pipeline of the visual subsystem is improved upon the Ground-Fusion algorithm, ³⁴ which is a low-cost SLAM framework that tightly couples RGBD-IMU-Wheel-GNSS sensors. The more detailed improvements are as follows:

• 1) Image Depth Estimation: In our visual subsystem, LiDAR measurements are optional input. During the state estimation process of visual odometry, accurate depth information is critical for recovering the camera’s 3D motion trajectory from 2D images. To enhance the accuracy of depth estimation, we employ a strategy that incorporates three different depth sources: firstly, when the LiDAR subsystem is operating normally, LiDAR-provided depth information is prioritized due to its superior measurement accuracy compared to other sensors; secondly, if the LiDAR becomes unavailable or fails, the system switches to using depth data obtained from RGBD cameras. That is, the 2D features tracked in the image are combined with the pixel values at the same locations in the depth image and the two are aligned; finally, when neither of the aforementioned sources is available, the system utilizes IMU sensors for depth estimation. Within this framework, the system dynamically selects the depth source based on the operational status of the LiDAR subsystem, ensuring reliable depth estimation under varying conditions.

• 2) System Initialization: Optimization-based methods are highly dependent on accurate system initialization. The optimization process relies on an initial state to commence iterative solving. If the initial values are inaccurate or deviate significantly from the true values, the back-end optimization may become trapped in local minima, leading to substantial errors in state estimation or even failure to converge to the correct solution. Furthermore, poor initialization can significantly slow the convergence rate of the optimization process. This issue is particularly pronounced in complex environments or large-scale scenarios, ultimately compromising the accuracy and robustness of the system. In the original system, the initialization process often failed under conditions of low acceleration excitation. To enhance the accuracy and robustness of the visual subsystem’s initialization, we utilize the system state $\mathbf{x}$ and IMU biases $\mathbf{b}$ , obtained from the back-end optimization of the LiDAR subsystem, as the initial estimates for the visual subsystem when the LiDAR subsystem is operational. Specifically, the optimized states are mapped to the corresponding keyframes of the visual subsystem based on the image timestamps, serving as the initial guesses for the visual state estimation.

Constraint factors

• 1) IMU Preintegration Factor: In the modeling of the IMU preintegration factor, the measurements involved include platform biases, noisy linear acceleration and angular velocity. Additionally, the acceleration needs to contain the gravity component. Therefore, the IMU’s acceleration and angular velocity measurements ³² can be modeled as:

$$\begin{array}{c}{\tilde{\mathbf{a}}}_t={\mathbf{a}}_t+{\mathbf{b}}_{a_t}+{\mathbf{R}}_w^{b_t}{\mathbf{g}}^w+{\mathbf{n}}_a\hfill \\ \multicolumn{1}{c}{{\tilde{\omega }}_t={\mathbf{\omega }}_{\mathbf{t}}+{\mathbf{b}}_{w_t}+{\mathbf{n}}_w.}\end{array}$$

(3)

Where $\{{\mathbf{a}}_t,{\mathbf{\omega }}_{\mathbf{t}}\}$ is the real linear acceleration and angular velocity in IMU sensor at time $t$ . $\{{\tilde{\mathbf{a}}}_t,{\tilde{\omega }}_t\}$ is the raw measurement of accelerometer and gyroscope at time $t$ . Assumed that ${\mathbf{n}}_a$ and ${\mathbf{n}}_{\omega }$ to be zero-mean Gaussian distributed, ${\mathbf{n}}_{b_a}~\mathcal{N}(0,{\mathbf{\Sigma }}_{b_a}),{\mathbf{n}}_{b_{\omega }}~\mathcal{N}(0,{\mathbf{\Sigma }}_{b_{\omega }})$ . The biases of the accelerometer and gyroscope are modeled as a random walk as follows:

$${\stackrel{·}{\mathbf{b}}}_{a_t}={\mathbf{n}}_{b_a},{\stackrel{·}{\mathbf{b}}}_{w_t}={\mathbf{n}}_{b_w},$$

(4)

In the process of nonlinear optimization, the state variables are iteratively updated, which requires re-integrating the IMU measurements after each update. To improve the efficiency of the overall optimization process, we employ the preintegration method, where the integration results between time instants $k$ and $k+1$ are precomputed and used as constraint factors in the back-end. The preintegration is defined as follows:

$$\begin{array}{c}{\mathit{\alpha }}_{b_{t_{k+1}}}^{b_{t_k}}=\int {\int }_{t\in [t_k,t_{k+1}]}{\mathbf{R}}_{b_t}^{b_{t_k}}({\tilde{\mathbf{a}}}_t-{\mathbf{b}}_{a_t})d{t}^2\hfill \\ \multicolumn{1}{c}{{\mathit{\beta }}_{b_{t_{k+1}}}^{b_{t_k}}={\int }_{t\in [t_k,t_{k+1}]}{\mathbf{R}}_{b_t}^{b_{t_k}}({\tilde{\mathbf{a}}}_t-{\mathbf{b}}_{a_t})\mathrm{dt}}\\ \multicolumn{1}{c}{{\mathit{\gamma }}_{b_{t_{k+1}}}^{b_{t_k}}={\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\mathbf{\Omega }({\tilde{\mathit{\omega }}}_t-{\mathbf{b}}_{w_t}){\mathbf{\gamma }}_{b_t}^{b_{t_k}}\mathrm{dt}.}\end{array}$$

(5)

Where $b_k$ stands for the body frame at time $t_k$ . $\{\alpha ,\beta ,\gamma \}$ represents the relative change in position, velocity, and rotation between the $k$ and $k+1$ moments. If the estimated bias undergoes significant changes, the preintegration process is repropagated using the updated bias value. $\mathbf{\Omega }\left(\mathit{\omega }\right)$ is defined as follows:

$$\mathbf{\Omega }\left(\mathit{\omega }\right)=\left[\begin{array}{cc}-{\lfloor \mathit{\omega }\rfloor }_{\times }\hfill & \mathit{\omega }\hfill \\ \multicolumn{1}{c}{-{\mathit{\omega }}^T}& 0\hfill \end{array}\right],{\lfloor \mathit{\omega }\rfloor }_{\times }=\left[\begin{array}{ccc}0\hfill & -{\omega }_z\hfill & {\omega }_y\hfill \\ \multicolumn{1}{c}{{\omega }_z}& 0\hfill & -{\omega }_x\hfill \\ \multicolumn{1}{c}{-{\omega }_y}& {\omega }_x\hfill & 0\hfill \end{array}\right].$$

(6)

Finally, the IMU preintegration residuals ⁹ can be expressed as:

$$\begin{array}{c}{\mathbf{r}}_{\mathcal{B}}({\tilde{\mathbf{z}}}_{b_{t_{k+1}}}^{b_{t_k}},\mathcal{X})=\left[\begin{array}{c}\delta {\mathit{\alpha }}_{b_{t_{k+1}}}^{b_{t_k}}\hfill \\ \multicolumn{1}{c}{\delta {\mathit{\beta }}_{b_{t_k}}^{b_{t_{k+1}}}}\\ \multicolumn{1}{c}{\delta {\mathit{\theta }}_{b_{t_{k+1}}}^{b_{t_{k+1}}}}\\ \multicolumn{1}{c}{\delta {\mathbf{b}}_a}\\ \multicolumn{1}{c}{\delta {\mathbf{b}}_g}\end{array}\right]\hfill \\ \multicolumn{1}{c}{=\left[\begin{array}{c}{\mathbf{R}}_w^{b_{t_k}}({\mathbf{p}}_{b_{t_{k+1}}}^w-{\mathbf{p}}_{b_{t_k}}^w+\frac{1}{2}{\mathbf{g}}^w\mathrm{\Delta }{t}_k^2-{\mathbf{v}}_{b_{t_k}}^w\mathrm{\Delta }{t}_k)-{\hat{\mathit{\alpha }}}_{b_{t_{k+1}}}^{b_{t_k}}\hfill \\ \multicolumn{1}{c}{{\mathbf{R}}_w^{b_{t_k}}({\mathbf{v}}_{b_{t_{k+1}}}^w+{\mathbf{g}}^w\mathrm{\Delta }{t}_k-{\mathbf{v}}_{b_{t_k}}^w)-{\hat{\mathit{\beta }}}_{b_{t_{k+1}}}^{b_{t_k}}}\\ \multicolumn{1}{c}{2{[{\mathbf{q}}_{b_{t_k}}^{w^{-1}}\otimes {\mathbf{q}}_{b_{t_{k+1}}}^w\otimes {\left({\hat{\gamma }}_{b_{t_{k+1}}}^{b_{t_k}}\right)}^{-1}]}_{\mathrm{xyz}}}\\ \multicolumn{1}{c}{{\mathbf{b}}_{a{b}_{t_{k+1}}}-{\mathbf{b}}_{a{b}_{t_k}}}\\ \multicolumn{1}{c}{{\mathbf{b}}_{w{b}_{t_{k+1}}}-{\mathbf{b}}_{w{b}_{t_k}}}\end{array}\right].}\end{array}$$

(7)

• 2) Visual Reprojection Residual Factor: In our system, visual measurements are represented as a set of feature points extracted from image frames. Specifically, prominent corners within the image are detected as feature points using the Harris corner detector, ³⁵ and their temporal correspondence is established through tracking based on the iterative Lucas and Kanade optical flow method. ³⁶ The process of projection can be modeled as:

$$\tilde{\mathcal{P}}={\pi }_c({\mathbf{R}}_b^c({\mathbf{R}}_w^b{\mathbf{x}}^w+{\mathbf{p}}_w^b)+{\mathbf{p}}_b^c)+{\mathbf{n}}_c.$$

(8)

Where $\tilde{\mathcal{P}}=[u,v{]}^T$ is the pixel coordinates of the image. ${\pi }_c(·)$ represents the camera projection function and $n_c$ is the sensor measurement noise. For the $l$ th feature point that is first detected in the $i$ th image frame, its corresponding visual reprojection residuals in the $j$ th image frame are as follows:

$${\mathbf{r}}_{\mathcal{C}}({\tilde{\mathbf{z}}}_l,\mathcal{X})={\tilde{\mathcal{P}}}_l^{c_{t_j}}-{\pi }_c\left({\hat{\mathbf{x}}}_l^{c_{t_j}}\right).$$

(9)

$$\begin{array}{c}\begin{array}{c}{\hat{\mathbf{x}}}_l^{c_{t_j}}={\mathbf{R}}_b^c({\mathbf{R}}_w^{b_{t_j}}({\mathbf{R}}_{b_{t_i}}^w({\mathbf{R}}_c^b\frac{1}{{\rho }_l}{\pi }_c^{-1}\left({\tilde{\mathcal{P}}}_l^{c_{t_i}}\right)+{\mathbf{p}}_c^b)\hfill \\ \multicolumn{1}{c}{+{\mathbf{p}}_{b_{t_i}}^w)+{\mathbf{p}}_w^{b_{t_j}})+{\mathbf{p}}_b^c.}\end{array}\hfill \end{array}$$

(10)

Where $\{R_b^c,t_c^b\}$ represents the transformation matrix between camera and IMU and ${\rho }_l$ represents the inverse depth in frame $i$ .

• 3) Wheel Preintegration Factor: For ground-based mobile robots, the motion dynamics often fail to meet the requirements for robust visual-inertial fusion. The weak observability of scale and pose during operation can significantly degrade localization accuracy. This limitation is particularly evident in scenarios where the robot’s motion lacks sufficient excitation, such as in low-speed or constrained environments. ³⁷ To address this challenge, ground robots are typically equipped with wheel encoders. These sensors provide reliable motion measurements with an absolute scale, independent of environmental conditions. Unlike vision-based measurements that depend on feature-rich scenes or inertial measurements that rely on sufficient acceleration and angular motion, wheel encoder offers consistent and accurate motion estimation under most operating conditions.

By incorporating wheel encoders into the SLAM framework, the system benefits from enhanced scale observability and more robust pose estimation, particularly in scenarios where visual-inertial methods alone struggle. The integration of wheel encoders as an additional constraint within the optimization process allows for improved localization accuracy and system robustness, even in environments with low texture or minimal motion excitation.

To estimate the motion of the robot from wheel encoder measurements, the following kinematic formulation ³⁸ is developed.

$$\begin{array}{c}\multicolumn{1}{c}{{\stackrel{·}{\mathit{R}}}_O^W={\mathit{R}}_O^W{\mathit{\omega }}^{O^{\wedge }}}\\ \multicolumn{1}{c}{{\stackrel{·}{\mathit{p}}}_O^W={\mathit{v}}_O^W}\end{array}$$

(11)

The equation (11) describes the pose transformation of the wheel encoder coordinate system. By integrating (11), the pose at time $t+\mathrm{\Delta }t$ can be obtained as follows.

$$\begin{array}{c}{\mathit{R}}_O^W(t+\mathrm{\Delta }t)={\mathit{R}}_O^W\left(t\right)\mathrm{Exp}\left({\int }_t^{t+\mathrm{\Delta }t}{\mathit{\omega }}^O\left(\tau \right)d\tau \right)\hfill \\ \multicolumn{1}{c}{{\mathit{p}}_O^W(t+\mathrm{\Delta }t)={\mathit{p}}_O^W\left(t\right)+{\int }_t^{t+\mathrm{\Delta }t}{\mathit{v}}_O^W\left(\tau \right)d\tau }\end{array}$$

(12)

The wheel encoder can provide motion measurement between frames. Based on the midpoint integration, we can derive the wheel encoder measurement model as follows:

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{R}}_O^W(t+\mathrm{\Delta }t)={\mathit{R}}_O^W\left(t\right)\mathrm{Exp}\left({\overline{\mathit{\omega }}}^O\mathrm{\Delta }t\right)}\\ \multicolumn{1}{c}{{\mathit{p}}_O^W(t+\mathrm{\Delta }t)={\mathit{p}}_O^W\left(t\right)+{\overline{\mathit{v}}}_O^W\mathrm{\Delta }t.}\end{array}$$

(13)

Where $O$ represents the coordinate of the wheel odometer. ${\overline{\mathit{\omega }}}^O$ and ${\overline{\mathit{v}}}_O^W$ represent the averages of the measurements at time $t$ and $t+\mathrm{\Delta }t$ :

$$\begin{array}{c}\multicolumn{1}{c}{{\overline{\mathit{\omega }}}^O=\frac{1}{2}({\mathit{\omega }}^O\left(t\right)+{\mathit{\omega }}^O(t+\mathrm{\Delta }t))}\\ \multicolumn{1}{c}{{\overline{\mathit{v}}}_O^W=\frac{1}{2}({\mathit{v}}_O^W\left(t\right)+{\mathit{v}}_O^W(t+\mathrm{\Delta }t))}\\ \multicolumn{1}{c}{=\frac{1}{2}({\mathit{R}}_O^W\left(t\right){\mathit{v}}^O\left(t\right)+{\mathit{R}}_O^W(t+\mathrm{\Delta }t){\mathit{v}}^O(t+\mathrm{\Delta }t)).}\end{array}$$

(14)

Similar to IMU preintegration, the wheel encoder measurements between keyframe $k=i$ and keyframe $k=j$ can be integrated into a composite measurement, which imposes a motion constraint between the two consecutive keyframes. The final wheel encoder preintegration measurement model is as follows:

$$\begin{array}{c}\multicolumn{1}{c}{\mathrm{\Delta }{\tilde{\mathit{p}}}_{O_j}^{O_i}={\mathit{R}}_{\mathrm{Oi}}^{W^T}({\mathit{p}}_{\mathrm{Oj}}^W-{\mathit{p}}_{\mathrm{Oi}}^W)+\delta {\mathit{p}}_{\mathrm{ij}}}\\ \multicolumn{1}{c}{\mathrm{\Delta }{\tilde{\mathit{R}}}_{O_j}^{O_i}={\mathit{R}}_{\mathrm{Oi}}^{W^T}{\mathit{R}}_{\mathrm{Oj}}^W\mathrm{Exp}\left(\delta {\mathit{\theta }}_{\mathrm{ij}}\right).}\end{array}$$

(15)

Where $[\delta p_{\mathrm{ij}},\delta {\theta }_{\mathrm{ij}}]$ is the random noise.

Based on the wheel encoder preintegration measurement model (15), the wheel preintegration residual can be given by:

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{r}}_{\mathrm{\Delta }{\mathit{R}}_j^{o_i}}=\mathrm{Log}\left(\mathrm{\Delta }{\tilde{\mathit{R}}}_{O_j}^{O_i^T}{\mathit{R}}_{O_i}^{W^T}{\mathit{R}}_{O_j}^W\right)}\\ \multicolumn{1}{c}{{\mathit{r}}_{p_{O_j}}={\mathit{R}}_{\mathrm{Oi}}^{W^T}({\mathit{p}}_{\mathrm{Oj}}^W-{\mathit{p}}_{\mathrm{Oi}}^W)-\mathrm{\Delta }{\tilde{\mathit{p}}}_{O_j}^{O_i}.}\end{array}$$

(16)

Where $p_O^W=R_B^W{p}_O^B,R_O^W=R_B^W{R}_O^B$ .

Therefore, the final wheel preintegration residual, expressed as a function of the system state, is given by:

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{r}}_{\mathrm{\Delta }{\mathit{R}}_{o_j}^{o_i}}=\mathrm{Log}\left(\mathrm{\Delta }{\tilde{\mathit{R}}}_{O_j}^{O_i^T}{\left({\mathit{R}}_{\mathrm{Bi}}^W{\mathit{R}}_O^B\right)}^T{\mathit{R}}_{\mathrm{Bj}}^W{\mathit{R}}_O^B\right)}\\ \multicolumn{1}{c}{{\mathit{r}}_{\mathrm{\Delta }{p}_{o_j}^{o_i}}={\left({\mathit{R}}_{\mathrm{Bi}}^W{\mathit{R}}_O^B\right)}^T({\mathit{R}}_{\mathrm{Bj}}^W{\mathit{p}}_O^B+{\mathit{p}}_{\mathrm{Bj}}^W}\\ \multicolumn{1}{c}{-{\mathit{R}}_{\mathrm{Bi}}^W{\mathit{p}}_O^B-{\mathit{p}}_{\mathrm{Bi}}^W)-\mathrm{\Delta }{\tilde{\mathit{p}}}_{O_j}^{O_i}.}\end{array}$$

(17)

Where $[R_O^B,p_O^B]$ is the known extrinsic parameter between IMU and wheel odometer.

LiDAR subsystem

The LiDAR subsystem is an enhancement based on the LIO-SAM algorithm. ¹⁷ The optimization variables in the system are same to those used in the visual subsystem. The system retains the factor graph used for global pose optimization. In this factor graph, three types of constraints are incorporated: IMU preintegration factor, LiDAR odometry factor, and loop closure factor. These constraints are then jointly optimized to refine the system’s state estimate. The IMU preintegration constraints account for the motion between consecutive time steps, while the LiDAR odometry constraints provide relative pose estimates between keyframes or consecutive frames. Additionally, loop closure constraints, which are derived from detecting revisited locations, help to correct drift and improve the global consistency of the trajectory.

By formulating the constraints in a factor graph framework, the system can simultaneously optimize the states associated with the IMU and LiDAR sensors, while also minimizing the error in the loop closure detection. This approach ensures that all available sensor information is efficiently utilized, leading to a more accurate and robust global trajectory estimation.

LiDAR odometry

Upon the arrival of a new LiDAR scan, we first perform feature extraction to identify key elements within the point cloud. The extraction process involves evaluating the roughness of points within a local neighborhood to distinguish between edge and planar features. By classifying points into edge and planar features based on local roughness thresholds, we can effectively segment the LiDAR scan into distinct feature types that are more suitable for matching and alignment tasks.

After extracting features from the current LiDAR scan, a local map is constructed by identifying keyframes within the sliding window that are spatially and temporally close to the current frame. This local map provides a reference for scan-to-map matching, which utilizes both edge and planar features for precise alignment. Following the design principles of the LIO-SAM framework, we adopt its foundational structure while implementing a series of enhancements tailored to improve system performance and robustness. The more details are as follows:

• 1) Wheel Preintegration Factor: Due to the presence of the visual subsystem, the initial values required for optimizing the backend of the LiDAR subsystem can be obtained not only from the IMU, but also through results derived from the backend optimization of the visual subsystem. These initial values are then propagated to the LiDAR subsystem to enhance the accuracy and efficiency of its optimization process. Because the initial system state provided by the visual subsystem, optimized through the factor graph containing various constraints, is more accurate compared to the state estimated by the IMU.

• 2) Plane Factor: To enhance the robustness of point cloud registration, we propose a plane extraction method incorporating dynamic threshold adaptation and normal vector constraints. After extracting the initial planar features through curvature, an improved RANSAC algorithm is used for multi plane segmentation based on this. For the $k$ -th iteration, the plane parameters ${\mathit{\pi }}_k=(a_k,b_k,c_k,d_k)$ are estimated by minimizing the sum of absolute distances for inliers:

$$\underset{{\mathbf{\pi }}_{\mathbf{k}}}{\min }\sum _{i\in {\mathcal{I}}_k}\frac{|a_k{x}_i+b_k{y}_i+c_k{z}_i+d_k|}{\sqrt{a_k^2+b_k^2+c_k^2}}.$$

(18)

Where ${\mathcal{I}}_k$ denotes the inlier index set.

To handle varying scene scales, we dynamically adjust the distance threshold based on the point cloud centroid:

$${\tau }_d=\alpha ·\Vert \mathit{\mu }{\Vert }_2,\mathit{\mu }=\frac{1}{N}\sum _{i=1}^N{(x_i,y_i,z_i)}^T.$$

(19)

Where $\alpha$ is the scaling factor.

For the ground, we enforce directional constraints to select physically meaningful planes:

$${\mathit{n}}_k·{\mathit{n}}_{\mathrm{ref}}>\gamma .$$

(20)

Where ${\mathit{n}}_k={(a_k,b_k,c_k)}^T/\sqrt{a_k^2+b_k^2+c_k^2}$ is the normalized normal vector, ${\mathit{n}}_{\mathrm{ref}}$ denotes reference direction, and $\gamma$ is the angular threshold.

Finally, the plane quality metric is defined as the mean absolute distance error of inliers:

$${\mathcal{E}}_k=\frac{1}{\left|{\mathcal{I}}_k\right|}\sum _{i\in {\mathcal{I}}_k}|a_k{x}_i+b_k{y}_i+c_k{z}_i+d_k|.$$

(21)

A plane is accepted as a valid constraint factor only when ${\mathcal{E}}_k<0.05$ is satisfied.

We have conducted tests on multiple sequences by adjusting $\alpha$ (within 0.1–0.3) and $\gamma$ (within 0.75–0.95) while keeping other settings consistent. The experimental results show that the localization accuracy (measured by ATE) varies by less than 5% across different parameter combinations.

Experiment

To comprehensively evaluate the accuracy and efficiency of the proposed algorithm, APGC-LVIW was tested on the M2DGR-PLUS ³⁴ and M2DGR. ³⁹ The M2DGR datasets contain various challenging urban campus scenarios, such as weakly-textured, open areas, and moving obstacles. The M2DGR-PLUS datasets are the Extension and update of M2DGR, which contain the scenarios of degraded bridge decks, indoor-outdoor transitions, complex street environments, etc. More details of the specifications of all sensors are summarized in Table 1.

Table 1.

Details of benchmark datasets.

Sensor		M2DGR-PLUS	M2DGR
LiDAR	Type	Robosense RS-16	Velodyne VLP-32C
	Frequency	5 Hz	10 Hz
IMU	Type	Built in IMU of Intel RealSense D435i	Handsfree
	Frequency	200 Hz	150 Hz
Camera	Type	Realsense d435i	Realsense d435i
	Frequency	15 Hz	15 Hz
Wheel	Type	WHEELTEC	*
	Frequency	20 Hz	*

*

indicates that there is no Wheel sensor in the M2DGR dataset.

On these two datasets, APGC-LVIW was compared with several state-of-the-art SLAM methods: LIO-SAM, ¹⁷ VINS-Mono, ⁹ LINS, ⁴⁰ Odom (The wheel odometry in Ground-Fusion. ³⁴ ), FAST-LIO2, ¹⁴ R3LIVE, ²⁵ FAST-LIVO, ²⁹ Ground-Fusion, ³⁴ LVI-SAM. ²⁸ And the computation platform used for tests is a laptop equipped with an Intel i7-12,700H 2.50 GHz CPU and 32 GB RAM.

Accuracy results

Table 2 shows the Root Mean Square Errors (RMSEs) of absolute trajectory error (ATE) for several algorithms on the M2DGR-PLUS datasets. APGC-LVIW performs best in seven out of nine sequences. Benefiting from the proposed multi-modal sensor fusion framework (LiDAR-Camera-IMU-Wheel Odometry), the system effectively integrates heterogeneous sensor measurements, achieving superior localization accuracy in seven out of nine test sequences.

Table 2.

RMSEs of ATE [ $m$ ] on M2DGR-PLUS datasets.

Sequence	LIO-SAM	VINS-Mono	LINS	Odom	FAST-LIO2	R3LIVE	FAST-LIVO	Ground-Fusion	LVI-SAM	APGC-LVIW (Ours)
Tree	0.226	1.452	0.207	2.789	0.772	*	*	0.594	0.320	0.151
Bridge1	*	1.647	*	0.822	*	*	*	0.491	0.248	0.161
Street1	0.153	0.657	0.191	0.264	0.297	*	1.016	0.198	0.093	0.081
Street2	0.106	0.835	0.106	0.974	0.186	3.135	2.428	0.271	0.095	0.089
Parking1	0.056	0.091	0.052	0.238	0.071	0.090	1.039	0.189	0.057	0.057
Parking2	0.037	0.143	0.038	0.263	0.050	0.468	0.771	0.197	0.038	0.035
Building1	0.278	0.547	0.280	1.607	0.284	1.894	2.444	0.628	0.254	0.241
Building2	0.222	0.491	0.193	0.971	0.374	1.752	1.863	0.252	0.233	0.216
Switch	0.217	1.446	0.451	4.820	0.889	*	7.169	0.785	0.216	0.214

The optimal results are marked in red and the suboptimal results are marked in blue.

*

indicates that the algorithm has degraded in these sequences, resulting in significant accuracy errors.

Notably, in challenging scenarios such as LiDAR-degraded environments (e.g. Bridge1 sequence) and dense urban campus scenes with heavy tree occlusions (e.g. Tree sequence), conventional LiDAR SLAM and existing multi-sensor fusion methods suffer from performance degradation or even localization failure due to sensor limitations or environmental interference. In contrast, our system, enhanced by wheel odometry integration and the optimization strategy, demonstrates significantly improved robustness and precision in degenerate conditions. Experimental results validate that the proposed multi-modal fusion framework exhibits higher reliability and localization accuracy in complex or degraded environments.

To more intuitively and accurately present the accuracy performance of APGC-LVIW, Figures 2 and 3 show the map construction effects and trajectory estimation results on M2DGR-PLUS datasets. As demonstrated by the mapping comparison in Figure 2 for the Tree and Bridge sequences, our system significantly enhances geometric consistency in multi-sensor state estimation by integrating wheel encoder constraints into the backend factor graph optimization framework. Compared with the LVI-SAM, our approach exhibits superior point cloud registration quality at structural boundaries, such as building walls and vehicles. While LVI-SAM shows noticeable point cloud stratification. And Figure 3 demonstrates that the proposed algorithm achieves higher localization accuracy in local regions compared to the other SOTA algorithms.

Figure 2.

Mapping result of APGC-LVIW on the tree sequence and bridge sequence presented from a bird’s eye view. The partial map highlighted by the red boxes is zoomed in on and compared with the LVI-SAM results.

Figure 3.

Comparison of trajectories on tree and switch sequence.

In addition, to validate the effectiveness of the proposed plane extraction method based on adaptive dynamic distance thresholds and normal vector consistency verification, along with the integration of planar factors into LiDAR backend optimization, ablation studies on the M2DGR datasets were conducted by incorporating the methodology into the LVI-SAM framework. As shown in Table 3, the proposed method achieves the highest localization accuracy. And the experimental results fully validate the effectiveness of the planar factor proposed in this paper.

Table 3.

RMSEs of ATE [ $m$ ] on M2DGR datasets.

Sequence	LIO-SAM 17	VINS-Mono 9	FAST-LIVO 30	LVI-SAM 29	APGC-LVIW(Ours)
Gate01	0.202	1.426	9.497	0.148	0.145
Gate02	0.409	*	2.927	0.317	0.315
Gate03	0.112	7.381	2.907	0.112	0.107
Street02	4.063	*	8.521	3.852	3.144
Street03	0.219	7.470	5.358	0.142	0.142
Street04	1.022	*	3.834	0.868	0.775
Street05	0.399	*	4.500	0.383	0.268
Street06	0.417	*	3.405	0.414	0.394

The optimal results are marked in red and the suboptimal results are marked in blue.

*

indicates that the algorithm has degraded in these sequences, resulting in significant accuracy errors.

Ablation study

To quantify the independent contributions of the two core innovations, adaptive planar factor and wheel encoder fusion, we designed three groups of experiments on the M2DGR-PLUS Tree (LiDAR-degraded, heavy occlusion) and Bridge1 (planar-dominant, degraded deck) sequences. The results are shown in Table 4.

Table 4.

Ablation results on M2DGR-PLUS sequences.

Sequence	Group	ATE (RMSE)	Increase
Tree	A (full system)	0.151	-
	B (without planar)	0.166	9.93%
	C (without wheel)	0.221	46.3%
Bridge1	A (full system)	0.161	-
	B (without planar)	0.180	11.8%
	C (without wheel)	0.251	55.9%

Group B (no planar factor) shows significant ATE degradation: In Sequence Tree and Bridge1, the loss of planar constraints increases local map noise (e.g. deck edge misalignment), leading to 9.93% and 46.3% higher ATE. This confirms the planar factor’s role in suppressing geometric ambiguity. Group C (no wheel fusion) exhibits 11.8% and 55.9% ATE increase: Wheel encoders provide absolute scale to mitigate IMU drift in low-excitation motion, so their removal reduces scale observability. Synergy of the two innovations. The full system (Group A) achieves the lowest error, proving that planar constraints and wheel fusion complement each other.

Runtime analysis

In this section, we evaluate the computation efficiency of our proposed method. Table 5 shows the runtime of each module. When the system receives camera images and LiDAR scans, our algorithm executes each module. Experimental results demonstrate that the total computational time remains at 97.937 ms per frame (including 48.65 ms for LiDAR subsystem and 49.287 ms for visual subsystem), which satisfies the real-time requirement for SLAM systems.

Table 5.

Runtime of each module (unit: ms).

Subsystem	Module	Average time
LiDAR	Feature extraction	0.947
	Backend optimization	47.703
Camera	Feature extraction	11.253
	Backend optimization	38.034
Total		97.937

Conclusion

This paper proposed APGC-LVIW, a novel multisensor fusion ground SLAM system that integrates LiDAR, visual, IMU, and wheel encoder data to address the challenges of localization and mapping in complex urban environments. The proposed system achieves high accuracy and robustness by leveraging the complementary strengths of LiDAR and visual sensors through bidirectional initialization and joint optimization. Furthermore, a planar factor based on scene geometric consistency is introduced into the LiDAR subsystem for joint optimization, which enforces geometric consistency on continuous planar structures (e.g. ground and walls), effectively reduces matching ambiguity and suppresses local map noise. This structural constraint not only improves the geometric integrity of the map but also enhances the system’s robustness to dynamic environmental changes. In future research, we plan to conduct more optimization studies on the real-time performance of the system.