LiDAR-IMU Extrinsic Calibrator (Very Experimental)

Estimates the rotation (3-DOF) between a LiDAR and an IMU using scan-to-scan GICP odometry and gyroscope integration, solved via SVD hand-eye calibration and GTSAM factor graph optimization.

No calibration target needed. No initial guess needed. Just drive.

Algorithm

Two-stage approach:

1. SVD Hand-Eye (AX=XB): Collects relative rotation pairs from GICP (LiDAR) and gyroscope (IMU) across multiple keyframe strides. Solves the quaternion hand-eye equation in closed form via SVD — no initial guess required.

2. GTSAM Path Alignment: Builds dual pose chains (LiDAR from GICP, IMU from gyro integration) and jointly optimizes them with the SVD result as initial guess. The calibration transform X links both chains: L(i) * X ≈ I(i). This uses full trajectory information for refinement.

Re-calibrates every N keyframes and reports observability analysis.

Requirements

• ROS2 Humble
• PCL (libpcl-dev)
• GTSAM (ros-humble-gtsam)
• Eigen3

Build

colcon build --symlink-install --packages-select lidar_imu_calibrator

Configuration

Edit config/calibrator_config.yaml:

lidar_imu_calibrator:
  ros__parameters:
    # === MUST CHANGE for your vehicle ===
    lidar_topic: "/your/lidar/pointcloud"    # sensor_msgs/PointCloud2
    imu_topic: "/your/imu/data"              # sensor_msgs/Imu (needs angular_velocity)
    lidar_frame: "velodyne_top"              # LiDAR TF frame name
    imu_frame: "imu"                         # IMU TF frame name
    output_file: "/path/to/output.yaml"      # Auto-saved calibration result

    # === Tuning (defaults work for most setups) ===
    keyframe_dist: 0.5          # Min distance (m) between keyframes
    keyframe_angle: 0.05        # Min rotation (rad) between keyframes
    calibrate_every_n_keyframes: 30  # Re-calibrate interval
    max_keyframes: 500          # Sliding window size

    # === GICP parameters ===
    voxel_size: 0.2             # Downsampling voxel size (m). Decrease for dense LiDAR
    gicp_max_corr_dist: 1.5     # Max correspondence distance (m)
    gicp_max_iter: 64           # GICP iterations
    min_range: 0.5              # Min point range (m)
    max_range: 80.0             # Max point range (m)

    # === Hand-eye solver ===
    min_pair_rotation_deg: 3.0  # Min rotation to consider a pair useful
    huber_threshold_deg: 5.0    # Huber robust weighting threshold
    max_pair_stride: 7          # Max keyframe stride for multi-stride pairs

Key parameters to adjust

Parameter	When to change	Guideline
voxel_size	Dense LiDAR (64/128ch)	Decrease to 0.1 for more points
voxel_size	Sparse LiDAR (16ch)	Keep 0.2-0.3
calibrate_every_n_keyframes	Want faster results	Decrease to 20 (noisier)
calibrate_every_n_keyframes	Want stable results	Increase to 50
min_pair_rotation_deg	Mostly straight driving	Decrease to 1.0
min_pair_rotation_deg	Aggressive driving	Keep 3.0-5.0
max_pair_stride	Short calibration runs	Increase to 10
gicp_max_corr_dist	Noisy scan matching	Decrease to 1.0

Usage

Run with RViz

source install/setup.bash
ros2 launch lidar_imu_calibrator calibrator.launch.py rviz:=true

Run headless

source install/setup.bash
ros2 launch lidar_imu_calibrator calibrator.launch.py rviz:=false

With rosbag

ros2 launch lidar_imu_calibrator calibrator.launch.py rviz:=true &
ros2 bag play /path/to/bag --clock

RViz Visualization

• Green path: LiDAR trajectory (from GICP odometry)
• Red path: IMU trajectory (gyro rotation transformed by calibration result, using LiDAR position)
• If calibration is correct, both path orientations (axes) should align

Output

Calibration result is automatically saved to output_file in YAML format:

parent_frame: "velodyne_top"
child_frame: "imu"

translation:
  x: 0.0    # Translation not estimated (rotation only)
  y: 0.0
  z: 0.0

rotation_rpy:
  roll: 3.141593
  pitch: 0.000000
  yaw: 1.570796

rotation_quaternion:
  x: 0.000000
  y: 0.707107
  z: 0.707107
  w: 0.000000

rotation_matrix:
  - [0.000000, 1.000000, 0.000000]
  - [1.000000, 0.000000, 0.000000]
  - [0.000000, 0.000000, -1.000000]

A static TF is also published: <lidar_frame> → <imu_frame>_calibrated.

Data Collection Tips

For best results:

1. Drive with turns — straight driving only constrains roll and pitch, not yaw
2. Varied terrain helps — hills and bumps excite pitch and roll axes
3. Figure-8 pattern is ideal — maximizes rotation diversity
4. 30+ keyframes minimum before first calibration
5. Longer is better — more data = more accurate, especially for yaw
6. Speed bumps are gold — even small pitch changes significantly improve yaw observability

Limitations

• Rotation only — translation between LiDAR and IMU is not estimated. Measure physically or from CAD.
• Yaw on flat ground — yaw offset is poorly observable when driving on flat ground with yaw-only turns. Roll and pitch are well-constrained regardless.
• GICP drift — scan-to-scan GICP accumulates drift over time. The multi-stride SVD approach mitigates this but does not eliminate it. Denser LiDARs produce better results.

Tested On

• CARLA 0.9.16 simulator with VLP-16 (16-channel, 150k pts/sec)
• ROS2 Humble on Ubuntu 22.04
• Roll/pitch accuracy: < 1 degree
• Yaw accuracy: < 10 degrees (flat ground), expected < 3 degrees on real terrain