PaperlayerView exact revision on arXiv

Source-linked AI summary

LIC-Fusion 2.0: LiDAR-Inertial-Camera Odometry with Sliding-Window Plane-Feature Tracking

Xingxing Zuo, Yulin Yang, Patrick Geneva, Jiajun Lv, Yong Liu, Guoquan Huang, Marc Pollefeys

arXiv:2008.07196v1cs.RO

TL;DR

Accurate, robust 3D localization requires combining complementary LiDAR, camera, and IMU measurements. LIC-Fusion 2.0 integrates sliding-window plane-feature tracking, uncertainty-aware association, and observability analysis into a calibrated fusion estimator, achieving improved performance with consistent pose estimation when online calibration is used.

  • Problem

    Accurate and robust 3D localization is essential for autonomous-robot tasks, while LiDAR, camera, and IMU each have complementary strengths and shortcomings.

  • Method

    LIC-Fusion 2.0 integrates sliding-window LiDAR plane-feature tracking with uncertainty-aware outlier rejection, plane initialization, tightly coupled sensor fusion, and LiDAR-IMU observability analysis.

  • Results

    The proposed system achieves consistent and accurate pose estimation with online calibration, while LIC-Fusion 2.0 improves performance and is validated on simulated and real-world datasets.

  • Takeaways & Limitations

    The analysis identifies motion- and plane-structure-dependent degenerate cases that constrain when online LiDAR-IMU calibration parameters are observable.

Abstract

from arXiv · show

Multi-sensor fusion of multi-modal measurements from commodity inertial, visual and LiDAR sensors to provide robust and accurate 6DOF pose estimation holds great potential in robotics and beyond. In this paper, building upon our prior work (i.e., LIC-Fusion), we develop a sliding-window filter based LiDAR-Inertial-Camera odometry with online spatiotemporal calibration (i.e., LIC-Fusion 2.0), which introduces a novel sliding-window plane-feature tracking for efficiently processing 3D LiDAR point clouds. In particular, after motion compensation for LiDAR points by leveraging IMU data, low-curvature planar points are extracted and tracked across the sliding window. A novel outlier rejection criterion is proposed in the plane-feature tracking for high-quality data association. Only the tracked planar points belonging to the same plane will be used for plane initialization, which makes the plane extraction efficient and robust. Moreover, we perform the observability analysis for the LiDAR-IMU subsystem and report the degenerate cases for spatiotemporal calibration using plane features. While the estimation consistency and identified degenerate motions are validated in Monte-Carlo simulations, different real-world experiments are also conducted to show that the proposed LIC-Fusion 2.0 outperforms its predecessor and other state-of-the-art methods.

I. INTRODUCTION AND RELATED WORK

LIC-Fusion 2.0 combines LiDAR, camera, and IMU measurements in a sliding-window filtering framework to improve robust 3D pose estimation. Its contributions include plane-feature tracking, uncertainty-aware association, observability analysis, and validation in simulations and real-world datasets.

  • System motivation and framework: LIC-Fusion 2.0 integrates LiDAR, camera, and IMU measurements in a lightweight filtering framework with spatial-temporal calibration.The system extends the prior LIC-Fusion estimator and stores long-lived visual and LiDAR landmarks to limit estimation drift.
  • Core contributions: A new tracking-based initialization strategy extracts and initializes only tracked planar points belonging to selected planes, improving efficiency and robustness.The approach addresses the computational cost of large-scale plane extraction and the tuning burden of plane-feature association.
  • Core contributions: Sliding-window plane-feature tracking follows 3D environmental planes across multiple LiDAR scans and uses transformation uncertainty for robust matching.The method is designed to avoid inconsistent-prone ICP scan matching by modeling LiDAR measurement uncertainty.
  • Observability analysis: The paper analyzes observability for LiDAR-inertial-camera calibration with plane features and identifies degenerate cases that create additional unobservable directions.These cases are not directly covered by previously identified visual-inertial calibration degeneracies.
  • Evaluation: Monte-Carlo simulations and real-world datasets verify the proposed system’s consistency and accuracy.The experiments compare LIC-Fusion 2.0 with its predecessor and other methods.

B. Point-to-Plane Measurement Model

