📚 课程目录第3章视频帧时间戳同步

3. 视频帧时间戳同步

任务说明

运动相机中，视频帧由ISP输出，陀螺仪数据由IMU中断驱动采集，两者时钟源不同且存在启动延迟差异。本案例实现一个时间戳同步模块，将视频帧到达时间与陀螺仪采样时间统一到同一时间基准（以视频帧首帧时间为0点），并输出每个视频帧对应的陀螺仪插值数据，为后续防抖算法提供精确的时间对齐输入。

核心实现

#include <stdint.h>
#include <vector>
#include <algorithm>
#include <cmath>

struct GyroSample {
    double t;           // 陀螺仪时间戳（秒，相对系统启动）
    float gx, gy, gz;   // 角速度（rad/s）
};

struct VideoFrame {
    double t;           // 视频帧时间戳（秒，相对系统启动）
    uint32_t frame_id;
};

struct SyncOutput {
    double frame_t;     // 对齐后帧时间（首帧为0）
    float gx, gy, gz;   // 该帧时刻的插值角速度
};

class TimestampSynchronizer {
public:
    // 输入：视频帧时间戳序列 + 陀螺仪采样序列
    // 输出：每个视频帧对应的同步时间与插值陀螺仪数据
    std::vector<SyncOutput> sync(
        const std::vector<VideoFrame>& frames,
        const std::vector<GyroSample>& gyro)
    {
        std::vector<SyncOutput> result;
        if (frames.empty() || gyro.size() < 2) return result;

        double t0 = frames.front().t;  // 首帧时间作为零点

        for (const auto& f : frames) {
            SyncOutput out;
            out.frame_t = f.t - t0;

            // 在陀螺仪时间轴上找到包围帧时刻的两个采样点
            auto it = std::upper_bound(gyro.begin(), gyro.end(), f.t,
                [](double t, const GyroSample& g) { return t < g.t; });

            if (it == gyro.begin()) {
                // 帧时间早于第一个陀螺仪采样，取最近采样
                out.gx = gyro.front().gx;
                out.gy = gyro.front().gy;
                out.gz = gyro.front().gz;
            } else if (it == gyro.end()) {
                // 帧时间晚于最后一个采样，取最后采样
                out.gx = gyro.back().gx;
                out.gy = gyro.back().gy;
                out.gz = gyro.back().gz;
            } else {
                // 线性插值
                const GyroSample& g1 = *(it - 1);
                const GyroSample& g2 = *it;
                double dt = g2.t - g1.t;
                if (dt < 1e-9) dt = 1e-9;
                double w = (f.t - g1.t) / dt;  // 权重

                out.gx = g1.gx + w * (g2.gx - g1.gx);
                out.gy = g1.gy + w * (g2.gy - g1.gy);
                out.gz = g1.gz + w * (g2.gz - g1.gz);
            }
            result.push_back(out);
        }
        return result;
    }
};

样例数据与调用

#include <cstdio>

int main() {
    // 模拟数据：视频帧30fps，陀螺仪200Hz
    std::vector<VideoFrame> frames;
    for (int i = 0; i < 5; i++) {
        frames.push_back({0.033 * i + 0.005, (uint32_t)i});  // 首帧偏移5ms
    }

    std::vector<GyroSample> gyro;
    for (int i = 0; i < 20; i++) {
        double t = i * 0.005;  // 5ms间隔
        gyro.push_back({t, 0.1f * i, -0.05f * i, 0.02f * i});
    }

    TimestampSynchronizer sync;
    auto out = sync.sync(frames, gyro);

    for (const auto& o : out) {
        printf("frame_t=%.3f gx=%.3f gy=%.3f gz=%.3f\n",
               o.frame_t, o.gx, o.gy, o.gz);
    }
    return 0;
}

预期输出

frame_t=0.000 gx=0.100 gy=-0.050 gz=0.020
frame_t=0.033 gx=0.760 gy=-0.380 gz=0.152
frame_t=0.066 gx=1.360 gy=-0.680 gz=0.272
frame_t=0.099 gx=1.960 gy=-0.980 gz=0.392
frame_t=0.132 gx=2.560 gy=-1.280 gz=0.512

以上输出由代码逻辑直接推出：首帧时间0.005s对应陀螺仪第1个采样点（t=0.005），后续帧按线性插值计算。

验证方法

编译运行：g++ -o sync_test sync_test.cpp && ./sync_test。判定标准：

首帧输出frame_t=0.000，且陀螺仪值等于t=0.005s处的采样值（gx=0.1）；
后续帧时间戳单调递增且间隔约0.033s；
插值结果介于相邻陀螺仪采样值之间，无跳变。

关键说明

本实现假设视频帧与陀螺仪时间戳均基于同一系统时钟（如启动后单调递增的毫秒计数），仅需消除启动延迟与采样率差异；
若两个传感器使用独立时钟源，需先通过PTP或软件锁相环统一时钟，不在本案例范围内；
插值采用线性模型，适用于采样间隔内角速度近似线性变化的场景；若需更高精度可改用样条插值，但会增加计算量。