雷达目标跟踪标注的数据格式转换为MOT格式

1. 点云 标注

GitHub – Earthwings/annotate: Create 3D labelled bounding boxes in RViz

2. 目标跟踪MOT数据 标注 格式

frame, id, x, y, width, height, conf(置信度), -1, -1, -1

这些坐标都是在二维图像上的坐标，因此需要将annotate.yaml 标注数据提取，然后再转换到图像坐标系

雷达坐标系一般是：

      x
      |
      |
y—— ——z(朝向外)

图像坐标系一般是指：

o —— ——&gt; x(width)
|
|
y(height)

还是比较好转换的，这里给出代码：

import yaml
import numpy as np


def quaternion_to_rotation_matrix(quaternion):
    q = np.array(quaternion)
    q /= np.linalg.norm(q)
    rotation_matrix = np.array([
        [1 - 2*q[2]**2 - 2*q[3]**2, 2*q[1]*q[2] - 2*q[0]*q[3], 2*q[1]*q[3] + 2*q[0]*q[2]],
        [2*q[1]*q[2] + 2*q[0]*q[3], 1 - 2*q[1]**2 - 2*q[3]**2, 2*q[2]*q[3] - 2*q[0]*q[1]],
        [2*q[1]*q[3] - 2*q[0]*q[2], 2*q[2]*q[3] + 2*q[0]*q[1], 1 - 2*q[1]**2 - 2*q[2]**2]
    ])
    return rotation_matrix

# 读取 YAML 文件
with open("annotate_all.yaml", "r") as file:
    yaml_data = yaml.safe_load(file)

# 提取并按照 frame_id 排列
entries = {}
for track_data in yaml_data["tracks"]:
    for entry in track_data["track"]:
        timestamp = entry["header"]["stamp"]["secs"] + entry["header"]["stamp"]["nsecs"] / 1e9
        if timestamp not in entries:
            entries[timestamp] = []
           
        #雷达坐标系下 
        # length, width, and height
        quaternion = np.array([
            entry["rotation"]["x"],
            entry["rotation"]["y"],
            entry["rotation"]["z"],
            entry["rotation"]["w"]
        ])
        
        translation = np.array([
            entry["translation"]["x"],
            entry["translation"]["y"],
            entry["translation"]["z"]
        ])
          
        w = entry["box"]["width"]
        h = entry["box"]["height"]
        l = entry["box"]["length"]
        label = entry["label"]
        
        rotation_matrix = quaternion_to_rotation_matrix(quaternion)
        
        corner1 = np.dot(rotation_matrix, np.array([translation[0] - l / 2.0, translation[1] - w / 2.0, 0]))
        
        ##图像坐标系下
        x = -corner1[1] #(-y1, -x1)
        y = -corner1[0]
        
        width = w
        height = l
        
        track_id = track_data["id"]

        entries[timestamp].append({
            "id": track_id,
            "label": label,
            "x": x,
            "y": y,
            "width": width,
            "height": height
        })

# 将提取的数据按照 frame_id 排序并写入文本文件
output_file_path = "gtbox.txt"
timestamp_counter = 0
with open(output_file_path, "w") as output_file:
    for timestamp, data_list in sorted(entries.items()):
        timestamp_counter += 1
        for entry in data_list:
            output_file.write(f"{timestamp_counter},{entry['id']},{entry['x']},{entry['y']},{entry['width']},{entry['height']},-1,-1,-1,-1n")

print(f"提取的数据已按照时间戳写入到 {output_file_path}")