The point-to-plane model represents environmental planes in anchored coordinates and uses tracked LiDAR measurements to update either transient or persistent plane landmarks within the filter.

  • Measurement representation: LiDAR planar point measurements are modeled through their point-to-plane distance to an anchored plane feature.The anchored closest-point representation avoids a singularity when the plane’s global closest-point norm approaches zero.
  • Feature categories: Tracked plane features are divided into MSCKF and SLAM categories according to track length.MSCKF planes are handled through nullspace projection, while existing SLAM planes directly update the state and covariance.
  • MSCKF plane update: Nullspace projection removes the dependency on an MSCKF plane landmark before stacking residuals and Jacobians for the EKF update.The stacked system is then used with standard EKF update equations.
  • SLAM plane initialization: A long-track plane becomes a SLAM feature only when its normal differs by more than ten degrees from currently estimated planes.The criterion selects informative planes whose directions constrain the state estimate.

III. SLIDING-WINDOW LIDAR PLANE TRACKING

The system removes LiDAR motion distortion using buffered, interpolated IMU poses before tracking planar features across consecutive scans in a sliding window.

  • Motion compensation: Motion distortion is removed by interpolating buffered high-frequency IMU poses at the acquisition time of each LiDAR ray.Orientation interpolation is performed on SO(3).
  • Sliding-window tracking: The sliding window tracks a plane landmark across multiple LiDAR frames rather than restricting association to consecutive scans.A planar point in one scan is associated with a triangle of three planar points in the subsequent scan, with all tracked points assumed to belong to one plane.

B. Planar Landmark Tracking

Planar landmark tracking extracts low-curvature points, forms local triangles across scans, and rejects candidate associations when plane normals are inconsistent under estimated uncertainty.

  • Planar landmark tracking: Low-curvature LiDAR points are extracted from each undistorted scan and tracked by projecting prior points into the latest frame.Each projected point is matched to a nearest neighbor, supplemented by nearby points on the same and adjacent scan rings.
  • Planar landmark tracking: Each candidate plane association uses three points in the latest scan to compute a plane normal and measurement covariance.The selected points must satisfy scan-ring and reuse constraints before the association is tested.
  • Normal-based data association: A Mahalanobis distance test compares candidate plane normals while incorporating current state and planar-point uncertainty.This rejects associations such as floor-wall matches that have similar spatial proximity but perpendicular normals.
  • Normal-based data association: Incorrect temporal planar tracks can be rejected only after more than two sequential LiDAR frames are available.The multi-frame requirement corresponds to the plane-landmark tracking setup illustrated in Fig. 2.

D. Planar Landmark Initialization

The method initializes plane landmarks from valid planar observations tracked across the sliding window, then refines their parameters through optimization.

  • Tracked plane landmarks are initialized in the oldest LiDAR frame using all valid planar point observations collected across the sliding window.The observations form the set Pfj used for initialization.
  • Each planar point measurement contributes to estimating the plane landmark from its observations and measurement noise.
  • A linear system provides initial estimates of the plane normal and plane distance from the stacked observations.The normal is normalized, while the plane distance is the norm of the estimated closest-point representation.
  • The initial plane estimate is further refined by minimizing a cost function over the observations.
  • The complete plane-initialization procedure is part of the LIC-Fusion 2.0 LiDAR processing pipeline.The pipeline is presented in Algorithm 2.

IV. OBSERVABILITY ANALYSIS

The observability analysis targets LiDAR-IMU navigation with online calibration and plane features, while the processing pipeline propagates, compensates, tracks, updates, and manages plane landmarks in a sliding window.

  • The analysis focuses on LiDAR-IMU observability with online calibration and plane features because prior IMU-camera results do not directly apply.
  • The observability matrix is constructed from measurement Jacobians, whose right null space represents the system’s unobservable directions.
  • State model: The state-transition model represents the calibration and plane-feature transitions, with the plane expressed in the global frame and one plane considered explicitly.
  • LiDAR processing pipeline: Algorithm 2 propagates IMU state, buffers poses, clones the corresponding pose, removes scan distortion, and extracts and tracks planar points.
  • Plane-feature management: Tracked planar observations are validated, converted into residuals and Jacobians, and used to decide whether a plane becomes a SLAM landmark or an MSCKF feature.
  • State management: Plane landmarks are inserted or updated according to track length and normal-direction informativeness, while lost landmarks and old cloned poses are marginalized.

B. Measurement Jacobians and Observability Matrix

The observability matrix is built from measurement-Jacobian blocks for the LiDAR-IMU system with plane features; under 3D random motion, spatial and temporal calibration become observable.

  • The k-th observability-matrix block is constructed following the stated observability methodology from the system’s measurement Jacobians.
  • The Jacobian block expressions are referenced in the technical report for the detailed coefficient definitions.
  • For LiDAR-aided INS with IMU state, LiDAR-IMU calibration, and a plane feature, the system has at least 7 unobservable directions.
  • The seventh unobservable direction corresponds to rotation around the plane normal.
  • Under 3D random motions, the relevant Jacobian blocks tend to have full column rank, making spatial and temporal LiDAR-IMU calibration observable.

