PaperlayerView exact revision on arXiv

Source-linked AI summary

Tightly Coupled 3D Lidar Inertial Odometry and Mapping

Haoyang Ye, Yuying Chen, Ming Liu

arXiv:1904.06993v1cs.ROcs.CV

TL;DR

The paper addresses unreliable or slow lidar-only ego-motion estimation, especially with degraded measurements, by tightly coupling lidar and IMU measurements in odometry and adding rotation-constrained global-map refinement. The resulting system provides high-rate, accurate pose estimation and reduced drift in challenging motion and sensing conditions, although it requires initialization.

  • Problem

    Lidar pose estimation can be limited by sparse or degraded measurements, motion distortion, insufficient features, and a low update rate.

  • Method

    The method jointly optimizes lidar and IMU measurements for tightly coupled odometry, then applies rotation-constrained refinement to align lidar poses with a global map.

  • Results

    The system estimates sensor-pair poses at the IMU update rate with high precision and outperforms lidar-only and loosely coupled methods in challenging tests.

  • Takeaways & Limitations

    The approach provides robust pose estimation in fast-motion, lidar-degraded, and limited-overlap cases, supported by sufficient IMU excitation.

  • Takeaways & Limitations

    The proposed method requires initialization.

Abstract

from arXiv · show

Ego-motion estimation is a fundamental requirement for most mobile robotic applications. By sensor fusion, we can compensate the deficiencies of stand-alone sensors and provide more reliable estimations. We introduce a tightly coupled lidar-IMU fusion method in this paper. By jointly minimizing the cost derived from lidar and IMU measurements, the lidar-IMU odometry (LIO) can perform well with acceptable drift after long-term experiment, even in challenging cases where the lidar measurements can be degraded. Besides, to obtain more reliable estimations of the lidar poses, a rotation-constrained refinement algorithm (LIO-mapping) is proposed to further align the lidar poses with the global map. The experiment results demonstrate that the proposed method can estimate the poses of the sensor pair at the IMU update rate with high precision, even under fast motion conditions or with insufficient features.

I. INTRODUCTION

The paper motivates tightly coupled lidar-IMU fusion to address lidar limitations in pose estimation and proposes odometry plus rotation-constrained refinement for robust, high-rate estimation.

  • Motivation: Lidar provides precise, illumination-invariant distance measurements but has low vertical resolution, sparse features, motion distortion, and degraded returns in narrow corridors.These conditions can make feature tracking difficult and lidar-only pose estimation unreliable.
  • Motivation: Lidar’s low update rate limits its use in fast-response tasks such as robot pose control.The paper identifies this as an additional limitation of stand-alone lidar sensing.
  • Proposed approach: The proposed tightly coupled lidar-IMU odometry jointly optimizes lidar and IMU measurements to provide real-time, accurate state estimation at a high update rate.The approach is designed to use both sensing modalities rather than treating IMU information only as a prior.
  • Proposed approach: A rotation-constrained refinement method further optimizes poses and generated point-cloud maps from the lidar-IMU odometry prior.It is intended to improve consistency and robustness in lidar-degraded cases.
  • Evaluation: Extensive indoor and outdoor tests report that the method outperforms state-of-the-art lidar-only and loosely coupled lidar-IMU algorithms.The paper also states that source code is available online.

III. NOTATIONS AND PRELIMINARIES

The notation distinguishes lidar scans and sweeps, coordinate-frame transformations, sensor measurements, extracted features, and the IMU and lidar reference frames used throughout the method.

  • A. Notations: A scan C is one line of measurement from the 3D lidar, while a sweep S contains all scans in one measurement.For example, a 16-line lidar produces 16 scans in one sweep.
  • A. Notations: The transformation matrix T^a_b maps quantities from frame F_b into frame F_a, with R^a_b and t^a_b denoting its rotation matrix and translation vector.The corresponding quaternion q^a_b follows Hamilton notation, and ⊗ denotes quaternion multiplication.
  • A. Notations: Raw IMU measurements are denoted by â_k and ω̂_k, while extracted features are represented in their original capture frame before transformation.The notation tracks feature-frame changes between coordinate systems.
  • A. Notations: F_Bi and F_Li reference the IMU body and lidar-center frames, respectively, when lidar sweep S_i is acquired.These frames establish the sensor-specific references used for pose estimation.

1) States:

