import numpy as np

# 实验数据
angle_diff_mean = -0.09949351795361913  # 角度差均值
angle_diff_std = 0.014604546858801595   # 角度差标准差
angular_velocity = 2.0  # 机器人旋转的角速度 (rad/s)

# 计算时间戳差异
timestamp_diff = angle_diff_mean / angular_velocity  # 单位：秒

print(f"时间戳差异: {timestamp_diff * 1000:.2f} ms")

# 假设我们有一个二维码摄像头的时间戳列表和里程计的时间戳列表
qr_timestamps = [1629460000.0, 1629460000.1, 1629460000.2, 1629460000.3]
odometry_timestamps = [1629460000.0, 1629460000.1, 1629460000.2, 1629460000.3]

# 调整二维码摄像头的时间戳
adjusted_qr_timestamps = [t + timestamp_diff for t in qr_timestamps]

# 打印调整后的时间戳
for original, adjusted in zip(qr_timestamps, adjusted_qr_timestamps):
    print(f"原始时间戳: {original:.6f}, 调整后时间戳: {adjusted:.6f}")

# 假设我们有一个二维码数据和里程计数据的列表
qr_data = [
    {"timestamp": 1629460000.0, "pose": {"x": 0.0, "y": 0.0, "theta": 0.0}},
    {"timestamp": 1629460000.1, "pose": {"x": 0.1, "y": 0.0, "theta": 0.1}},
    {"timestamp": 1629460000.2, "pose": {"x": 0.2, "y": 0.0, "theta": 0.2}},
    {"timestamp": 1629460000.3, "pose": {"x": 0.3, "y": 0.0, "theta": 0.3}}
]

odometry_data = [
    {"timestamp": 1629460000.0, "pose": {"x": 0.0, "y": 0.0, "theta": 0.0}},
    {"timestamp": 1629460000.1, "pose": {"x": 0.1, "y": 0.0, "theta": 0.1}},
    {"timestamp": 1629460000.2, "pose": {"x": 0.2, "y": 0.0, "theta": 0.2}},
    {"timestamp": 1629460000.3, "pose": {"x": 0.3, "y": 0.0, "theta": 0.3}}
]

# 调整二维码数据的时间戳
for data in qr_data:
    data["timestamp"] += timestamp_diff

# 打印调整后的二维码数据
for data in qr_data:
    print(f"调整后二维码数据: 时间戳: {data['timestamp']:.6f}, 位姿: {data['pose']}")

# 对比调整后的二维码数据和里程计数据
for qr, odometry in zip(qr_data, odometry_data):
    print(f"二维码时间戳: {qr['timestamp']:.6f}, 里程计时间戳: {odometry['timestamp']:.6f}")

解释

1. 计算时间戳差异：通过实验得到的角度差均值和机器人旋转的角速度，计算出时间戳差异。
2. 调整二维码摄像头的时间戳：将二维码摄像头的时间戳加上计算出的时间戳差异，使其与里程计的时间戳对齐。
3. 打印调整后的时间戳：输出调整前后的二维码摄像头时间戳，以便验证调整效果。
4. 调整二维码数据的时间戳：将二维码数据中的时间戳进行调整。
5. 对比调整后的数据：输出调整后的二维码数据和里程计数据的时间戳，验证两者是否对齐。

通过这种方法，你可以确保二维码摄像头和里程计的时间戳同步，从而提高系统的精度和可靠性。