Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping

I. INTRODUCTION

• VIL-SLAM combines IMU, stereo cameras, and LiDAR, performing on par with pure LiDAR systems in most cases and better where those systems fail.It integrates stereo VIO and LiDAR mapping with loop closure.Source: §I · p. 1
• Pure LiDAR systems are robust to environmental changes but can fail in repetitive environments such as tunnels and hallways.These settings are challenging for mapping and localization.Source: §I · p. 1

II. RELATED WORK

• Tightly coupled optimization-based VIO approaches jointly optimize visual and inertial information, typically trading higher computation cost for better accuracy and robustness.The paper bounds computation by forming landmarks in a structureless fashion and optimizing a fixed-size pose graph.Source: §II · pp. 1–2
• VIL-SLAM uses tightly coupled VIO to initialize LiDAR mapping, while VLOAM uses loosely coupled IMU and camera; VIL-SLAM also adds LiDAR-enhanced loop closure.The paper notes that VLOAM has a more interactive system for using camera and LiDAR information to correct IMU biases.Source: §II · p. 2

III. SYSTEM OVERVIEW

• The system incrementally optimizes a global pose graph, returning corrected LiDAR poses in real time for map updates and re-localization; dense maps are stitched in post-processing.Loop closure combines visual detection and initial constraint estimation with sparse-point-cloud ICP refinement.Source: §III · p. 2

IV. VISUAL FRONTEND

• The visual frontend tracks features across frames and matches stereo pairs, using KLT tracking and replenishing matches with detected, described, and stereo-matched features when tracking falls below a threshold.Stereo matching on the first pair initializes the system.Source: §IV · p. 2

V. STEREO VISUAL INERTIAL ODOMETRY

• Stereo VIO tracks stereo features and combines stereo matches with IMU measurements in a tightly coupled fixed-lag pose graph.The graph estimates recent states and marginalizes past states, adding prior factors to related variables.Source: §V · pp. 2–3, Figure 3 · p. 3
• The fixed-lag smoother bounds computation by limiting the number of optimized variables, and rejects visual outliers using average stereo and temporal reprojection error.The VIO provides a real-time state estimate at relatively high frequency as the LiDAR mapping motion model.Source: §V · p. 2

A. IMU pre-integration factor

• IMU pre-integration generates relative measurements between states and supports efficient re-linearization during optimization.Its residual includes pose, velocity, and bias terms.Source: A. IMU pre-integration factor · p. 3

B. Structureless vision factor

• The structureless vision model forms image residuals from landmark observations across multiple states, with perspective projection defining predicted measurements.The same formulation is used for the right camera image.Source: B. Structureless vision factor · p. 3
• Projecting residuals into the null space of the landmark Jacobian avoids optimizing landmark positions and yields the structureless vision factor.The passage identifies the projector as an orthogonal projector.Source: B. Structureless vision factor · p. 3

C. Optimization and marginalization

• The pose-graph optimization is formulated as a maximum a posteriori (MAP) problem.Its optimal solution is obtained from the stated residuals.Source: C. Optimization and marginalization · p. 3
• The window contains state variables with prior factors and associated covariance, alongside IMU measurement covariance.The window is the set of state variables being optimized.Source: C. Optimization and marginalization · p. 3
• The optimizer retains the most recent N state variables and marginalizes those leaving the window using Schur complement.Prior factors are then added to related variables inside the window.Source: C. Optimization and marginalization · p. 3

VI. LIDAR MAPPING

• LiDAR mapping extracts points on sharp edges and planar surfaces from each complete-rotation scan.The scan is the point cloud obtained from one complete LiDAR rotation.Source: §VI · p. 3
• High-frequency IMU-rate VIO poses provide the motion prior for LiDAR-point dewarping and scan-to-map registration.Source: §VI · p. 3
• Registration matches current-scan feature points to the map of previous features by minimizing Euclidean-distance residuals.The feature-based registration is solved as an optimization problem.Source: §VI · p. 3

A. LiDAR scan dewarping

• LiDAR points timestamped within a scan are dewarped to the scan-end time using IMU-rate VIO poses.The poses are transformed from the closest IMU-rate VIO poses.Source: A. LiDAR scan dewarping · pp. 3–4

B. Scan to map registration

• The initial scan-to-map pose propagates the previous optimized LiDAR pose using the relative transform from IMU-rate VIO poses.Dewarped feature points are transformed into world coordinates for registration.Source: B. Scan to map registration · p. 4
• Registration minimizes feature residuals, including surface-point distance to a patch formed by three closest map points, using Levenberg–Marquardt.The nonlinear cost stacks feature-point functions.Source: B. Scan to map registration · p. 4

VII. LIDAR ENHANCED LOOP CLOSURE

