BEV:车轮接地点车辆修正3D框位置精度

一、背景

3D目标检测任务中，标注数据（伪标签和人工标注）或模型预测的车辆3D坐标存在位置精度不准（航向角和位置精度）的情况，特别是旁车道车辆位置不准不准造成目标障碍物有压线或切入的趋势，影响行车安全。2D框检测算法非常成熟（yolo系列），考虑在图像上检测车轮2D位置，通过相机内外参和车身参数将检测出的车轮接地点（接地点可以利用相似三角形将相机坐标系中的点投影到BEV坐标系下）投影到BEV坐标系下，对车辆3D坐标的yaw角和横向位置进行修复。

二、代码

输入参数

# 输入参数举例，以front_long_camera为例
# 示例相机标定的pitch角
front_long_camera_pitch_angle = 3.1397834 
#自车车轮直径
vehicle_param_wheeldiameter = 0.786
# 示例相机到坐标原点的高度（坐标原点一般是自车后轴中心）
front_long_camera_pose_translation_z = 1.1628705
# 示例相机到地面高度
front_long_camera_ground_height = front_long_camera_pose_translation_z + vehicle_param_wheeldiameter / 2
# 示例相机内参 (fx, fy, cx, cy)
front_long_camera_K = np.array([[7329.6445, 0., 1915.2565],
                                [0., 7330.802, 1079.506],
                                [0., 0., 1]])
# 示例相机外参（平移和旋转参数）
front_long_camera_translation = [1.982978, -0.18810384, 1.1628705]
front_long_camera_rotation = [-0.49923673, 0.49701047, -0.5016547, 0.5020815]
# 图像中的轮胎位置 (像素坐标)，通过2D模型得到
wheel1_xmin, wheel1_ymin, wheel1_xmax, wheel1_ymax = [3364.4599609375, 1882.9097900390625, 3637.5610961914062, 2160.0]
wheel2_xmin, wheel2_ymin, wheel2_xmax, wheel2_ymax = [2750.3476333618164, 1678.0156860351562, 2905.0335388183594, 2159.3624267578125]
wheel1 = [(wheel1_xmin + wheel1_xmax) / 2, wheel1_ymax]  # 图像中的轮胎1接地点坐标
wheel2 = [(wheel2_xmin + wheel2_xmax) / 2, wheel2_ymax]  # 图像中的轮胎2接地点坐标

依赖的功能函数

import numpy as np
import math

def pixel2cam(wheel, K, pitch_angle=0, camera_ground_height=1.55):
    """
    将图像坐标转换为相机坐标系中的 3D 坐标。
    参数：
    - wheel: 图像中的像素坐标，格式为 (u, v)
    - K: 相机的内参矩阵 (3x3)
    - pitch_angle: 相机俯仰角 (默认 0)
    - camera_ground_height: 相机到地面的高度 (默认 1.55 米)
    返回：
    - cam_points: 相机坐标系中的 3D 坐标
    """
    # 将图像坐标转换为相机坐标系的 3D 点
    x, y = wheel
    pt_m = np.array([x, y, 1]).reshape(3, 1)
    # 相机内参的逆矩阵
    intrinsic_params_inverse = np.linalg.inv(K)
    # 反向投影，得到相机坐标系中的点
    org_camera_point = intrinsic_params_inverse @ pt_m
    # 计算俯仰角的旋转矩阵
    cos_pitch = math.cos(pitch_angle)
    sin_pitch = math.sin(pitch_angle)
    pitch_matrix = np.array([[1, 0, 0], [0, cos_pitch, sin_pitch], [0, -sin_pitch, cos_pitch]])
    # 应用旋转矩阵
    rotate_point = pitch_matrix @ org_camera_point
    # 缩放因子，保证 Z 轴的深度与地面高度一致
    scale = -camera_ground_height / rotate_point[1]  # 使用相机坐标系中的 y 坐标 (深度)
    # 计算 3D 坐标
    cam_points = scale * org_camera_point.flatten()

    return cam_points