The state representation estimates IMU motion in the world frame together with lidar-IMU extrinsics, while IMU acceleration and gyroscope inputs propagate states between lidar sweeps.

  • 1) States:: The estimated state includes the IMU state X^W_Bi in the world frame and the extrinsic transform T^L_B between lidar and IMU sensors.The formulation explicitly includes both motion states and sensor calibration parameters.
  • 1) States:: The IMU state contains body position, velocity, and orientation relative to the world frame, along with acceleration and gyroscope biases.These variables define the motion and bias components optimized by the system.
  • 2) Dynamic model:: Accelerometer and gyroscope inputs update the preceding IMU state to the current state through discrete evolution between lidar sweeps.All IMU measurements between sweep times are integrated using the interval Δt and gravity vector g^W.
  • 2) Dynamic model:: Quaternion products are written using shorthand notation to represent the integrated sequence of rotations between timestamps.This notation simplifies the dynamic-model expressions.

3) Pre-integration:

The system uses IMU pre-integration and lidar processing within a two-part architecture: tightly coupled odometry estimates states locally, while mapping refines alignment globally.

  • 3) Pre-integration:: IMU measurements between lidar sweeps are integrated into a pre-integration measurement for estimating body motion between timestamps.The integrated measurements are used in the error-state model.
  • 3) Pre-integration:: The proposed system separates tightly coupled local-window odometry from rotation-constrained refinement that aligns lidar sweeps to a global map.The local window bounds odometry optimization, while gravity information contributes to globally consistent mapping.
  • 3) Pre-integration:: Before each lidar sweep arrives, IMU states are updated iteratively and IMU inputs are pre-integrated for joint optimization.The resulting pre-integrated quantities include ∆pij, ∆vij and ∆qij.
  • 3) Pre-integration:: The odometry pipeline de-skews each raw lidar sweep using IMU-propagated motion and a linear motion model during the sweep.Each point is corrected by interpolation into the sweep-ending pose.
  • 3) Pre-integration:: Feature extraction retains points most alike on planes or edges to reduce the lidar data dimension while preserving scan-to-scan features.These feature points are selected from the de-skewed sweep.

C. Relative Lidar Measurements

Relative lidar measurements constrain poses using a local map built from prior feature points, with states estimated from the pivot’s successor through the current sweep.

  • C. Relative Lidar Measurements: The method builds a local map because a single lidar sweep is not dense enough to calculate accurate point correspondences.The map supports relative lidar constraints between sweeps.
  • C. Relative Lidar Measurements: The local map contains lidar feature points from Nm timestamps spanning the first sweep, pivot sweep, and last processed sweep.It is built in the pivot lidar-sweep frame using previously optimized lidar poses.
  • C. Relative Lidar Measurements: The estimated states cover timestamps from p+1 through i and j, where p+1 follows the pivot and j is the current sweep.These states define the optimization window’s active lidar poses.

