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

此方法使用基于距离和位置的算法来匹配毫米波雷达和相机的目标，先将雷达目标从极坐标系转换到笛卡尔坐标系，然后遍历所有相机目标，计算雷达目标和相机目标中心位置之间的距离。如果距离小于匹配半径，就将这个匹配结果保存到一个匹配结果列表中。

import numpy as np

# 雷达目标数据（距离，角度，速度）
radar_targets = np.array([[10.0, 30.0, 2.0],
                          [12.0, 45.0, 3.0],
                          [15.0, 90.0, 1.5]])

# 相机目标数据（位置，宽度，高度）
camera_targets = np.array([[(100, 200), 50, 50],
                           [(300, 400), 80, 80],
                           [(500, 600), 30, 30]],dtype=object)

# 匹配半径
match_radius = 100.0

# 初始化匹配结果列表
match_result = []

# 遍历雷达目标
for radar_target in radar_targets:
    # 计算目标的坐标
    x = radar_target[0] * np.cos(np.deg2rad(radar_target[1]))
    y = radar_target[0] * np.sin(np.deg2rad(radar_target[1]))
    
    # 遍历相机目标
    for camera_target in camera_targets:
        # 计算目标的中心位置
        center_x, center_y = camera_target[0]
        
        # 判断目标是否在匹配半径内
        if np.sqrt((x - center_x)**2 + (y - center_y)**2) < match_radius:
            # 如果在匹配半径内，则将匹配结果保存到列表中
            match_result.append((radar_target, camera_target))

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