def quaternion_to_euler(q):
    """
    四元数 (w, x, y, z) 转欧拉角 (roll, pitch, yaw)
    输出：欧拉角 (弧度制)
    """
    w, x, y, z = q
    # 计算 Roll
    sin_r_cos_p = 2 * (w * x + y * z)
    cos_r_cos_p = 1 - 2 * (x**2 + y**2)
    roll = np.arctan2(sin_r_cos_p, cos_r_cos_p)
    # 计算 Pitch
    sin_p = 2 * (w * y - z * x)
    pitch = np.arcsin(sin_p)  # 限制范围 [-1, 1]
    # 计算 Yaw
    sin_y_cos_p = 2 * (w * z + x * y)
    cos_y_cos_p = 1 - 2 * (y**2 + z**2)
    yaw = np.arctan2(sin_y_cos_p, cos_y_cos_p)

    return np.array([roll, pitch, yaw])


def quaternion2Rot(q): 
    """
    功能: 四元数转旋转矩阵
    q: 四元素
    return R: 旋转矩阵
    """
    q0,q1,q2,q3=q 
    R=np.array([[1-2*(q2**2+q3**2),2*(q1*q2-q3*q0),2*(q1*q3+q2*q0)], 
                     [2*(q1*q2+q3*q0),1-2*(q1**2+q3**2),2*(q2*q3-q1*q0)], 
                     [2*(q1*q3-q2*q0),2*(q2*q3+q1*q0),1-2*(q1**2+q2**2)]]) 
    return R 

def getPose(translation, rotation):
    """功能: 获取设备的旋转平移矩阵

    Args:
        translation: 平移参数['x', 'y', 'z']
        rotation: 四元素参数['w', 'x', 'y', 'z']

    Returns:
        tranformation: 根据四元素和平移量计算出来的旋转平移矩阵
    """
    tranformation = np.identity(4)
    if rotation.ndim == 1:
        tranformation[0:3, 0:3] = quaternion2Rot(rotation) 
    elif rotation.ndim == 2:
        tranformation[0:3, 0:3] = rotation
    tranformation[0][3] = translation[0]
    tranformation[1][3] = translation[1]
    tranformation[2][3] = translation[2]
    return tranformation

def transformPoints(points, transformation):
    """
    功能: 坐标转换
    points: 点(n,3)
    transformation: 旋转平移矩阵(4,4)

    return points: 坐标转换之后的点
    """
    points = np.dot(transformation[0:3, 0:3], points.transpose()).transpose()
    points += transformation[0:3, 3]
    return points

def angle_between_points(x1, y1, x2, y2):
    # 计算方向向量的 y 和 x 分量
    delta_x = x2 - x1
    delta_y = y2 - y1
    # 使用反正切函数计算弧度
    angle_radians = math.atan2(delta_y, delta_x)
    return angle_radians

def normalize_angle(angle):
    """
    将角度归一化到 [-π, π] 范围。
    """
    while angle > math.pi:
        angle -= 2 * math.pi
    while angle < -math.pi:
        angle += 2 * math.pi
    return angle