• Loop closure incorporates loop constraints and LiDAR mapping transformations into global pose-graph optimization, then returns corrected LiDAR poses in real time for new-scan registration.The global graph is optimized with the incremental iSAM2 solver.Source: §VII · p. 4

A. Loop detection

• Visual Bag-of-Words detects candidate key-image revisits, with a time-threshold restriction and left-image descriptor matching used to filter false positives.Source: A. Loop detection · p. 4
• Figure 5 places VIL-SLAM and LOAM trajectories on the left and VIL-SLAM maps on the right, with start/end positions marked by red triangles.The plot origin indicates the trajectory start/end position.Source: Figure 5 · p. 4

B. Loop constraint

• Visual loop constraints initialize ICP alignment of corresponding LiDAR key-scan feature points, improving constraint accuracy while sparse points and good initialization speed convergence.Stereo matches are triangulated and the visual constraint is evaluated with EPNP before ICP refinement.Source: B. Loop constraint · pp. 4–5

C. Global pose graph optimization

• The global pose graph uses LiDAR odometry and loop-constraint factors, each measuring a relative transformation between scan poses.The previous scan supplies the odometry factor's reference; a loop key scan supplies the loop factor's reference.Source: C. Global pose graph optimization · p. 5
• iSAM2 incrementally optimizes the global pose graph, whose variables are LiDAR mapping poses in a 6-DoF representation.Source: C. Global pose graph optimization · p. 5

D. Re-localization

• After a true loop candidate, LiDAR mapping buffers feature points until loop correction arrives, then updates the map with the globally optimized trajectory and resumes operation.The authors say real-time updates are feasible because loop closure is real-time, the sparse feature map uses little memory, and scan-to-map registration can catch up with the LiDAR data rate.Source: D. Re-localization · p. 5

VIII. EXPERIMENTAL RESULTS

• VIL-SLAM is compared with LOAM2 on custom datasets; KITTI odometry is excluded because its sequences lack inertial measurements and are mostly not challenging for the robustness focus.The stereo VIO submodule is also evaluated on the EuRoC MAV dataset.Source: §VIII · p. 5
• Figure 6 compares VIL-SLAM and LOAM map-registration errors against the model, marking errors above 0.3 m in panels (a–b) and 0.5 m in (c).Discontinuous red regions within blue and green correspond to model gaps from Faro-scan occlusions.Source: Figure 6 · p. 5

A. Platform and software

• The platform combines two megapixel cameras, a 16-line LiDAR, a 400 Hz IMU, and a 4 GHz four-core computer with custom synchronization.The implementation uses GTSAM for fixed-lag VIO and global optimization, LibPointMatcher for ICP, and DBoW3 for the visual dictionary.Source: A. Platform and software · p. 5

• The tests cover five representative environments, use final drift error (FDE) for odometry, and assess mean registration error (MRE) against Faro scans.Maps are aligned with the Faro model before computing each map point's distance to its closest model point.Source: B. Tests and results · pp. 5–6
• In the highbay, both systems handle the structured, feature-rich warehouse, and VIL-SLAM's globally refined map has most errors below 0.15 m.LiDAR map registration reduces VIL-SLAM's odometry error, and loop closure recognizes the starting position.Source: B. Tests and results · p. 6
• 0.05% hallway FDE and 0.08% tunnel FDE follow loop closures; VIL-SLAM succeeds where LOAM accumulates hallway error and fails in the tunnel.Hallway visual measurements are sparse and under-constrain VIO, while LiDAR degeneracy leaves some tunnel traversal-direction error; loop closure corrects the maps, with the tunnel correction incomplete after one constraint.Source: B. Tests and results · p. 6, Figure 7 · p. 6
• Without loop closure, VIL-SLAM achieves 0.01% FDE in odometry and registers new scans to the original highbay map on re-entry.Source: B. Tests and results · p. 6
• On the 546 m outdoor trajectory, VIL-SLAM and LOAM have comparable results in the xy plane, while LOAM fails to capture z-direction changes.Source: B. Tests and results · p. 6
• Overall, VIL-SLAM generates more accurate mapping results and lower FDE than LOAM when both finish, and succeeds in more challenging environments where LOAM fails.Source: B. Tests and results · p. 6

C. EuRoC MAV Dataset test

• VIL-VIO succeeds on all EuRoC MAV sequences with accuracy comparable to three state-of-the-art methods, including under aggressive motion, illumination changes, motion blur, and textureless regions.The reported VIL-VIO results are deterministic and obtained in real time on a desktop computer.Source: C. EuRoC MAV Dataset test · p. 6

IX. CONCLUSIONS

• The current framework loosely couples VIL-VIO and LiDAR mapping; the authors are extending it so refined LiDAR poses can correct IMU biases.They also suspect scan-to-map matching could yield a better loop constraint than ICP between sparse feature points.Source: §IX · p. 6