VehicleMapping HD AV Spatial Data

Estimating LiDAR-to-IMU Extrinsics with Hand-Eye Calibration

The LiDAR-to-IMU transform has no direct correspondences — you cannot point a checkerboard at an accelerometer — so it is recovered from motion instead. This task solves the classic hand-eye equation AX = XB, pairing LiDAR-odometry motion increments (A) with integrated-IMU increments (B) to recover the fixed transform X, then gates on rotation residual and trajectory excitation. It is the motion-based method in the extrinsic calibration workflows, providing the inertial link that ties LiDAR into the multi-sensor coordinate alignment chain.

Paired LiDAR and IMU motion increments over an excited trajectory constrain the fixed transform X via AX = XB:

LiDAR-to-IMU Hand-Eye Calibration A trajectory produces paired increments A from LiDAR and B from IMU. They feed an AX = XB box. A staged solver box recovers rotation then translation. A gate checks rotation residual under 0.2 degrees and axis excitation. Paired motion A: LiDAR · B: IMU A·X = X·B hand-eye equation Staged solve rotation → translation Gate resid ≤0.2° · excited excite rotation about all axes or the transform is unobservable

Prerequisites #

  • Python 3.10+, NumPy 1.24+, SciPy 1.11+ (spatial.transform.Rotation, linalg).
  • Input: synchronized LiDAR-odometry poses and integrated-IMU poses over an excited trajectory, sharing one clock.
  • Upstream stage: LiDAR odometry from a registration pipeline; IMU pre-integration producing relative poses; timestamps aligned.
  • Output: the LiDAR→IMU transform X with a rotation-residual report.

Step-by-Step #

1. Form motion increments A and B #

Over matching intervals, build relative-motion pairs: A from consecutive LiDAR poses, B from consecutive IMU poses.

python
import numpy as np

def increments(poses: list[np.ndarray]) -> list[np.ndarray]:
    """Relative motions T_i^{-1} T_{i+1} from a sequence of 4x4 poses."""
    return [np.linalg.inv(poses[i]) @ poses[i + 1]
            for i in range(len(poses) - 1)]

Key parameter: the intervals for A and B must correspond to the same physical motion — pair increments by their shared timestamps before solving.

2. Solve the rotation from the rotational constraints #

The rotational part of AX = XB constrains X's rotation independently of translation. Stack the constraints and solve the resulting eigenvector problem.

python
from scipy.spatial.transform import Rotation

def solve_rotation(A_list, B_list) -> np.ndarray:
    """Recover R_X from axis-angle constraints (Tsai-Lenz style)."""
    M = np.zeros((3, 3))
    for A, B in zip(A_list, B_list):
        ra = Rotation.from_matrix(A[:3, :3]).as_rotvec()
        rb = Rotation.from_matrix(B[:3, :3]).as_rotvec()
        M += np.outer(rb, ra)                 # accumulate axis correspondences
    U, _, Vt = np.linalg.svd(M)
    d = np.sign(np.linalg.det(U @ Vt))
    return U @ np.diag([1, 1, d]) @ Vt

Key parameter: each pair contributes its rotation-axis correspondence; the SVD recovers the rotation aligning the IMU axes to the LiDAR axes, with a proper-rotation correction.

3. Solve the translation linearly #

With R_X known, the translational part of AX = XB is a linear system in the translation vector.

python
def solve_translation(A_list, B_list, R_X) -> np.ndarray:
    C, d = [], []
    for A, B in zip(A_list, B_list):
        Ra, ta = A[:3, :3], A[:3, 3]
        tb = B[:3, 3]
        C.append(Ra - np.eye(3))              # (R_A - I) t_X = R_X t_B - t_A
        d.append(R_X @ tb - ta)
    C = np.vstack(C); d = np.concatenate(d)
    t_X, *_ = np.linalg.lstsq(C, d, rcond=None)
    return t_X

Key parameter: the stacked least-squares system needs several well-excited increments to be full-rank; a degenerate trajectory makes C rank-deficient.

Verification & Acceptance Criteria #

Validate the rotation residual and confirm the trajectory excited all axes.

python
def assert_handeye(A_list, B_list, X, tol_deg=0.2):
    from scipy.spatial.transform import Rotation
    res = []
    for A, B in zip(A_list, B_list):
        E = np.linalg.inv(A @ X) @ (X @ B)    # should be identity
        res.append(np.degrees(np.linalg.norm(Rotation.from_matrix(E[:3, :3]).as_rotvec())))
    assert np.mean(res) <= tol_deg, f"rotation residual {np.mean(res):.2f}° > {tol_deg}"
    print(f"hand-eye residual {np.mean(res):.3f}°")

Acceptance gate: mean rotation residual ≤0.2°; the trajectory excited rotation about all three axes (check the spread of rotation axes across increments); and the translation system was full-rank. Under-excitation leaves some axes unobservable — collect a richer trajectory rather than trusting the fit.

Common Errors & Fixes #

Roll and pitch of the transform are wrong. The trajectory only turned about the vertical axis, leaving those axes unobservable. Drive a path that excites pitch and roll, even mild grade changes and turns.

Translation is noisy while rotation is fine. The translation system is near-degenerate from too few or too-similar increments. Add increments with varied rotation, which conditions the (R_A − I) matrix.

Residual is large despite good data. A time-sync offset between LiDAR and IMU misaligns the paired increments. Align timestamps first, and pair increments strictly by shared time.

Solution flips to a reflection. The SVD produced a det −1 rotation. Apply the proper-rotation correction in step 2.

FAQ #

What is the hand-eye calibration equation? #

Hand-eye calibration solves AX = XB, where A is a motion increment measured by one sensor, B is the same physical motion measured by another, and X is the unknown fixed transform between them. It originated in robotics for finding the transform between a robot gripper and a mounted camera. For LiDAR-to-IMU, A comes from LiDAR odometry and B from integrated IMU motion over the same interval, and X is the rigid transform bolting the two sensors together.

Why does the trajectory need to be well-excited? #

The hand-eye equation only constrains the transform in the directions the motion actually rotates about. A trajectory that only ever turns about the vertical axis leaves the roll and pitch components of the transform unobservable. To recover all three rotation axes you must excite rotation about all of them — drive a path with turns, and if possible some pitch and roll — so every degree of freedom of X is constrained by the data.

Why solve rotation before translation? #

The hand-eye equation decouples: the rotational part depends only on the rotations of A and B, while the translational part depends on both the rotations and the recovered rotation X. So you first solve the rotation from the rotational constraints alone, which is a well-studied eigenvector problem, and then substitute it into a linear system for the translation. Attempting to solve both together is nonlinear and less stable, whereas the staged solve is robust and closed-form for the rotation.

Up one level: Extrinsic Calibration Workflows for LiDAR, Camera & IMU — the parent workflow this motion-based method is one part of.