【radar_camera_fusuion】毫米波雷达与摄像头时间戳对齐数据融合简单实现

此方法使用基于距离和位置的算法来匹配毫米波雷达和相机的目标，将雷达和摄像头统一到一个坐标系，再对数据在时间戳上进行对齐操作，然后遍历所有相机目标，计算雷达目标和相机目标中心位置之间的距离。如果距离小于匹配半径，就将这个匹配结果保存到一个匹配结果列表中。

import numpy as np
import time 

# 毫米波雷达和相机转换的同一个坐标系下，匹配目标
def process_fusion_data(radar_targets , camera_targets):
    # 外参标定得到的变换矩阵
    transform_matrix = np.array([[1.0, 0.0, 0.0, 10.0],
                                [0.0, 1.0, 0.0, 20.0],
                                [0.0, 0.0, 1.0, 30.0],
                                [0.0, 0.0, 0.0, 1.0]])

    # 初始化匹配结果列表
    match_result = []
    
    #匹配半径
    match_radius = 200
    
    # 遍历雷达目标
    for radar_target in radar_targets:
        # 将毫米波雷达目标转换到相机坐标系下
        radar_target_camera = transform_matrix @ np.array([radar_target[0]*np.cos(np.deg2rad(radar_target[1])),
                                                        radar_target[0]*np.sin(np.deg2rad(radar_target[1])),
                                                        0.0, 1.0])
        radar_target_camera = radar_target_camera[:-1] / radar_target_camera[-1]

        #其中，transform_matrix为相机和毫米波雷达之间的变换矩阵，@符号表示矩阵乘法，
        # [:-1]表示取除了最后一个元素之外的所有元素，/表示元素相除。
        # 在将毫米波雷达目标转换到相机坐标系下时，需要考虑变换矩阵中的平移部分，
        # 即将毫米波雷达目标的坐标转换到相机坐标系下。
        
        # 遍历相机目标
        for camera_target in camera_targets:
            # 判断目标是否在匹配半径内
            if np.linalg.norm(camera_target[0] - radar_target_camera[:2]) < match_radius:
                # 如果在匹配半径内，则将匹配结果保存到列表中
                match_result.append((radar_target, camera_target))
                return match_result

# 获取雷达数据并获取时间戳
def get_radar_data():
    # 从雷达设备获取数据
    # 毫米波雷达预处理后的目标数据（距离，角度，速度）
    radar_data = np.array([[10.0, 30.0, 2.0],
                            [12.0, 45.0, 3.0],
                            [15.0, 90.0, 1.5]])

    # 获取当前时间戳
    timestamp = 1676450497.8695247
    #timestamp = time.time()

    # 将雷达数据和时间戳返回
    return radar_data, timestamp

# 获取相机数据并获取时间戳
def get_camera_data():
    # 从相机设备获取数据
    # 相机目标数据（像素坐标，宽度，高度）
    camera_data = np.array([[(100, 200), 50, 50],
                            [(300, 400), 80, 80],
                            [(500, 600), 30, 30]],dtype=object)

    # 获取当前时间戳
    timestamp = 1676450497.8695247
    #timestamp = time.time()

    # 将相机数据和时间戳返回
    return camera_data, timestamp

# 数据融合
def data_fusion():
    # 初始化雷达和相机数据
    radar_data = None
    camera_data = None

    # 初始化时间戳
    radar_timestamp = 0
    camera_timestamp = 0

    # 获取雷达数据和时间戳
    radar_data, radar_timestamp = get_radar_data()

    # 获取相机数据和时间戳
    camera_data, camera_timestamp = get_camera_data()

    if radar_timestamp == camera_timestamp:
        # 此时两个时间戳应该是对齐的，将数据保存
        radar_data = radar_data
        camera_data = camera_data
        timestamp = radar_timestamp

    # 如果雷达数据时间戳比相机数据时间戳早，则等待获取相机数据
    if radar_timestamp < camera_timestamp:
        time.sleep(camera_timestamp - radar_timestamp)
        new_camera_data, new_camera_timestamp = get_camera_data()
        camera_data = new_camera_data
        timestamp = new_camera_timestamp

    # 如果雷达数据时间戳比相机数据时间戳晚，则等待获取雷达数据
    if radar_timestamp > camera_timestamp:
        time.sleep(radar_timestamp - camera_timestamp)
        new_radar_data, new_radar_timestamp = get_radar_data()
        radar_data = new_radar_data
        timestamp = new_radar_timestamp

    # 对雷达数据和相机数据进行融合处理
    fusion_data = process_fusion_data(radar_data, camera_data)

    # 打印匹配结果
    for match in fusion_data:
        print("Time: {},Radar Target: {}, Camera Target: {}".format(timestamp, match[0], match[1]))

    # 返回融合数据和时间戳
    return fusion_data, timestamp


if __name__ == '__main__':
    data = data_fusion()
    print(data)