C. Degenerate Cases Analysis for LiDAR-IMU Calibration

LiDAR-IMU calibration with plane features becomes degenerate under specific plane structures and motions, especially in the one-plane case, leaving calibration parameters unobservable.

  • Degenerate calibration cases depend on both plane structure and system motion, with the analysis illustrating one-plane or mutually parallel-plane configurations.Two-plane and three-plane cases are deferred to the companion technical report.
  • Pure translation makes the LiDAR-IMU rigid transformation, including orientation and translation, unobservable.
  • Rotation about a fixed axis leaves LiDAR-IMU translation unobservable along that rotation axis.An additional unobservable direction occurs when the rotation axis is perpendicular to the plane direction.
  • Constant angular velocity with constant velocity or constant acceleration makes LiDAR-IMU temporal calibration unobservable.
  • In the one-plane case, additional degeneracy occurs when angular velocity is parallel to the plane normal under the stated motion condition.
  • The identified LiDAR-IMU degeneracies overlap with some IMU-camera cases, but pure translation additionally makes LiDAR-IMU orientation unobservable.
  • Any combination of the degenerate motions listed in Table I is also degenerate.

V. SIMULATION RESULTS

Simulation evaluates LIC-Fusion 2.0’s accuracy and consistency with online calibration and examines a predicted degenerate motion. The results support accurate estimation while exposing an unobservable calibration component in yaw-only motion.

  • Simulation setup: The simulation evaluates accuracy with ATE and consistency with NEES across 12 Monte-Carlo runs, with and without online LiDAR-IMU calibration.The setup includes perturbed initial states and measurement noise; Table III compares ground-truth (“true”) and perturbed (“bad”) calibration initialization.
  • Calibration evaluation: Online calibration achieves consistent and accurate pose estimation, whereas omitting it under perturbed calibration produces much larger ATE and NEES.LIC-Fusion 2.0 also outperforms the IMU-camera subsystem in the simulation.
  • Calibration evaluation: The system uses 14.61 MSCKF plane landmarks and 1.60 SLAM plane landmarks for updates on average every scan.
  • Degenerate motion: In yaw-only motion, all calibration parameters except the z component of LpI converge with shrinking uncertainty bounds across six runs.The experiment tests the degenerate-motion analysis identified for online LiDAR-IMU calibration.
  • Degenerate motion: The yaw-only experiment supports the analysis that the z component of LpI is unobservable in this motion.

VI. REAL-WORLD EXPERIMENTAL RESULTS

The real-world evaluation uses an asynchronous three-sensor platform with online time-offset estimation. Its processing pipelines combine prior OpenVINS image processing with the proposed LiDAR processing.

  • Platform: The evaluation platform consists of a Velodyne VLP-16, an Xsens IMU, and a global-shutter monocular camera.
  • Calibration: All sensors publish asynchronously, and their time offsets are estimated online from zero initial guesses.
  • Processing: The image pipeline uses prior OpenVINS processing, while the LiDAR pipeline is proposed in this work.

A. Teaching Building Sequences

Teaching Building experiments evaluate start-to-end drift across seven sequences and compare LIC-Fusion 2.0 with its subsystems and other state-of-the-art methods. LIC-Fusion 2.0 is reported as more robust and more accurate on most sequences, while corridor geometry and difficult lighting expose failures in competing or subsystem methods.

  • Evaluation protocol: The Teaching Building evaluation uses start-to-end drift, expected to be zero, and reports averaged start and end errors over five runs on seven sequences.
  • Evaluation protocol: The comparison includes LIC-Fusion 2.0, OpenVINS, Proposed-LI, and other state-of-the-art algorithms.
  • Observed failures: OpenVINS fails on Seq 1 because huge exposure changes occur under poor lighting while ascending stairs.
  • Observed failures: Proposed-LI has larger drift on Seq 3 and Seq 6, where the sensor suite traverses long corridors with only parallel planes observed.
  • Observed failures: LIO-MAP also fails on Seq 3 despite maintaining a global map.
  • Overall results: LIC-Fusion 2.0 is more robust and achieves higher accuracy on most sequences, while typical Seq 5 updates use 18.81 MSCKF planes and 2.09 SLAM planes on average.The broader evaluation reports better accuracy than existing LiDAR-IMU-camera fusion algorithms and real-time suitability in the tested indoor scenario.
Loading 2008.07196v1…