VehicleMapping HD AV Spatial Data

Motion-Compensating LiDAR Scans with SLERP

A spinning LiDAR stamps an entire 100 ms revolution with one time, but at highway speed the vehicle moves nearly three metres across that window — so a single wall comes back sheared, and registration fails on the distortion. This task undistorts the sweep: assign every return its true capture time, interpolate the pose at that instant with SLERP for rotation and linear interpolation for translation, and reproject each point into one reference instant. It refines the timing work of LiDAR and camera temporal synchronization into clean geometry, and feeds cleaner clouds into point cloud registration.

Each point is stamped by azimuth, its pose SLERP-interpolated, and reprojected into one reference instant:

SLERP Motion Compensation of a LiDAR Sweep A timeline from t-start to t-end with a pose at each end. Points along the timeline get interpolated poses via SLERP and linear translation. An arrow leads to a reproject-to-reference box, then to a gate checking residual smear under 0.03 metres. pose @ t_start pose @ t_end per-point t by azimuth · SLERP + lerp Reproject to reference instant relative pose T(t_i → t_ref) Undistorted cloud smear ≤ 0.03 m

Prerequisites #

  • Python 3.10+, NumPy 1.24+, SciPy 1.11+ (spatial.transform.Rotation, Slerp).
  • Input: a raw LiDAR sweep with per-point azimuth or relative timestamps, and two or more ego-poses bracketing the sweep window (from odometry or IMU integration).
  • Upstream stage: per-point timing derives from the sweep's start time and rotation rate; poses share the sweep's clock, disciplined as in aligning LiDAR and camera timestamps in ROS.
  • Output: an undistorted point cloud expressed at a single reference instant.

Step-by-Step #

1. Assign each point its true capture time #

Points arrive by azimuth; convert azimuth to a fraction of the sweep to get each point's timestamp within the window.

python
import numpy as np

def point_times(azimuth_rad: np.ndarray, t_start: float, sweep_period: float) -> np.ndarray:
    """Per-point capture time from azimuth within a single revolution."""
    frac = (azimuth_rad % (2 * np.pi)) / (2 * np.pi)   # 0..1 through the sweep
    return t_start + frac * sweep_period

Key parameters: sweep_period (~0.1 s for a 10 Hz LiDAR) scales the azimuth fraction to real time. Expected output: a per-point timestamp array.

2. Interpolate the pose at each timestamp #

SLERP the rotation between the bracketing poses and linearly interpolate the translation.

python
from scipy.spatial.transform import Rotation, Slerp

def interp_poses(times, key_times, key_rots: Rotation, key_trans: np.ndarray):
    """SLERP rotation + linear translation at each point time."""
    slerp = Slerp(key_times, key_rots)
    rots = slerp(np.clip(times, key_times[0], key_times[-1]))
    trans = np.column_stack([
        np.interp(times, key_times, key_trans[:, k]) for k in range(3)
    ])
    return rots, trans

Key parameters: key_times/key_rots/key_trans are the bracketing poses; SLERP handles rotation on the sphere, np.interp handles translation in flat space.

3. Reproject every point to the reference instant #

Transform each point from its capture pose into the reference pose, so the whole sweep is expressed at one time.

python
def compensate(points, times, ref_time, key_times, key_rots, key_trans):
    rots, trans = interp_poses(times, key_times, key_rots, key_trans)
    ref_rot, ref_trans = interp_poses(np.array([ref_time]), key_times, key_rots, key_trans)
    R_ref_inv = ref_rot.inv()
    world = rots.apply(points) + trans                 # point -> world
    return R_ref_inv.apply(world - ref_trans)          # world -> reference frame

Key parameter: ref_time is the chosen reference instant (sweep midpoint, or the camera frame time); every point lands in that single frame.

Verification & Acceptance Criteria #

Validate on a known-static planar surface — after compensation it must return flat.

python
def assert_compensated(plane_points: np.ndarray, tol=0.03):
    # fit a plane; residual thickness is the smear
    centroid = plane_points.mean(0)
    _, s, _ = np.linalg.svd(plane_points - centroid)
    thickness = float(s[-1] / np.sqrt(len(plane_points)))
    assert thickness <= tol, f"residual smear {thickness:.3f} m > {tol}"
    print(f"plane thickness {thickness*1000:.1f} mm after compensation")

Acceptance gate: a static planar surface returns with ≤0.03 m thickness at highway speed; heading interpolation continuous across the sweep (no wrap artefact); and the same reference instant used for every point. Residual smear above budget usually means the pose bracket is too coarse — add an intermediate pose from the IMU.

Common Errors & Fixes #

Walls still appear doubled after compensation. Points were all stamped with the sweep time, so the per-point timing in step 1 was skipped. Recover true timestamps from azimuth before interpolating.

Rotation glitches near a large heading change. Linear interpolation of quaternion components was used instead of SLERP. Use scipy.spatial.transform.Slerp, which walks the correct great-circle arc.

A slight offset remains on all surfaces. Different consumers used different reference instants. Fix one ref_time per sweep and pass it to every stage.

Compensation makes the cloud worse. The bracketing poses are wrong or from a different clock. Confirm the poses and the LiDAR share a disciplined clock before compensating.

FAQ #

Why does a spinning LiDAR sweep need motion compensation? #

A mechanical LiDAR takes roughly 100 milliseconds to complete one revolution, and it stamps the whole sweep with a single time even though the first and last points were captured a tenth of a second apart. During that window the vehicle moves — at highway speed nearly three metres — so the points are expressed in poses that no longer agree. Without compensation a straight wall appears sheared or doubled, corrupting registration and mapping.

Why SLERP for the rotation instead of linear interpolation? #

Rotations do not live in a flat space, so linearly interpolating quaternion components or Euler angles does not produce a valid constant-rate rotation and distorts near large angle changes. Spherical linear interpolation, SLERP, walks the shortest great-circle arc between two orientations at constant angular velocity, which is exactly the assumption of smooth ego-rotation over a short sweep. Translation, living in flat space, is interpolated linearly.

Which reference instant should points be reprojected to? #

Pick one consistent instant per sweep, commonly the sweep start, the sweep midpoint, or the timestamp of the camera frame you are fusing with. The midpoint minimizes the maximum per-point correction and so the residual error, while matching the camera timestamp is best when the goal is LiDAR-camera projection. What matters is that every point in the sweep, and every downstream consumer, uses the same reference.

Up one level: LiDAR and Camera Temporal Synchronization — the parent workflow whose timing this compensation turns into clean geometry.