Aligning LiDAR and Camera Timestamps in ROS

This guide pairs a mechanically scanning LiDAR with a rolling-shutter camera in ROS under a sub-millisecond timing budget, the synchronization step that every projection-based sensor fusion pipeline depends on before LiDAR points can be drawn onto an image frame.

Temporal misalignment between a spinning LiDAR and a rolling-shutter camera introduces spatial shear, ghosting, and projection artifacts that directly corrupt HD map generation and perception fidelity. The challenge is not ROS header matching alone; it demands deterministic timestamp propagation from sensor firmware through kernel drivers and switches to the fusion node, while compensating for clock drift, queue saturation, and intra-frame exposure skew. This page builds the alignment as a runnable sequence: discipline the clock, backdate the stamps, broker the streams, gate on drift, then verify with reprojection error. It sits inside LiDAR and camera temporal synchronization and feeds the projection and HD-map stages downstream.

How message_filters brokers asynchronous LiDAR and camera streams into validated pairs:

Prerequisites #

• ROS Noetic (Python rospy); the message_filters API shown is identical under ROS 2 rclpy with message_filters.ApproximateTimeSynchronizer.
• Python 3.8+ with rospy, message_filters, and sensor_msgs from your ROS distribution.
• linuxptp 3.1+ on the host — ptp4l and phc2sys — plus a NIC with hardware timestamping (ethtool -T <iface> must report SOF_TIMESTAMPING_TX_HARDWARE / RX_HARDWARE).
• Sensor drivers that emit hardware stamps: a LiDAR driver publishing sensor_msgs/PointCloud2 with a firmware return-time header.stamp, and a camera driver that stamps from the V4L2 VIDIOC_DQBUF kernel timestamp rather than the publish instant.
• Timing budget: target ≤0.005 s (5 ms) stamp delta per accepted pair and ≤0.5 px reprojection residual against a calibration target.
• Input assumption: both sensors share a PTP-disciplined clock domain. This step runs after the drivers are publishing and before projection; extrinsics from the multi-sensor coordinate alignment stage are applied only once timestamps are coherent.

Step-by-Step #

1. Discipline the host clock with PTP #

Software stamping cannot compensate for divergent hardware clocks. IEEE 1588 PTPv2 is the automotive standard; NTP's millisecond jitter and lack of hardware timestamping make it structurally inadequate for this budget. Run ptp4l against the NIC PHC, then phc2sys to slave the system clock to that PHC.

# ptp4l configuration (ptp.cfg)
[global]
priority1         128
domainNumber      0
twoStepFlag       1
tx_timestamp_timeout 10
assume_two_step   1
logging_level     6

# run the protocol handler against the NIC, then slave the system clock
ptp4l  -i eth0 -f ptp.cfg -m &
phc2sys -s /dev/ptp0 -c CLOCK_REALTIME -w -O 0 -m

Key parameters: twoStepFlag 1 separates the sync and follow-up messages so the master's true egress time is used; phc2sys -w waits for ptp4l to synchronize before disciplining CLOCK_REALTIME; -O 0 applies zero UTC offset because both endpoints share TAI. Expected output: phc2sys log lines whose offset settles within ±1000 ns on a stable segment. Record raw epochs with rosbag record --clock under use_sim_time=false so true temporal relationships survive replay.

2. Backdate ROS header stamps to hardware capture time #

header.stamp is frequently misread as capture time; it actually reflects the publish instant on the ROS graph, not the photon or laser-return event. Drivers must backdate the stamp using hardware times — the camera from the V4L2 buffer's kernel timestamp, the LiDAR from firmware return time.

import rospy
from sensor_msgs.msg import Image

def stamp_from_v4l2(msg: Image, v4l2_buf_ts: float) -> Image:
    # v4l2_buf_ts: VIDIOC_DQBUF v4l2_buffer.timestamp in CLOCK_MONOTONIC_RAW (seconds)
    # Convert the monotonic capture instant into the ROS (realtime) epoch the
    # synchronizer compares against, using a measured mono->realtime offset.
    realtime_capture = v4l2_buf_ts + rospy.get_param("~mono_to_realtime_offset", 0.0)
    msg.header.stamp = rospy.Time.from_sec(realtime_capture)
    return msg