8 Find KNN(FLp

Correspondences are found between the pivot-frame local map and later lidar features, with nearest-neighbor plane fitting providing relative measurements.

  • 8 Find KNN(FLp: The predicted transform maps local-map features into the pivot lidar frame before correspondence search.The notation simplifies the predicted transform between lidar poses.
  • 8 Find KNN(FLp: Correspondences are found between the pivot-frame local map and original features from later lidar sweeps.The resulting measurements are relative because the pivot pose changes with the sliding window.
  • 8 Find KNN(FLp: KNN is applied to each transformed feature point, and neighboring planar points are fitted into a plane.The paper reports that edged points did not improve lidar-IMU odometry, so subsequent discussion uses planar features.
  • 8 Find KNN(FLp: Each planar correspondence is represented as m = [x, ω, d], with x in the later sweep and ω and d defined in the pivot frame.ω is the plane normal and d is the plane’s distance to the pivot-frame origin.

D. Lidar Sweep Matching

Lidar sweep matching jointly optimizes poses and IMU states in a fixed-lag window, minimizing lidar, IMU, and marginalized-prior residuals.

  • D. Lidar Sweep Matching: Relative lidar measurements constrain the pivot lidar pose and subsequent lidar poses, while the pivot pose remains jointly optimized.Optimizing the pivot helps reduce pre-integration error and align the sensor pair with gravity.
  • D. Lidar Sweep Matching: Each relative lidar residual is represented as a point-to-plane distance and incorporated through lidar constraints.The lidar residual is derived from the point-to-plane measurement equation.
  • D. Lidar Sweep Matching: A fixed-lag smoother keeps Ns IMU states in a sliding window, adding new states and marginalizing the oldest as constraints arrive.This bounds computational cost while retaining recent measurements.
  • D. Lidar Sweep Matching: The mapping stage uses the odometry rotation as a virtual measurement so the final map remains aligned with gravity.The odometry pose first provides a prior for global point-cloud registration.
  • D. Lidar Sweep Matching: The optimized MAP estimate minimizes a Mahalanobis-norm cost containing marginalized priors, relative lidar residuals, and IMU residuals.The nonlinear least-squares problem is solved with Gauss–Newton using Ceres Solver.

V. REFINEMENT WITH ROTATIONAL CONSTRAINTS

The refinement aligns lidar feature points with a global map while imposing rotational constraints from lidar-IMU odometry. This prevents accumulated rotation error from producing a gravity-inconsistent map.

  • Global-map refinement: Global-map registration constrains lidar poses to a consistent world frame and produces the map as a by-product of refinement.The method aligns latest lidar feature points using relative lidar measurements and an associated cost function.
  • Motivation: Accumulated rotation error during long-term operation can leave the merged global map misaligned with gravity and cause subsequent mapping to align with a tilted map.The paper identifies this as a failure mode of unconstrained refinement.
  • Rotational constraints: The constrained mapping strategy modifies the orientation Jacobian using rotational constraints from lidar-IMU odometry so the final map remains aligned with gravity.The strategy is presented as an SE(3) optimization with SE(2)-constraints.
  • Rotational constraints: Because z-axis orientation is more uncertain than the other two orientation degrees of freedom, the cost function constrains orientation through information-based Jacobian modification.The x- and y-axis weights are obtained from their information ratios relative to the z-axis orientation.
  • Optimization: The optimization uses constrained Jacobians to obtain incremental rotation and translation updates, which update the lidar position and quaternion states.The incremental poses are obtained as δθz and δp before updating the lidar states.

VI. IMPLEMENTATION

The implementation section introduces the differing parameters and initialization procedures used for indoor and outdoor experiments.

  • Implementation: Indoor and outdoor tests use different sensor configurations, system initialization procedures, and parameters.These implementation differences are introduced for the respective experimental settings.

A. Different Sensor Configurations

The experiments evaluate the system across sensor configurations, motion speeds, and indoor and outdoor settings. Quantitative tests use a motion-capture ground truth and compare LIO variants with LOAM.

  • Different Sensor Configurations: Hand-held sensor pairs place the lidar and IMU close together, while car-mounted pairs separate them and use a prior on extrinsic translation parameters.The outdoor golf-car configuration mounts the IMU above the base link and the lidar at the front.
  • Initialization: Initialization uses lidar odometry for rough feature-point matching, then requires sufficient sensor-pair motion to make IMU states observable.Lidar poses and IMU measurements initialize the IMU states using established methods.
  • Quantitative Evaluation: The quantitative setup uses a 16-line Velodyne VLP-16 at 10Hz, an Xsens MTi-100 IMU at 400Hz, and motion capture for ground-truth poses.Estimated trajectories are aligned with ground truth for comparison.
  • Quantitative Evaluation: Table I reports translational and rotational RMSE across six sequences spanning fast to slow motions, comparing LOAM with LIO variants and LIO-mapping.LIO-raw removes motion compensation, while LIO-no-ex removes online extrinsic parameter estimation.
  • Results: LIO-mapping provides accurate translational and rotational estimation across all tested motion conditions.LIO performs better with faster motion but can drift during slow motion because the local map is relatively sparse.

2) Tests of Drift over Time:

Over longer tests, LIO maintains relatively accurate poses but accumulates drift, while LIO-mapping reduces drift through global-map registration. LOAM and LOAM+IMU fail to provide robust estimation during rapid motion, whereas the proposed method is reported to outperform lidar-only and loosely coupled approaches.

  • Drift over time: LIO provides relatively accurate poses over time but suffers from trajectory drift.The drift is attributed to its small local window.
  • Drift over time: LIO-mapping results in less trajectory drift and greater state-estimation consistency by registering current sweeps to the global map.
  • Drift over time: LOAM and LOAM+IMU cannot provide robust estimation when motion becomes rapid in the latter half of the test.
  • Drift over time: The proposed method was evaluated across indoor hand-held, outdoor golf-cart, and KAIST Urban dataset tests, with results reported as outperforming lidar-only and loosely coupled methods.Additional pose-estimation and mapping results are provided in the supplementary video.
  • Drift over time: Both LIO and LIO-mapping provide smoother translational and rotational error results than their counterparts.
Loading 1904.06993v1…