# 通过车轮接地点修正3D框示例
def rectify_original_data(original_position_y, original_dimension_width, original_yaw, wheel_rectify_data, current_gt_ids, position_y_threshold = 0.15, yaw_threshold = 0.05):
    #小车后视镜距离车身0.2米左右，打车后视镜距离车身0.9米左右
    car_rearview_mirror_distance = 0.2
    big_car_rearview_mirror_distance = 0.9
    width_diff = abs(abs(original_position_y - wheel_rectify_data[current_gt_ids]["two_ego_point_mean_y"]) - original_dimension_width / 2)
  #wheel_label classes = ['LEFT_FRONT', 'LEFT_REAR', 'RIGHT_FRONT', 'RIGHT_REAR', 'MID']
    wheel_label = wheel_rectify_data[current_gt_ids]["wheel_label"]
    if wheel_label == 5:
        width_diff_flag = 0
    elif wheel_label == 4:
        width_diff_flag = 0
    elif wheel_label < 2 :
        width_diff_flag = -1
    else: 
        width_diff_flag = 1
    if abs(width_diff - car_rearview_mirror_distance) < position_y_threshold:
        new_position_y = original_position_y + width_diff_flag * np.sign(original_yaw) * abs(width_diff - car_rearview_mirror_distance)
    elif abs(width_diff - big_car_rearview_mirror_distance) < position_y_threshold:
        new_position_y = original_position_y + width_diff_flag * np.sign(original_yaw) * abs(width_diff - big_car_rearview_mirror_distance)
    else:
        new_position_y = original_position_y
    
    two_ego_point_angle = wheel_rectify_data[current_gt_ids]["two_ego_point_angle"]
    two_ego_point_angle_2 = normalize_angle(two_ego_point_angle - math.pi)  #补角
    if abs(two_ego_point_angle - original_yaw) > yaw_threshold and abs(two_ego_point_angle_2 - original_yaw) > yaw_threshold:
        new_yaw = original_yaw
    else:
        if abs(two_ego_point_angle - original_yaw) <= yaw_threshold:
            new_yaw = two_ego_point_angle
        else:
            new_yaw = two_ego_point_angle_2
     
    return new_position_y, new_yaw

def find_closest_points_by_y(points_info):
    """
    根据第二个坐标 y 的值找出距离最近的两个点
    :param points: 点的列表，每个点是一个 numpy 数组
    :return: 距离最近的点对和对应的 y 距离
    """
    min_distance = float('inf')
    closest_pair = None
    closest_indices = None

    indexed_points_info = list(enumerate(points_info))
    # 遍历所有点对的组合
    for (i1, p1), (i2, p2) in combinations(indexed_points_info, 2):
        y1 = p1["wheel_cam_ego"][1]  # 提取第一个点的 y 坐标
        y2 = p2["wheel_cam_ego"][1]  # 提取第二个点的 y 坐标
        distance = abs(y1 - y2)  # 计算 y 坐标的绝对差值
        if distance < min_distance:
            min_distance = distance
            closest_pair = (p1, p2)
            closest_indices = (i1, i2)  # 保存索引
    return closest_pair, min_distance, closest_indices