The LiDAR path is analogous: write the firmware sweep time into PointCloud2.header.stamp rather than letting the driver default to publish time. Expected output: both topics' header.stamp now track physical capture, so the delta between a LiDAR sweep and the nearest camera frame reflects real timing, not graph scheduling jitter.

3. Broker the streams with ApproximateTimeSynchronizer #

message_filters is the synchronization primitive. ExactTime requires identical sequence IDs and stamps, which is unattainable across independent clocks at different native rates. ApproximateTimeSynchronizer runs a sliding-window queue with a configurable slop, making it the standard for heterogeneous fusion. For 10 Hz LiDAR and 30 Hz camera, slop=0.005 (5 ms) balances precision against pairing yield.

import rospy
import message_filters
from sensor_msgs.msg import PointCloud2, Image

MAX_QUEUE_SIZE = 15
SYNC_SLOP = 0.005   # 5 ms pairing window

def init_sync_node():
    rospy.init_node("lidar_cam_synchronizer", anonymous=True)

    lidar_sub = message_filters.Subscriber("/lidar_points", PointCloud2)
    cam_sub = message_filters.Subscriber("/camera/image_raw", Image)

    sync = message_filters.ApproximateTimeSynchronizer(
        [lidar_sub, cam_sub],
        queue_size=MAX_QUEUE_SIZE,
        slop=SYNC_SLOP,
        allow_headerless=False,   # never pair a message that lacks a stamp
    )
    sync.registerCallback(sync_callback)

    rospy.loginfo(f"Synchronizer up: queue={MAX_QUEUE_SIZE}, slop={SYNC_SLOP}s")
    rospy.spin()

Key parameters: allow_headerless=False forbids pairing any message without a stamp — a silent correctness trap; queue_size bounds memory and must cover at least one slop window of the faster sensor (here ≥ ceil(30 Hz × slop) frames plus headroom). Expected output: sync_callback fires once per matched pair, in stamp order, only when a LiDAR sweep and camera frame land within 5 ms of each other.

4. Gate pairs on a drift threshold #

Even matched pairs can drift if the clock discipline slips or a queue saturates. Enforce an explicit drift bound inside the callback and drop violators with a throttled warning, so a backlogged queue cannot pass stale pairs to projection.

DRIFT_THRESHOLD = 0.008   # 8 ms hard reject bound

def validate_timestamps(lidar_msg: PointCloud2, cam_msg: Image) -> bool:
    dt = abs(lidar_msg.header.stamp.to_sec() - cam_msg.header.stamp.to_sec())
    if dt > DRIFT_THRESHOLD:
        rospy.logwarn_throttle(2.0, f"Temporal drift {dt:.4f}s > {DRIFT_THRESHOLD}s — dropping pair")
        return False
    return True

def sync_callback(lidar_msg: PointCloud2, cam_msg: Image):
    if not validate_timestamps(lidar_msg, cam_msg):
        return
    # Pair is within budget: hand off to projection / HD-map generation.
    # Zero-copy buffer handling belongs at the C++ bridge for production throughput;
    # Python here is orchestration only.

The slop window in step 3 admits pairs; the DRIFT_THRESHOLD here is the stricter safety gate that catches discipline slip. Keep DRIFT_THRESHOLD ≥ SYNC_SLOP so the synchronizer, not the gate, does the routine matching. Expected output: in-budget pairs continue to projection; out-of-budget pairs are dropped with at most one warning every 2 s.

Verification & Acceptance Criteria #

Confirm alignment with explicit metrics, not visual inspection:

• Clock offset: phc2sys -m must hold offset within ±1 µs on a stable segment; a climbing offset means the master is unreachable or the NIC PHC is undisciplined.
• Stamp monotonicity: rostopic echo /lidar_points/header/stamp --noarr and the camera equivalent must increase monotonically — a backward jump signals a clock reset or a driver that reverted to publish-time stamping.
• Pairing yield: accepted pairs per second should approach the LiDAR rate (≈10 Hz). A collapse to near zero means slop is smaller than the residual offset; widen it only after re-checking PTP.
• Reprojection residual: project synchronized LiDAR points onto camera imagery against a known calibration target and measure edge alignment.

import numpy as np

def reprojection_residual_px(proj_pts: np.ndarray, target_edges: np.ndarray) -> float:
    # proj_pts, target_edges: (N, 2) pixel coordinates of matched edge points
    residual = float(np.sqrt(np.mean(np.sum((proj_pts - target_edges) ** 2, axis=1))))
    assert residual <= 0.5, f"reprojection residual {residual:.3f} px > 0.5 px budget"
    return residual

Acceptance: stamp delta ≤0.005 s per accepted pair, drift gate rejecting >0.008 s, and reprojection residual ≤0.5 px. A residual above budget on aligned timestamps points to uncorrected rolling-shutter skew or extrinsic drift rather than a timing fault.

Common Errors & Fixes #

sync_callback never fires — the two topics carry stamps from different clock domains (e.g. camera still on publish time, LiDAR on firmware time), so no pair falls inside slop. Verify both stamps with rostopic echo .../header/stamp and confirm step 2 is actually rewriting them before widening slop.

Pairs arrive but reprojection shears straight poles — this is rolling-shutter skew, not a timestamp bug. Rows are exposed sequentially, so one header.stamp cannot describe the whole frame. Switch to a global-shutter sensor, or apply per-row temporal interpolation in the projection stage keyed to row readout time.

Drift warnings spike under heavy logging — a saturated message_filters queue introduces latency that mimics clock drift. Lower queue_size to bound buffering, move heavy work off the callback thread, and check CPU headroom; do not raise DRIFT_THRESHOLD to silence it.

phc2sys offset grows without bound — ptp4l is not actually disciplining the PHC, usually because the NIC lacks hardware timestamping or the segment has no reachable master. Confirm ethtool -T <iface> reports hardware tx/rx, and check the PTP master is on the same L2 segment before falling back to software timestamping.

FAQ #

Why not just use ExactTimeSynchronizer? #

ExactTime requires identical header stamps to the nanosecond. A 10 Hz LiDAR and a 30 Hz camera run on independent hardware clocks with different native periods, so their stamps never coincide exactly and the synchronizer emits no pairs. ApproximateTimeSynchronizer matches nearest stamps within a slop window — the only workable policy for heterogeneous sensors.

Is NTP good enough instead of PTP? #

No. NTP carries software-path jitter on the order of milliseconds and cannot use NIC hardware timestamping, so its offset alone can exceed the entire LiDAR-camera budget. IEEE 1588 PTPv2 disciplines the NIC PHC in hardware and holds sub-microsecond offset on a stable segment, which is required before message-level matching means anything.

How do I pick the slop value? #

Start near half the slower sensor's period plus the measured residual clock offset. For 10 Hz LiDAR and 30 Hz camera, 0.005 s (5 ms) balances pairing yield against alignment error. Too small drops most pairs; too large admits poorly aligned pairs that surface as reprojection error.

Why do straight poles look sheared after projection even when timestamps match? #

Rolling-shutter cameras expose rows sequentially, so the top and bottom of a frame are captured milliseconds apart. A single header stamp cannot represent that, so during ego-motion vertical structures shear. Use a global-shutter sensor, or apply per-row temporal interpolation in the projection stage keyed to the row readout time.

Related #

• Handling Coordinate Drift in Multi-Sensor Setups — correcting the spatial drift that remains once timestamps are coherent.
• Building Async Sensor Fusion Pipelines with Celery — scaling synchronized pairs through an asynchronous processing backend.
• Point Cloud Registration with the ICP Algorithm in Python — registering the LiDAR sweeps these aligned pairs feed.
• LiDAR and Camera Temporal Synchronization — the synchronization architecture this step plugs into.

Up one level: LiDAR and Camera Temporal Synchronization.