def get_ego_wheel_data(camera_info, gt_lidar_info, original_wheel_data_info):
    ego_wheel_data_info = {}
    for box_id, original_box_info in original_wheel_data_info.items():
        if len(original_box_info) > 1:
            ego_wheel_data_info[str(box_id)] = {}
            ego_wheel_point_info = []
            for current_original_box_info_i in original_box_info:
                current_wheel = [(current_original_box_info_i["wheel_xmin"] + current_original_box_info_i["wheel_xmax"]) / 2, current_original_box_info_i["wheel_ymax"]]
                current_wheel_cam = pixel2cam(current_wheel, camera_info["front_long_camera_K"], \
                                                camera_info["front_long_camera_pitch_angle"], \
                                                    camera_info["front_long_camera_ground_height"])
                current_wheel_cam_ego = transformPoints(np.array(current_wheel_cam), \
                                                        np.array(camera_info["front_long_camera_transform"]))
                current_wheel_ego_point_info = {}
                current_wheel_ego_point_info["wheel_cam_ego"] = current_wheel_cam_ego
                current_wheel_ego_point_info["obs_class"] = current_original_box_info_i["obs_class"]
                current_wheel_ego_point_info["wheel_label"] = current_original_box_info_i["wheel_label"]
                ego_wheel_point_info.append(current_wheel_ego_point_info)
            if len(ego_wheel_point_info) > 2:
                new_ego_wheel_point_info, two_ego_point_min_distance, two_ego_point_closest_indices = find_closest_points_by_y(ego_wheel_point_info)
                ego_wheel_point = (ego_wheel_point_info[two_ego_point_closest_indices[0]]["wheel_cam_ego"], ego_wheel_point_info[two_ego_point_closest_indices[0]]["wheel_cam_ego"])
            else:
                ego_wheel_point = (ego_wheel_point_info[0]["wheel_cam_ego"], ego_wheel_point_info[1]["wheel_cam_ego"])
                two_ego_point_min_distance = abs(ego_wheel_point_info[0]["wheel_cam_ego"][1] - ego_wheel_point_info[1]["wheel_cam_ego"][1])
                two_ego_point_closest_indices = (0, 1)
                new_ego_wheel_point_info = ego_wheel_point_info
            two_ego_point_angle = angle_between_points(ego_wheel_point[0][0], \
                                                        ego_wheel_point[0][1], \
                                                        ego_wheel_point[1][0], \
                                                        ego_wheel_point[1][1])
            ego_wheel_data_info[str(box_id)]["ego_wheel_point_info"] = ego_wheel_point_info
            ego_wheel_data_info[str(box_id)]["new_ego_wheel_point_info"] = new_ego_wheel_point_info
            ego_wheel_data_info[str(box_id)]["two_ego_point_angle"] = two_ego_point_angle
            ego_wheel_data_info[str(box_id)]["two_ego_point_min_distance"] = two_ego_point_min_distance
            ego_wheel_data_info[str(box_id)]["obs_class"] = ego_wheel_point_info[0]["obs_class"]
            ego_wheel_data_info[str(box_id)]["two_ego_point_mean_y"] = (ego_wheel_point[0][1] + ego_wheel_point[1][1]) / 2
            #classes = ['LEFT_FRONT', 'LEFT_REAR', 'RIGHT_FRONT', 'RIGHT_REAR', 'MID']
            wheel_label_0 = ego_wheel_point_info[two_ego_point_closest_indices[0]]["wheel_label"]
            wheel_label_1 = ego_wheel_point_info[two_ego_point_closest_indices[1]]["wheel_label"]
            if wheel_label_0 == 4 or wheel_label_1 == 4:
                if wheel_label_0 == 4 and wheel_label_1 == 4:   #两个中间车轮
                    current_wheel_label = 4
                elif wheel_label_0 == 4 and wheel_label_1 != 4:
                    current_wheel_label = wheel_label_1
                else:
                    current_wheel_label = wheel_label_0
            elif wheel_label_0 < 2 and wheel_label_1 < 2:
                current_wheel_label = 0
            elif wheel_label_0 >= 2 and wheel_label_1 >= 2:
                current_wheel_label = 2
            else: #一个左轮一个右轮
                current_wheel_label = 5
            ego_wheel_data_info[str(box_id)]["wheel_label"] = current_wheel_label
            
    return ego_wheel_data_info

计算流程

#构建相机的外参矩阵（旋转和平移）
front_long_camera_transform = getPose(np.array(front_long_camera_translation), np.array(front_long_camera_rotation))

# 将图像坐标转换为相机坐标系中的 3D 坐标。
wheel1_cam = pixel2cam(wheel1, front_long_camera_K, front_long_camera_pitch_angle, front_long_camera_ground_height)
wheel2_cam = pixel2cam(wheel2, front_long_camera_K, front_long_camera_pitch_angle, front_long_camera_ground_height)

# 将相机坐标系下的点转换到自车坐标系
wheel1_cam_ego = transformPoints(np.array(wheel1_cam), np.array(front_long_camera_transform))
wheel2_cam_ego = transformPoints(np.array(wheel2_cam), np.array(front_long_camera_transform))

# 计算两点连线的角度（yaw角）
two_ego_point_angle = angle_between_points(wheel1_cam_ego[0], wheel1_cam_ego[1], wheel2_cam_ego[0], wheel2_cam_ego[1])

#计算车轮接地点中点
two_ego_point_mean_y = (wheel1_cam_ego[1] + wheel2_cam_ego[1]) / 2

# 通过车轮接地点修正3D框示例
new_position_y, new_yaw = rectify_original_data(vehicle_position[1], vehicle_dimension[0], vehicle_yaw, wheel_rectify_data, current_gt_ids)

位置修复策略说明：

1）yaw：在自车坐标系计算两个车轮接地点的夹角（弧度），夹角或夹角补角与原始gt的yaw角差值的绝对值大于角度阈值（0.05弧度=2.866°）则不修改原始gt的yaw角，若夹角与原始gt的yaw角差值绝对值小于角度阈值则用夹角替换原始gt的yaw角，若夹角补角与原始gt的yaw角差值绝对值小于角度阈值则用夹角补角替换原始gt的yaw角；
2）横向位置：小车后视镜距离车身car_rearview_mirror_distance0.2米左右，打车后视镜距离车身big_car_rearview_mirror_distance0.9米左右。根据检测出的距离最近的车轮的属性classes = [‘LEFT_FRONT’, ‘LEFT_REAR’, ‘RIGHT_FRONT’, ‘RIGHT_REAR’, ‘MID’]，判断位置调整方向，两轮都是中间车轮时，无法判断车轮方向，width_diff_flag = 0，两轮一左一右时，width_diff_flag = 0，有一个以上为左轮时，width_diff_flag = -1，有一个以上为右轮时，width_diff_flag = 1；自车前方车辆有同向车辆和对向车辆，可以根据原始gt的yaw角正负值判断，一起用于位置调整方向判断，最终目的，计算的车辆宽度减去后视镜距离的差值绝对值后与原始gt的宽度差值绝对值小于阈值position_y_threshold（小车与大车的阈值都设为0.15米）时，以自车坐标系前进方向视角，目标车辆位于自车左侧，将目标车辆gt中心点向左移，目标车辆位于自车右侧，将目标车辆gt中心点向右移；具体计算逻辑如下，计算两个车轮接地点横向坐标的中点y_mean，计算y_mean与原始gt横向边界的差值width_diff，原始gt的中心点横向坐标为original_position_y，修复的中心点横向坐标new_position_y，

if abs(width_diff - car_rearview_mirror_distance) < position_y_threshold:
        new_position_y = original_position_y + width_diff_flag * np.sign(original_yaw) * abs(width_diff - car_rearview_mirror_distance)
    elif abs(width_diff - big_car_rearview_mirror_distance) < position_y_threshold:
        new_position_y = original_position_y + width_diff_flag * np.sign(original_yaw) * abs(width_diff - big_car_rearview_mirror_distance)
    else:
        new_position_y = original_position_y

二、拓展

车轮矫正策略（rectify_original_data函数），可以根据2D图像车轮检测效果和自己数据的质量对阈值和调整方法进行改进。

将3D框画到图像上可视化代码：

import cv2
import torch
import numpy as np

def check_numpy_to_torch(x):
    if isinstance(x, np.ndarray):
        return torch.from_numpy(x).float(), True
    return x, False

def rotate_points_along_z(points, angle):
    """
    Args:
        points: (B, N, 3 + C)
        angle: (B), angle along z-axis, angle increases x ==> y
    Returns:

    """
    points, is_numpy = check_numpy_to_torch(points)
    angle, _ = check_numpy_to_torch(angle)

    cosa = torch.cos(angle)
    sina = torch.sin(angle)
    zeros = angle.new_zeros(points.shape[0])
    ones = angle.new_ones(points.shape[0])
    rot_matrix = torch.stack((
        cosa,  sina, zeros,
        -sina, cosa, zeros,
        zeros, zeros, ones
    ), dim=1).view(-1, 3, 3).float()
    points_rot = torch.matmul(points[:, :, 0:3], rot_matrix)
    points_rot = torch.cat((points_rot, points[:, :, 3:]), dim=-1)
    return points_rot.numpy() if is_numpy else points_rot


def boxes_to_corners_3d(boxes3d):
    """
        7 -------- 4
       /|         /|
      6 -------- 5 .
      | |        | |
      . 3 -------- 0
      |/         |/
      2 -------- 1
    Args:
        boxes3d:  (N, 7) [x, y, z, dx, dy, dz, heading], (x, y, z) is the box center
    Returns:
    """
    boxes3d, is_numpy = check_numpy_to_torch(boxes3d)
    template = boxes3d.new_tensor((
        [1, 1, -1], [1, -1, -1], [-1, -1, -1], [-1, 1, -1],
        [1, 1, 1], [1, -1, 1], [-1, -1, 1], [-1, 1, 1],
    )) / 2
    corners3d = boxes3d[:, None, 3:6].repeat(1, 8, 1) * template[None, :, :]
    corners3d = rotate_points_along_z(corners3d.view(-1, 8, 3).float(), boxes3d[:, 6]).view(-1, 8, 3)
    corners3d += boxes3d[:, None, 0:3]
    return corners3d.numpy() if is_numpy else corners3d

def lidar2camera(position: list, lv_trans: tuple, vc_trans: tuple):
    lidar_position = np.array(position).reshape(3, 1)
    lv_R, lv_T = lv_trans
    vehicle_position = np.matmul(lv_R, lidar_position) + lv_T
    
    vc_R, vc_T = vc_trans
    camera_postion = np.matmul(vc_R, vehicle_position) + vc_T
    return list(camera_postion.reshape(-1)), list(vehicle_position.reshape(-1))

def draw_line(img, pt0, pt1, width, height, color, line_width):
    if 0 <= int(pt0[0]) < width and 0 <= int(pt0[1]) < height \
        and 0 <= int(pt1[0]) < width and 0 <= int(pt1[1]) < height:
        cv2.line(img, (int(pt0[0]), int(pt0[1])), 
            (int(pt1[0]), int(pt1[1])), color, line_width)

def draw_3dbbox_in_image(name, cv_img, fov, lidar_boxes3d_list=None, vehicle_boxes3d_list=None,
                        lv_trans=None, vc_trans=None, camera_intrinsics=None, distort=None, 
                        color = (0, 255, 0), 
                        score=None, 
                        cam_key=None,
                        bbox_show_height=True,
                        show_score = True,
                        show_heading = True,
                        ):
    if lidar_boxes3d_list is None and vehicle_boxes3d_list is None:
        print("error: lidar_boxes3d_list is None, vehicle_boxes3d_list is None")
        return
    if lidar_boxes3d_list and vehicle_boxes3d_list:
        print("error: lidar_boxes3d_list and vehicle_boxes3d_list is  both not None!")
        return
    height, width  = cv_img.shape[0:2]
    if lidar_boxes3d_list is not None:
        lidar_position = lidar_boxes3d_list[0][:3]
        lidar_boxes3d = torch.tensor(lidar_boxes3d_list)
        # 雷达坐标系x朝左，y朝后，z朝上，，传入的对应的lidar_yaw角为与x轴的夹角
        # 其lidar_yaw角转车辆坐标系(x朝前，y朝左)需要+lv_yaw的操作,即车辆系yaw角=lidar_yaw+lv_yaw
        # 传入的position(x, y, z) is of the box center, not bootom center
        lidar_boxes3d = boxes_to_corners_3d(lidar_boxes3d)
        camera_center, vehicle_center = lidar2camera(lidar_position, lv_trans, vc_trans)
    

    cor_fov = np.rad2deg(np.arctan2(camera_center[0], camera_center[2]))*2
    if abs(cor_fov)+3 > fov: return
    # print("cam_key： ", cam_key, lidar_boxes3d_list, lidar_boxes3d, __file__) # =================ct:===========
    # 把每个lidar的3d点转换到相机上
    corner_3d_gts = []
    for i in range(9): # 最后一維是中心点
        corner_3d_camera = None
        if lidar_boxes3d_list is not None and i < 8:
            corner_3d_camera, _ = lidar2camera(lidar_boxes3d[0][i], lv_trans, vc_trans)
        if i == 8:corner_3d_camera = camera_center
        
        if "fisheye" in cam_key:
            corner_3d_gt, _ = cv2.fisheye.projectPoints(np.array([[corner_3d_camera]]), 
                                                        (0, 0, 0), (0, 0, 0), 
                                                        camera_intrinsics, 
                                                        np.array(distort))
            
        else:
            corner_3d_gt, _ = cv2.projectPoints(np.array(corner_3d_camera), 
                                                np.array((0,0,0), dtype=np.float32).reshape(3,1), 
                                                np.array((0,0,0),dtype=np.float32).reshape(3,1), 
                                                camera_intrinsics, 
                                                np.array(distort))
        # draw_3dbbox_in_image 支持 过滤无效的投影点！！！
        Xc,Yc,Zc = corner_3d_camera[0]/corner_3d_camera[2], corner_3d_camera[1]/corner_3d_camera[2], corner_3d_camera[2]
        # z=1平面上的入射点，与x轴的夹角和y轴的夹角
        arctanx = np.arctan(np.abs(Xc)) / np.pi * 180
        arctany = np.arctan(np.abs(Yc)) / np.pi * 180
        corner_3d_gt = corner_3d_gt[0][0]
        valid_mask = (corner_3d_gt[0] >= 0) & (corner_3d_gt[0] < width) \
                        & (corner_3d_gt[1] >= 0) & (corner_3d_gt[1] < height) & (Zc > 0.01)
        if valid_mask==False: corner_3d_gt = [-1, -1]
        corner_3d_gts.append(corner_3d_gt) 
    """
        7 -------- 4
       /|         /|
      6 -------- 5 .
      | |        | |
      . 3 -------- 0
      |/         |/
      2 -------- 1
    [0, 1, 4, 5]為正前方
    """
    ########################################### 3D框在圖像上的绘制 #############################
    line_width = 1
    cv2.circle(cv_img, (int(corner_3d_gts[8][0]), int(corner_3d_gts[8][1])), 2, (0, 0, 255),3) # 3D框中心點
    draw_line(cv_img, corner_3d_gts[0], corner_3d_gts[1], width, height, color, line_width)
    draw_line(cv_img, corner_3d_gts[0], corner_3d_gts[3], width, height, color, line_width)
    draw_line(cv_img, corner_3d_gts[1], corner_3d_gts[2], width, height, color, line_width)
    draw_line(cv_img, corner_3d_gts[2], corner_3d_gts[3], width, height, color, line_width)
    if bbox_show_height:
        draw_line(cv_img, corner_3d_gts[0], corner_3d_gts[4], width, height, color, line_width)
        draw_line(cv_img, corner_3d_gts[1], corner_3d_gts[5], width, height, color, line_width)
        draw_line(cv_img, corner_3d_gts[3], corner_3d_gts[7], width, height, color, line_width)
        draw_line(cv_img, corner_3d_gts[2], corner_3d_gts[6], width, height, color, line_width)
        draw_line(cv_img, corner_3d_gts[4], corner_3d_gts[5], width, height, color, line_width)
        draw_line(cv_img, corner_3d_gts[4], corner_3d_gts[7], width, height, color, line_width)
        draw_line(cv_img, corner_3d_gts[5], corner_3d_gts[6], width, height, color, line_width)
        draw_line(cv_img, corner_3d_gts[6], corner_3d_gts[7], width, height, color, line_width)
    if show_heading:
        ########################################### 立方体前面四边形heading:# draw `X' in the front
        draw_line(cv_img, corner_3d_gts[0], corner_3d_gts[5], width, height, (241, 101, 72), line_width)
        draw_line(cv_img, corner_3d_gts[1], corner_3d_gts[4], width, height, (241, 101, 72), line_width)
    if show_score:
        info_str = ""
        if score: info_str += f"{score:.2f}"
        cv2.putText(cv_img, info_str, (int(corner_3d_gts[4][0]+1), int(corner_3d_gts[4][1])-7), 
                    cv2.FONT_HERSHEY_DUPLEX, 0.9, color, thickness=line_width)