      
# coding:utf-8
import os
import glob
import json
import re
from copy import deepcopy
import math
import cv2
import copy
import matplotlib.cm as cm
import numpy as np
from pyquaternion import Quaternion
import random
from multiprocessing import Pool, cpu_count
import traceback
# 前11个颜色对应车辆类别，颜色区别明显
# det_cls_index: car, suv, pickup, van, bus, medium_car, truck, tanker, large_truck, construction_vehicle, special_vehicle
colors = [(255, 0, 0),        # 0: car - 红色
          (0, 255, 0),        # 1: suv - 绿色
          (0, 0, 255),        # 2: pickup - 蓝色
          (255, 255, 0),      # 3: van - 黄色
          (0, 255, 255),      # 4: bus - 青色
          (255, 0, 255),      # 5: medium_car - 洋红色
          (255, 165, 0),      # 6: truck - 橙色
          (128, 0, 255),      # 7: tanker - 紫色
          (50, 205, 50),      # 8: large_truck - 酸橙绿
          (255, 105, 180),    # 9: construction_vehicle - 热粉色
          (0, 191, 255),      # 10: special_vehicle - 深天蓝
          (240, 32, 160), (128, 128, 128),
          (204, 161, 141),(101, 134, 179),(89, 109, 61),(141, 137, 194),(132, 228, 208),
          (249, 118, 35),(80, 91, 182),(222, 91, 125),(91, 63, 123),(173, 232, 91),
          (255, 164, 26),(44, 56, 142),(74, 148, 81),(179, 42, 50),(250, 226, 21),(191, 81, 160),
          (6, 142, 172),(252, 252, 252),(230, 230, 230),(200, 200, 200),(143, 143, 142),(100, 100, 100),(50, 50, 50),
          (64, 224, 208), (255, 140, 0), (186, 85, 211), (255, 20, 147),
          (72, 209, 204), (218, 112, 214), (255, 215, 0), (127, 255, 212),
          (220, 20, 60), (46, 139, 87), (255, 99, 71)]  # 扩充到40个颜色

subcls = {
        'kNegative' : 0,        # 背景
        'kBus' : 1,             # 大巴
        'kCar' : 2,             # 小轿车,suv
        'kMiniBus' : 3,         # 面包车
        'kBucketTruck' : 4,     # 斗卡
        'kContainerTruck' : 5,  # 箱卡
        'kTricycle' : 6,        # 三轮车
        'kTanker' : 7,          # 油罐车,晒水车（车身带有圆形,椭圆形,半圆形的罐）
        'kCementTankTruck' : 8, # 水泥罐车
        'kPickup' : 9,          # 皮卡
        'kSedimentTruck' : 10,  # 渣土车
        'kIveco' : 11,          # 依维柯
        'kSpecialCar' : 12,     # 异型车
        'kCityAuto' : 13,       # 市政车
        'kVehicleUnknown' : 14, # 未知车辆
        'kWrecker' : 15,        # 小型拖车，清障车
        'kFlatTransporter' : 16,# 大型平板拖车
        'kTractorHead' : 17,    # 卡车车头,牵引车车头
        'kSpecialFlatTransporter' : 18,  # 大型特种拖车（相对于大型平板拖车会多挡板）
        'kCarCarrier' : 19,  # 轿运车
        # 盖布车（车身上的货物全部被布盖着，同时对车辆尾部进行了遮挡）
        'kTruckWithTrap' : 20,
        'kEngineeringVehicle' : 21,  # 工程车
        'kSweeper' : 22,             # 扫地车
        'kRoadServiceCar' : 23,      # 道路维修车
        'kSanitationTruck' : 24,     # 环卫车
        'kGarbageTruck' : 25,        # 垃圾车
}
det_cls_index = [
            "car", "suv", "pickup", "van", "bus", "medium_car",
            "truck", "tanker", "large_truck", "construction_vehicle",
            "special_vehicle", "unknown",
            'pedestrian', 'bicycle', "bicyclist",
            "motorcycle", "motorcyclist", "tricycle", "tricyclist",
            'traffic_light_bbox', 'traffic_light_bulb',
            'traffic_sign', 'animal', 'movable_object',
            'warning_triangle', 'traffic_cone', 'water_barrier', 'crash_barrel',
            'movable_barrier', 'bollard', 'sphere_bollard', 'cube_bollard',
            'cylinder_bollard', 'construction_barrier', 'other_barrier',
            'road_barrier_unknown', "wheel", "plate", "face"
        ]
def readcalib(calibpath):
    calib_params, cam_intrinsic, distortion_param, cam_rectify, _, _ = build_camera_config(calibpath)
    Vehiclepos = np.array(calib_params['pos'])
    VehiclePitch, VehicleYaw, VehicleRoll = calib_params['pitch'],calib_params['yaw'],calib_params['roll']
    # camera_yaw = calib_params['yaw']
    # camera_pitch = calib_params['pitch']
    cu = calib_params['cu']
    cv = calib_params['cv']
    focal_u = calib_params['focal_u']
    focal_v = calib_params['focal_v']
    vanish_x = cu + focal_u * np.tan(VehicleYaw * np.pi/180)
    vanish_y = cv -  focal_v * np.tan(VehiclePitch * np.pi/180)
    centerx ,centery = vanish_x,vanish_y + 0
    roi0 = [0,int(centery - 880 / 2.0),1920,int(centery + 880 / 2.0)]
    roi1 = [int(centerx - 768 / 2.0),int(centery - 352 / 2.0),int(centerx + 768 / 2.0),int(centery + 352 / 2.0)]
    # opt.roi[0] = [0,120,1920,1080]
    # opt.roisize[0] = [704,352]

    # opt.roisize[1] = [704,352]
    # centerx ,centery = vanish_x,vanish_y + 0
    # roi_rect = [int(centerx - 704 / 2.0), int(centery - 352 / 2.0), int(centerx + 704/2.0), int(centery + 352/2.0)]
    # opt.roi[1] = roi_rect
    return cam_intrinsic,distortion_param, cam_rectify,Vehiclepos,VehiclePitch,VehicleYaw, VehicleRoll

def build_camera_config(calib_file):
    calib_params = json.load(open(calib_file, "r"))
    cam_intrinsic = np.array([calib_params['focal_u'], 0, calib_params['cu'], 0, calib_params['focal_v'], calib_params['cv'], 0, 0, 1], dtype=np.float64).reshape(3, 3)
    distortion_param = np.array(calib_params['distort_coeffs'])
    image_w = int(calib_params.get('image_width') or round(calib_params['cu'] * 2))
    image_h = int(calib_params.get('image_height') or round(calib_params['cv'] * 2))
    if len(distortion_param) == 5:
        cam_rectify, _ = cv2.getOptimalNewCameraMatrix(cam_intrinsic, distortion_param, (image_w, image_h), 1, (image_w, image_h))
    elif len(distortion_param) == 4:
        cam_rectify = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(cam_intrinsic, distortion_param, [image_w, image_h], np.eye(3), balance=1)
    else:
        raise ValueError(f"Unsupported distort_coeffs length {len(distortion_param)} in {calib_file}")
    return calib_params, cam_intrinsic, distortion_param, cam_rectify, image_w, image_h

def infer_camera_name_from_path(path):
    matches = re.findall(r'camera\d+', os.path.normpath(path), flags=re.IGNORECASE)
    return matches[-1].lower() if matches else None

def resolve_camera_calib_path(calib_root, reference_path=''):
    if os.path.isfile(calib_root):
        return calib_root

    camera_name = infer_camera_name_from_path(reference_path) or infer_camera_name_from_path(calib_root)
    candidate_dirs = [calib_root]
    l2_calib_dir = os.path.join(calib_root, 'L2_calib')
    if os.path.isdir(l2_calib_dir):
        candidate_dirs.insert(0, l2_calib_dir)

    if camera_name:
        for candidate_dir in candidate_dirs:
            candidate_path = os.path.join(candidate_dir, f'{camera_name}.json')
            if os.path.exists(candidate_path):
                return candidate_path

    candidate_paths = []
    for candidate_dir in candidate_dirs:
        candidate_paths.extend(sorted(glob.glob(os.path.join(candidate_dir, 'camera*.json'))))

    if len(candidate_paths) == 1:
        return candidate_paths[0]
    if not candidate_paths:
        raise FileNotFoundError(f'No calibration file found under {calib_root}')
    raise ValueError(
        f'Unable to determine calibration file for {reference_path or calib_root}. '
        f'Candidates: {candidate_paths}'
    )

def resolve_image_path(label_path):
    image_base = label_path.replace('labels', 'images')[0:-4]
    for ext in ('.png', '.jpg', '.jpeg'):
        image_path = image_base + ext
        if os.path.exists(image_path):
            return image_path
    raise FileNotFoundError(f'No image file found for label {label_path}. Tried {image_base}.png/.jpg/.jpeg')

def _project_label_3d_bbox(co, cam_intrinsic, distortion_param):
    if len(co) <= 11:
        return None

    x3d_ori, y3d_ori, z3d_ori, l3d, h3d, w3d, rotation_y = map(float, co[5:12])
    if z3d_ori <= 0:
        return None

    corners = cam_corners_front_rear(
        np.array([x3d_ori, y3d_ori, z3d_ori, l3d, h3d, w3d, rotation_y]),
        'whole',
        0,
    )
    pts2d = []
    for x, y, z in corners:
        if z <= 0:
            return None
        if len(distortion_param) == 5:
            image_coord = xyz_xy(x / z, y / z, distortion_param, cam_intrinsic)
        else:
            image_coord = cam_img_kb(x, y, z, cam_intrinsic, distortion_param)
        pts2d.append([float(image_coord[0]), float(image_coord[1])])

    pts2d = np.array(pts2d, dtype=np.float64)
    return np.array(
        [pts2d[:, 0].min(), pts2d[:, 1].min(), pts2d[:, 0].max(), pts2d[:, 1].max()],
        dtype=np.float64,
    )

def _build_label_box(co, canvas_w, canvas_h):
    cx = float(co[1]) * canvas_w
    cy = float(co[2]) * canvas_h
    box_w = float(co[3]) * canvas_w
    box_h = float(co[4]) * canvas_h
    return np.array([cx - 0.5 * box_w, cy - 0.5 * box_h, cx + 0.5 * box_w, cy + 0.5 * box_h], dtype=np.float64)

def resolve_label_canvas_size(calib_path, image_w, image_h, label_temp_all, cam_intrinsic, distortion_param):
    camera_name = infer_camera_name_from_path(calib_path)
    if camera_name != 'camera2' or (image_w, image_h) != (1920, 1280):
        return image_w, image_h

    candidate_sizes = [(image_w, image_h), (3840, 2160)]
    candidate_scores = {candidate: [] for candidate in candidate_sizes}
    valid_count = 0

    for co in label_temp_all:
        projected_box = _project_label_3d_bbox(co, cam_intrinsic, distortion_param)
        if projected_box is None:
            continue

        projected_center = np.array(
            [(projected_box[0] + projected_box[2]) * 0.5, (projected_box[1] + projected_box[3]) * 0.5],
            dtype=np.float64,
        )
        for candidate in candidate_sizes:
            label_box = _build_label_box(co, candidate[0], candidate[1])
            label_center = np.array(
                [(label_box[0] + label_box[2]) * 0.5, (label_box[1] + label_box[3]) * 0.5],
                dtype=np.float64,
            )
            center_distance = np.linalg.norm(label_center - projected_center)
            size_distance = np.linalg.norm((label_box[2:] - label_box[:2]) - (projected_box[2:] - projected_box[:2]))
            candidate_scores[candidate].append(center_distance + 0.5 * size_distance)
        valid_count += 1
        if valid_count >= 8:
            break

    if not valid_count:
        return image_w, image_h

    best_candidate = min(
        candidate_sizes,
        key=lambda candidate: float(np.mean(candidate_scores[candidate])) if candidate_scores[candidate] else float('inf'),
    )
    return best_candidate

def plot_box(x, img, color=None, label=None, line_thickness=None):
    # Plots one bounding box on image img
    tl = line_thickness or round(0.002 * (img.shape[0] + img.shape[1]) / 2) + 1  # line/font thickness
    color = color or [random.randint(0, 255) for _ in range(3)]
    c1, c2 = (int(x[0]), int(x[1])), (int(x[2]), int(x[3]))
    cv2.rectangle(img, c1, c2, color, thickness=tl, lineType=cv2.LINE_AA)
    if label:
        tf = max(tl - 1, 1)  # font thickness
        t_size = cv2.getTextSize(label, 0, fontScale=tl / 3, thickness=tf)[0]
        c2 = c1[0] + t_size[0], c1[1] - t_size[1] - 3
        cv2.rectangle(img, c1, c2, color, -1, cv2.LINE_AA)  # filled
        cv2.putText(img, label, (c1[0], c1[1] - 2), 0, tl / 3, [225, 255, 255], thickness=tf, lineType=cv2.LINE_AA)

def append_class_legend(img, legend_items):
    if not legend_items:
        return img

    font = cv2.FONT_HERSHEY_SIMPLEX
    font_scale = 0.6
    thickness = 1
    margin_x = 20
    margin_y = 16
    swatch_size = 18
    text_gap = 8
    item_gap = 24
    row_gap = 12
    title = '2D legend'
    title_size = cv2.getTextSize(title, font, font_scale, thickness)[0]
    text_height = cv2.getTextSize('Ag', font, font_scale, thickness)[0][1]
    item_height = max(swatch_size, text_height)
    row_width_limit = img.shape[1] - margin_x

    layout = []
    x = margin_x
    row = 0
    max_row = 0
    for label_cls, label_name, color in legend_items:
        item_text = f'{label_cls}: {label_name}'
        text_size = cv2.getTextSize(item_text, font, font_scale, thickness)[0]
        item_width = swatch_size + text_gap + text_size[0]
        if x + item_width > row_width_limit and x > margin_x:
            row += 1
            x = margin_x
        layout.append((x, row, item_text, color))
        x += item_width + item_gap
        max_row = row

    legend_height = margin_y * 3 + title_size[1] + (max_row + 1) * item_height + max_row * row_gap
    legend_img = np.full((legend_height, img.shape[1], 3), 255, dtype=img.dtype)
    cv2.line(legend_img, (0, 0), (img.shape[1] - 1, 0), (200, 200, 200), 1, cv2.LINE_AA)
    cv2.putText(legend_img, title, (margin_x, margin_y + title_size[1]), font, font_scale, (0, 0, 0), thickness, cv2.LINE_AA)

    base_y = margin_y * 2 + title_size[1]
    for x, row, item_text, color in layout:
        row_y = base_y + row * (item_height + row_gap)
        swatch_top = row_y
        swatch_bottom = row_y + swatch_size
        cv2.rectangle(legend_img, (x, swatch_top), (x + swatch_size, swatch_bottom), color, -1, cv2.LINE_AA)
        cv2.rectangle(legend_img, (x, swatch_top), (x + swatch_size, swatch_bottom), (0, 0, 0), 1, cv2.LINE_AA)
        cv2.putText(legend_img, item_text, (x + swatch_size + text_gap, row_y + swatch_size - 3), font, font_scale, (0, 0, 0), thickness, cv2.LINE_AA)

    return cv2.vconcat([img, legend_img])

T1Q2Dlabel = {'vehicle': 0, 'pedestrian':1, 'bicycle':2, 'rider':3, 'roadblock':4,'cone': 4, 'crashbarrels' :4, 'plasticpile':4, 'bumpingpost':4, 'head':5, 'tsr':6, 'guideboard':7, 'plate':8, 'wheel':9, 'tl_border':10,'tl_wick':11,'tl_num':12}
def detectmap(path):
    # labeldict = {0:'vehicle', 1:'pedestrian', 2:'bicycle', 3:'cone', 4:'warning', 5:'head', 6:'tsr', 7:'guideboard', 8:'plate', 9:'wheel', 10:'tl_border',11:'tl_wick',12:'tl_num'}
    labeldict = {0:'vehicle', 1:'pedestrian', 2:'bicycle', 3:'rider', 4:'roadblock', 5:'head', 6:'tsr', 7:'guideboard', 8:'plate', 9:'wheel', 10:'tl_border',11:'tl_wick',12:'tl_num'}
    textlists = open(path,'r').readlines()
    label_maps = {}
    for text in textlists:
        data = text.strip('\n').split(' ')
        filename = data[0]
        box = [float(data[2]), float(data[3]), float(data[4]),float(data[5])]
        # label = labeldict.get(int(data[1]))
        label = str(data[1])
        # ignore = int(data[6])
        ignore = 1
        if filename not in label_maps:
            label_maps[filename] = []
        label_maps[filename].append((label,box,ignore))

    return label_maps

def get_rot_mat_and_trans(calib_dict):
    su = np.sin(calib_dict['roll'] * np.pi / 180)
    cu = np.cos(calib_dict['roll'] * np.pi / 180)
    sv = np.sin(calib_dict['pitch'] * np.pi / 180)
    cv = np.cos(calib_dict['pitch'] * np.pi / 180)
    sw = np.sin(calib_dict['yaw'] * np.pi / 180)
    cw = np.cos(calib_dict['yaw'] * np.pi / 180)

    rot = np.array([[cv * cw, su * sv * cw - cu * sw, su * sw + cu * sv * cw],
                    [cv * sw, cu * cw + su * sv * sw, cu * sv * sw - su * cw],
                    [-sv, su * cv, cu * cv]])
    rot = np.transpose(rot)

    rot_yxz = np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]])
    rot_ego2cam = np.matmul(rot_yxz, rot)
    # rot_cam2ego = np.transpose(rot_ego2cam)  # equals np.linalg.inv

    pos = np.array(calib_dict['pos'])
    trans_ego2cam = -1 * np.matmul(rot_ego2cam, pos)

    ego_to_cam = np.zeros((4, 4))
    ego_to_cam[:3, :3] = rot_ego2cam
    ego_to_cam[:3, 3] = trans_ego2cam
    ego_to_cam[3, 3] = 1

    # cam_to_ego = np.linalg.inv(ego_to_cam)

    return ego_to_cam

def Vehicle2OrigImg(pts3d, ego2cam, distort_param, K, origW, origH):
    pts3d_cam = np.dot(ego2cam[:3, :3],[pts3d[0],pts3d[1],pts3d[2]]) + ego2cam[:3, 3]
    # pts2d_cam = cam_img_kb(pts3d_cam[0],pts3d_cam[1],pts3d_cam[2],K,distort_param)
    if len(distort_param)==5:
        pts2d_cam = xyz_xy(pts3d_cam[0] /pts3d_cam[2], pts3d_cam[1] / pts3d_cam[2], distort_param, K)
    else:
        pts2d_cam = cam_img_kb(pts3d_cam[0],pts3d_cam[1],pts3d_cam[2],K,distort_param)
    pts2d_cam = pts2d_cam[0:2]
    pts2d_cam[0] = np.clip(pts2d_cam[0],0,origW-1)
    pts2d_cam[1] = np.clip(pts2d_cam[1],0,origH-1)
    return pts2d_cam, pts3d_cam

def deal_cutoff(cor,bbox_cam3d,cen2d,cen2d_di,alpha,lidar_to_cam,K,distortion_param,K_new,image_w,image_h,ydis_dict):
    cor = np.array(cor)
    # a = cor[:,0] * K_new[0][0]
    # b = -cor[:,2]*K_new[0][2]
    # c = cor[:,2]*(image_w-1-K_new[0][2])
    x_left = cor[:,0]>=-cor[:,2]*K_new[0][2]/K_new[0][0]                       # (8,)
    x_right = cor[:,0]<=cor[:,2]*(image_w-1-K_new[0][2])/K_new[0][0]
    if (x_left==True).sum()==(x_right==True).sum() and (x_right==True).sum()<8:
        if bbox_cam3d[0]<=-bbox_cam3d[2]*(image_w-1-K_new[0][2])/K_new[0][0]:  ####右边界线
            is_left = True
            # assert bbox_cam3d[0] < -bbox_cam3d[2]*K_new[0][2]/K_new[0][0]
        else:
            is_left = False
            # assert bbox_cam3d[0]>-bbox_cam3d[2]*(image_w-1-K_new[0][2])/K_new[0][0]
    else:
        is_left = (x_left==True).sum()<(x_right==True).sum()             # 左边界被切

    have_in = ((x_left & x_right)[[0,1,4,5]]==True).sum()
    pts2ds_c,_ = cam2pixel(np.array([bbox_cam3d[0], bbox_cam3d[1], bbox_cam3d[2]]), K_new)
    condition_c = np.logical_and((pts2ds_c[1] <= image_h-1) & (pts2ds_c[1] >= 0),(pts2ds_c[0] <= image_w-1) & (pts2ds_c[0] >= 0))

    cutoff = 0
    if have_in ==2 and condition_c == False:
        cutoff = 1
        y_dis = max(0,min(image_h-1,round(K_new[1][2]+K_new[1][1]*(bbox_cam3d[1]/bbox_cam3d[2]))))
        if is_left:
            x_dis = ydis_dict[0][y_dis]
        else:
            x_dis = ydis_dict[1][y_dis]
        z_new = (bbox_cam3d[2]*K_new[0][0]+bbox_cam3d[0]*K_new[0][0]*np.tan(bbox_cam3d[-1]))/((x_dis-K_new[0][2])*np.tan(bbox_cam3d[-1])+K_new[0][0])
        x_new = z_new*(x_dis-K_new[0][2])/K_new[0][0]
        # cen2d = xyz_xy(x_new/z_new,bbox_cam3d[1]/z_new,distortion_param,K)
        cen2d = cam_img_kb(x_new,bbox_cam3d[1],z_new,K,distortion_param)
        x_nc,y_nc = float(cen2d[0]),float(cen2d[1])
        if x_nc<0 or x_nc>image_w-1 or y_nc<0 or y_nc>image_h-1:
            x_nc,y_nc = max(0,min(image_w-1,x_nc)),max(0,min(image_h-1,y_nc))
            img_corners = np.array([x_nc,y_nc]).reshape((-1, 2))
            x_new,y_new = img_cam_kb(img_corners,K,distortion_param)
            x_new,y_new = x_new*z_new,y_new*z_new
            if y_new!=bbox_cam3d[1]:
                bbox_cam3d[4] = bbox_cam3d[4]-(y_new-bbox_cam3d[1])*2
                bbox_cam3d[1] = y_new
        is_l = np.logical_or((x_left & x_right)[[0,1]].all(),(x_left & x_right)[[4,5]].all())
        see_radio = [1,1]
        if is_l:
            l = (bbox_cam3d[3]/2 - np.sqrt(np.sum(np.square(np.array([x_new,bbox_cam3d[1],z_new],dtype=np.float32) - np.array(bbox_cam3d[0:3],dtype=np.float32)))))*2
            assert l<bbox_cam3d[3]
            new_box = [x_new,bbox_cam3d[1],z_new,l,bbox_cam3d[4],bbox_cam3d[5],bbox_cam3d[6]]
            see_radio = [l/bbox_cam3d[3],1]
        else: ###
            w = (bbox_cam3d[5]/2 - np.sqrt(np.sum(np.square(np.array([x_new,bbox_cam3d[1],z_new],dtype=np.float32) - np.array(bbox_cam3d[0:3],dtype=np.float32)))))*2
            assert w<bbox_cam3d[5]
            new_box = [x_new,bbox_cam3d[1],z_new,bbox_cam3d[3],bbox_cam3d[4],w,bbox_cam3d[6]]
            see_radio = [1,w/bbox_cam3d[4]]          
        if min(see_radio) > 0.1:
            # cen2d = xyz_xy(new_box[0]/new_box[2],new_box[1]/new_box[2],distortion_param, K)      # 中心点（x,y,z）;底部中心点(x,y+l/2,z)   1933.3505753743707, 593.9974302913548
            # cen2d_di = xyz_xy(new_box[0]/new_box[2],(new_box[1]+0.5*new_box[4])/new_box[2],distortion_param,K)
            cen2d = cam_img_kb(new_box[0],new_box[1],new_box[2],K,distortion_param)      # 中心点（x,y,z）;底部中心点(x,y+l/2,z)   1933.3505753743707, 593.9974302913548
            cen2d_di = cam_img_kb(new_box[0],(new_box[1]+0.5*new_box[4]),new_box[2],K,distortion_param)

            # assert new_box[2]>0
            alpha = -np.arctan2(new_box[0],new_box[2])+new_box[-1]            # -torch.atan2(torch.tensor(loc[0]), torch.tensor(loc[2]))+torch.tensor(rot[0])
            if alpha<-np.pi:
                alpha+=2*np.pi
            if alpha>np.pi:
                alpha-=2*np.pi
            bbox_cam3d = new_box
    return bbox_cam3d, cen2d,cen2d_di, alpha, cutoff

def rotation_3d_in_axis(points, angles, axis=0):
    ####需要特别注意旋转的轴和旋转矩阵正逆问题 以下为逆时针旋转矩阵
    rot_sin = np.sin(angles)
    rot_cos = np.cos(angles)
    ones = np.ones_like(rot_cos)
    zeros = np.zeros_like(rot_cos)
    if axis == 1:  ####Y
        rot_mat = np.stack([
            np.stack([rot_cos, zeros, -rot_sin]),
            np.stack([zeros, ones, zeros]),
            np.stack([rot_sin, zeros, rot_cos])
        ])
    elif axis == 2:  #####Z
        rot_mat = np.stack([
            np.stack([rot_cos, rot_sin, zeros]),
            np.stack([-rot_sin, rot_cos, zeros]),
            np.stack([zeros, zeros, ones])
        ])
    elif axis == 0: #####X
        rot_mat = np.stack([
            np.stack([ones, zeros, zeros]),
            np.stack([zeros, rot_cos, rot_sin]),
            np.stack([zeros, -rot_sin, rot_cos])
        ])
    else:
        raise ValueError(f'axis should in range [0, 1, 2], got {axis}')

    return np.dot(points, rot_mat)

def create_corner(object = [0,0,0,0,0,0,0], type= 'cam',dtype=np.float32):
    dims = np.array([object[3],object[4],object[5]])    ###lwh   # torch.Size([1, 3])
    corners_norm =np.stack(np.unravel_index(np.arange(8), [2] * 3), axis=1)

    corners_norm = corners_norm[[0, 1, 3, 2, 4, 5, 7, 6]]
    # use relative origin [0.5, 1, 0.5]
    corners_norm = corners_norm -[0.5, 0.5, 0.5]
    corners = dims.reshape([-1, 3]) * corners_norm.reshape([8, 3])
    # rotate around y axisa 
    corners = rotation_3d_in_axis(corners, object[6], axis=2) ######
    corners += np.array(object[:3]).reshape(1, 3)
    return corners

def cam_corners_front_rear(pred3d, facetype,pitch = 0):
    dims = pred3d[3:6]
    corners_norm = np.stack(np.unravel_index(np.arange(8), [2] * 3), axis=1)

    corners_norm = corners_norm[[0, 1, 3, 2, 4, 5, 7, 6]]
    # use relative origin [0.5, 1, 0.5]
    if facetype ==  'tail':
        # front
        corners_norm = corners_norm - [0, 0.5, 0.5]
    elif facetype == 'front':
        # tail
        corners_norm = corners_norm - [1, 0.5, 0.5]
    elif facetype == 'right':
        # left
        corners_norm = corners_norm - [0.5, 0.5, 0]
    elif facetype == 'left':
        # right
        corners_norm = corners_norm - [0.5, 0.5, 1]
    elif facetype == 'whole':
        # center
        corners_norm = corners_norm - [0.5, 0.5, 0.5]
    else:
        raise AssertionError('Non valid face type')
    corners = dims.reshape([1, 3]) * corners_norm.reshape([8, 3])
    # rotate around y axis
    corners = rotation_3d_in_axis(corners, pred3d[6], axis=1)
    ####
    corners = rotation_3d_in_axis(corners, pitch * np.pi / 180.0, axis=0)
    ####
    corners += pred3d[:3].reshape(1, 3)
    return corners
def ious_function(boxes1, boxes2, iom = False):
    # boxes1-(4,),mx4
    boxes1 = np.array(boxes1)
    boxes2 = np.array(boxes2)

    boxes1_area = (boxes1[..., 2] - boxes1[..., 0]) * (boxes1[..., 3] - boxes1[..., 1])
    boxes2_area = (boxes2[..., 2] - boxes2[..., 0]) * (boxes2[..., 3] - boxes2[..., 1])

    left_up = np.maximum(boxes1[..., :2], boxes2[..., :2])
    right_down = np.minimum(boxes1[..., 2:], boxes2[..., 2:])

    inter_section = np.maximum(right_down - left_up, 0.0)
    inter_area = inter_section[..., 0] * inter_section[..., 1]
    if iom:
        union_area = boxes1_area
    else:
        union_area = boxes1_area + boxes2_area - inter_area
    ious = np.maximum(1.0 * inter_area / union_area, np.finfo(np.float32).eps)
    return ious

def get_point_xyz(xyz1,xyz2,num=1):            # num=1为黑色
        (x1,y1,z1),(x2,y2,z2) = xyz1,xyz2
        dis = np.array([abs(x1-x2),abs(y1-y2),abs(z1-z2)])
        dis_idx = dis.argmax()
        res = []
        if dis_idx==0:
            for x in np.concatenate((np.arange(x1,x2,0.01) ,np.arange(x1,x2,-0.01))):
                res.append([x,(x-x1)*(y2-y1)/(x2-x1)+y1,(x-x1)*(z2-z1)/(x2-x1)+z1,num])
        elif dis_idx==1:
            for y in np.concatenate((np.arange(y1,y2,0.01) ,np.arange(y1,y2,-0.01))):
                res.append([(y-y1)*(x2-x1)/(y2-y1)+x1,y,(y-y1)*(z2-z1)/(y2-y1)+z1,num])
        else:
            for z in np.concatenate((np.arange(z1,z2,0.01) ,np.arange(z1,z2,-0.01))):
                res.append([(z-z1)*(x2-x1)/(z2-z1)+x1,(z-z1)*(y2-y1)/(z2-z1)+y1,z,num])
        return res

def cam_img_kb(x,y,z,cam_intrinsic,distort_param):
    # objp=np.array([x/z,y/z,1]).reshape(1,-1,3)
    # rvec = np.array([[[0., 0., 0.]]])
    # tvec = np.array([[[0., 0., 0.]]])
    # image_coord, _ = cv2.fisheye.projectPoints(objp, rvec, tvec,cam_intrinsic, distort_param)
    # image_coord = image_coord.reshape(-1)
    # return image_coord
    camxyz = np.array([x/z, y/z]).reshape(-1,2)
    a = camxyz[...,0]
    b = camxyz[...,1] 
    r = np.sqrt(a * a + b * b)
    theta = np.arctan(r)
    k1 = distort_param[0]
    k2 = distort_param[1]
    k3 = distort_param[2]
    k4 = distort_param[3]
    thetaD = theta * (1 + k1 * theta**2 + k2 * theta**4 + k3 * theta**6 + k4 * theta**8)
    xp = thetaD / r * a
    yp = thetaD / r * b
    fx = cam_intrinsic[0,0]
    fy = cam_intrinsic[1,1]
    cx = cam_intrinsic[0,2]
    cy = cam_intrinsic[1,2]
    imgx = (fx * xp + cx)
    imgy = (fy * yp + cy)
    # mask_2d = (imgx>=0)&(imgx<3840)&(imgy>=0)&(imgy<2160)
    pix_point = np.round(np.concatenate((imgx.reshape((-1,1)),imgy.reshape((-1,1))),axis=-1))
    # pix_point = pix_point[mask_2d].astype(np.int32)
    pix_point = pix_point.astype(np.int32)
    return pix_point.reshape(-1)

def img_cam_kb(img_corners,cam_intrinsic,distort_param):   # nx2
    ones_col = np.ones((img_corners.shape[0], 1), dtype=np.float32)
    img_corners = np.concatenate((img_corners.astype(np.float32),ones_col),axis=-1)  # （n,3）
    k1 = distort_param[0]
    k2 = distort_param[1]
    k3 = distort_param[2]
    k4 = distort_param[3]
    points = np.dot(np.linalg.inv(cam_intrinsic),img_corners.T).T          # (2073600, 3)
    xp = points[:,0] # n
    yp = points[:,1] # n
    thetaD = np.sqrt(xp*xp+yp*yp)
    cam_theta,femask = [],[]
    theta = thetaD
    for i in range(10):
        ftheta = theta * (1 + k1 * theta**2 + k2 * theta**4 + k3 * theta**6 + k4 * theta**8) - thetaD
        ftheta_dao = 1 + 3 * k1 * theta**2 + 5 * k2 * theta**4 + 7 * k3 * theta**6 + 9 * k4 * theta**8
        theta = theta - ftheta/ftheta_dao
        if abs(ftheta)<0.0000001:
            break
        # print(ftheta)
    
    r = np.tan(theta)
    ###################################
    normx = xp * r / thetaD # 4,H,W
    normy = yp * r / thetaD
    ones_col = np.ones((normy.shape[0], 1), dtype=np.float32)
    x3dy3d = np.concatenate((normx.reshape((-1,1)),normy.reshape((-1,1)),ones_col),axis=-1)
    return x3dy3d[0,0], x3dy3d[0,1]

# def img_cam_kb(img_corners,cam_intrinsic,distort_param):   # nx2
#     ones_col = np.ones((img_corners.shape[0], 1), dtype=np.float32)
#     img_corners = np.concatenate((img_corners.astype(np.float32),ones_col),axis=-1)  # （n,3）
#     k1 = distort_param[0]
#     k2 = distort_param[1]
#     k3 = distort_param[2]
#     k4 = distort_param[3]
#     points = np.dot(np.linalg.inv(cam_intrinsic),img_corners.T).T          # (2073600, 3)
#     xp = points[:,0] # n
#     yp = points[:,1] # n
#     thetaD = np.sqrt(xp*xp+yp*yp)
#     cam_theta,femask = [],[]
#     # for d in tqdm(thetaD):
#     for d in thetaD:
#         coeff = np.array([-d,1,0,k1,0,k2,0,k3,0,k4],dtype=np.float64) # k4*x**9+k3*x**7+k2*x**5+k1*x**3+x-d=0
#         success,roots = cv2.solvePoly(coeff)
#         roots = roots.squeeze(1)
#         roots = np.array([root[0] for root in roots if abs(root[1]) < 1e-10]) # 找到唯一实数根解
#         assert len(roots)==1,'come with many theta'
#         if roots[0]>=0 and roots[0] <=np.pi/2:
#             femask.append(True)
#         else:
#             femask.append(False)
#         cam_theta.append(roots[0])
#     r = np.tan(cam_theta)
#     mask3d = np.array(femask)
#     ###################################
#     normx = xp * r / thetaD # 4,H,W
#     normy = yp * r / thetaD
#     ones_col = np.ones((normy.shape[0], 1), dtype=np.float32)
#     x3dy3d = np.concatenate((normx.reshape((-1,1)),normy.reshape((-1,1)),ones_col),axis=-1)
#     x3dy3d = x3dy3d[mask3d]
#     return x3dy3d[0,0], x3dy3d[0,1]

def cam2RectifyImg(x,y, distort_param, K):
    # R = np.array([[0, -1, 0],[-1, 0, 0],[0, 0, 1]])
    # pts3d_cam = np.dot(lidar_to_cam[:3, :3],[pts3d[0],pts3d[1],pts3d[2]]) + lidar_to_cam[:3, 3]
    # pts3d_cam = lidar_to_cam[:3, :3] @ (pts3d.T) + lidar_to_cam[:3, 3].reshape(3, 1)  # 1x3

    pts2d_cam = K @ np.array([x,y,1])
    pts2d_cam[2] = np.clip(pts2d_cam[2],1,500)
    pts2d_cam = (pts2d_cam[:2] / pts2d_cam[2]).T

    pts2d_cam = pts2d_cam[0:2]
    image_h, image_w = 1080,1920
    pts2d_cam[0] = np.clip(pts2d_cam[0],0,image_w-1)
    pts2d_cam[1] = np.clip(pts2d_cam[1],0,image_h-1)

    return pts2d_cam

def xyz_xy(x,y,distort_param,K):
    r = np.sqrt(x * x + y * y)
    r_2 = r * r
    r_4 = r_2 * r_2
    r_6 = r_4 * r_2
    m_distort_coeffs = distort_param
    coeffs = np.zeros(12,dtype=np.float32)
    for i in range(min(m_distort_coeffs.shape[0],12)):
        coeffs[i] = m_distort_coeffs[i]

    radial_ratio = (1 + coeffs[0] * r_2 + coeffs[1] * r_4 + coeffs[4] * r_6) / (1 + coeffs[5] * r_2 + coeffs[6] * r_4 + coeffs[7] * r_6)
    res_x = x * radial_ratio + 2 * coeffs[2] * x * y + coeffs[3] * (r_2 + 2 * pow(x, 2)) + coeffs[8] * r_2 + coeffs[9] * r_4
    res_y = y * radial_ratio + 2 * coeffs[2] * (r_2 + 2 * pow(y, 2)) + coeffs[3] * x * y + coeffs[10] * r_2 + coeffs[11] * r_4
    image_coord = np.dot(K,np.array([res_x,res_y,1])).T
    return image_coord[:2]

def xy_xyz(x,y,m_distort_coeffs,k):
    x = (x - k[0][2])/k[0][0]
    y = (y - k[1][2])/k[1][1]

    coeffs = np.zeros(12,dtype=np.float32)
    for i in range(min(m_distort_coeffs.shape[0],12)):
        coeffs[i] = m_distort_coeffs[i]
    iter_num = 5
    x0,y0 = x,y
    for i in range(iter_num):
        r = np.sqrt(x * x + y * y)
        r_2 = r * r
        r_4 = r_2 * r_2
        r_6 = r_4 * r_2
        inv_radial_ratio = (1 + coeffs[5] * r_2 + coeffs[6] * r_4 + coeffs[7] * r_6) / (1 + coeffs[0] * r_2 + coeffs[1] * r_4 + coeffs[4] * r_6)
        delta_x = 2 * coeffs[2] * x * y + coeffs[3] * (r_2 + 2 * pow(x, 2)) + coeffs[8] * r_2 + coeffs[9] * r_4
        delta_y = 2 * coeffs[2] * (r_2 + 2 * pow(y, 2)) + coeffs[3] * x * y + coeffs[10] * r_2 + coeffs[11] * r_4
        x = (x0 - delta_x) * inv_radial_ratio
        y = (y0 - delta_y) * inv_radial_ratio

    # x = x*k[0][0]+k[0][2]
    # y = y*k[1][1]+k[1][2]
    return x,y

def get_cutoff_corners(cam3d_corners,pts2ds,cam_intrinsic,distortion_param,cam_rectify,imgW,imgH):
    xyz_list = []
    cut_pts2d = np.zeros_like(pts2ds)
    cam3d_corners_update = np.zeros_like(cam3d_corners)
    for (i,j) in [(0, 1), (1, 2), (2, 3), (3, 0),(0,4), (1,5), (2,6),(3,7)]:
        res = get_point_xyz(cam3d_corners[i],cam3d_corners[j],0)
        xyz_list.extend(res)
        if (i,j) in [(0,4), (1,5), (2,6),(3,7)]:
            point = np.array(res)
            pointx_in = np.logical_and(point[:,0]>=-point[:,2]*cam_rectify[0][2]/cam_rectify[0][0],point[:,0]<=point[:,2]*(imgW-1-cam_rectify[0][2])/cam_rectify[0][0])
            pointy_in = np.logical_and(point[:,1]>=-point[:,2]*cam_rectify[1][2]/cam_rectify[1][1],point[:,1]<=point[:,2]*(imgH-1-cam_rectify[1][2])/cam_rectify[1][1])
            pointxy_in = np.logical_and(pointx_in,pointy_in)
            t_point = point[pointxy_in]
            if t_point.shape[0] == 0:
                if len(distortion_param)==5:
                    origimg0 = xyz_xy(point[0,0] /point[0,2], point[0,1] / point[0,2], distortion_param, cam_intrinsic)
                else:
                    origimg0 = cam_img_kb(point[0,0] , point[0,1] , point[0,2],cam_intrinsic,distortion_param)
                cut_pts2d[i] = origimg0[:2]
                cut_pts2d[j] = origimg0[:2]
                continue
            if len(distortion_param)==5:
                point_origimg0 = xyz_xy(t_point[0,0] /t_point[0,2], t_point[0,1] / t_point[0,2], distortion_param, cam_intrinsic)
                point_origimg1 = xyz_xy(t_point[-1,0] /t_point[-1,2], t_point[-1,1] / t_point[-1,2], distortion_param, cam_intrinsic)
            else:
                point_origimg0 = cam_img_kb(t_point[0,0] , t_point[0,1] , t_point[0,2],cam_intrinsic,distortion_param)
                point_origimg1 = cam_img_kb(t_point[-1,0] , t_point[-1,1] , t_point[-1,2],cam_intrinsic,distortion_param)
            cut_pts2d[i] = point_origimg0[:2]
            cut_pts2d[j] = point_origimg1[:2]
            cam3d_corners_update[i] = t_point[0,:3]
            cam3d_corners_update[j] = t_point[-1,:3]
    return   cut_pts2d ,cam3d_corners_update
def cam2pixel(pts3d_cam, K):
    pts2d_cam = K @ pts3d_cam
    Z = pts2d_cam[2]
    pts2d_cam = (pts2d_cam[:2] / Z).T

    return pts2d_cam, Z

def nms_area(boxes,lables,area_thresh=0.9):
    if len(boxes) < 2:
        discard = []
        return discard
    # for i in range(boxes.shape[0]):
    #     if boxes[i][0] < 40 or boxes[i][3] > 3840 - 40:
    #         discard = []
    #         return discard
    rect_boxes =  np.array(boxes)
    x1 = rect_boxes[:,0]
    y1 = rect_boxes[:,1]
    x2 = rect_boxes[:,2]
    y2 = rect_boxes[:,3]
    areas = (y2-y1+1)*(x2-x1+1)
    discard = []
    index = areas.argsort()[::-1]   ##降序
    for idx in index[1:]:
        x11=np.maximum(x1[index[0]],x1[idx])
        y11=np.maximum(y1[index[0]],y1[idx])
        x22=np.minimum(x2[index[0]],x2[idx])
        y22=np.minimum(y2[index[0]],y2[idx])
        w=np.maximum(0,x22-x11+1)
        h=np.maximum(0,y22-y11+1)
        
        overlaps = w*h
        iou=overlaps /areas[idx]
        if iou > area_thresh:
            if lables[index[0]] == lables[idx]:
                discard.append(idx)
            elif lables[index[0]] in ['Pedestrian','Rider'] and lables[idx] in ['Pedestrian','Rider']:
                discard.append(idx)
            elif lables[index[0]] in ['Bicycle','Rider'] and lables[idx] in ['Bicycle','Rider']:
                discard.append(idx)
    return discard

def Lidar2OrigImg(pts3d, lidar_to_cam, distort_param, K, origW, origH):
    pts3d_cam = np.dot(lidar_to_cam[:3, :3],[pts3d[0],pts3d[1],pts3d[2]]) + lidar_to_cam[:3, 3]
    # pts2d_cam = cam_img_kb(pts3d_cam[0],pts3d_cam[1],pts3d_cam[2],K,distort_param)
    if len(distort_param)==5:
        pts2d_cam = xyz_xy(pts3d_cam[0] /pts3d_cam[2], pts3d_cam[1] / pts3d_cam[2], distort_param, K)
    else:
        pts2d_cam = cam_img_kb(pts3d_cam[0],pts3d_cam[1],pts3d_cam[2],K,distort_param)
    pts2d_cam = pts2d_cam[0:2]
    pts2d_cam[0] = np.clip(pts2d_cam[0],0,origW-1)
    pts2d_cam[1] = np.clip(pts2d_cam[1],0,origH-1)
    return pts2d_cam, pts3d_cam

def Lidar2RectifyImg(pts3d, lidar_to_cam, K,origW, origH):
    # R = np.array([[0, -1, 0],[-1, 0, 0],[0, 0, 1]])
    pts3d_cam = np.dot(lidar_to_cam[:3, :3],[pts3d[0],pts3d[1],pts3d[2]]) + lidar_to_cam[:3, 3]
    # pts3d_cam = lidar_to_cam[:3, :3] @ (pts3d.T) + lidar_to_cam[:3, 3].reshape(3, 1)  # 1x3

    pts2d_cam = K @ pts3d_cam
    pts2d_cam[2] = np.clip(pts2d_cam[2],1,500)
    pts2d_cam = (pts2d_cam[:2] / pts2d_cam[2]).T
    pts2d_cam = pts2d_cam[0:2]
    image_h, image_w = origH,origW
    pts2d_cam[0] = np.clip(pts2d_cam[0],0,image_w-1)
    pts2d_cam[1] = np.clip(pts2d_cam[1],0,image_h-1)
    return pts2d_cam, pts3d_cam
def Calculate_vector_angle(vector_a ,vector_b):
    dot_product = np.dot(vector_a, vector_b)
    # 计算两个向量的范数（大小）
    norm_a = np.linalg.norm(vector_a)
    norm_b = np.linalg.norm(vector_b)

    # 计算夹角的余弦值
    cos_angle = dot_product / (norm_a * norm_b)

    # 使用 arccos 计算角度，得到的结果是弧度
    angle_radians = np.arccos(cos_angle)

    # 将弧度转换为度
    angle_degrees = np.degrees(angle_radians)
    return angle_degrees 
def angle2score(angle):
    angle = (angle - 90)/90
    angle = min(max(0,angle),1)
    angle = np.sqrt(angle)
    return angle

def drawPointBox(img, imgW, imgH, rect_corners, colors, thickness=1):
    line_indices = ((4, 5), (5, 6), (6, 7), (7, 4),       # 尾部
                    (0,4), (1,5), (2,6),(3,7),
                    (0, 1), (1, 2), (2, 3), (3, 0), (0, 2), (1, 3))     # 头部(1, 3),  
    
    border_x = imgW ####
    border_y = imgH #####
    for i in range(len(rect_corners)):
        if int(rect_corners[i][0]) < 0 or int(rect_corners[i][0]) > border_x or int(rect_corners[i][1]) < 0 or int(rect_corners[i][1]) > border_y:
            continue    
        # cv2.circle(img,(int(rect_corners[i][0]),int(rect_corners[i][1])),5,(255,0,0),2,cv2.LINE_8,0)
        # cv2.putText(img, str(i), (int(rect_corners[i][0]+5),int(rect_corners[i][1] - 5)), 1, 1, (0, 0, 255), lineType=cv2.LINE_AA)

    corners = rect_corners.astype(np.int32)
    for start, end in line_indices:              
        try:
            if (start,end) in [(0, 1), (1, 2), (2, 3), (3, 0),(0, 2), (1, 3)] and corners.shape[0]==8:
                if int(corners[start, 0]) < 5 or int(corners[start, 0]) > (border_x - 5) or int(corners[start, 1]) < 5 or int(corners[start, 1]) > (border_y - 5):
                    continue 
                if int(corners[end, 0]) < 5 or int(corners[end, 0]) > (border_x - 5) or int(corners[end, 1]) < 5 or int(corners[end, 1]) > (border_y - 5):
                    continue
                cv2.line(img, (corners[start, 0], corners[start, 1]),(corners[end, 0], corners[end, 1]), colors[1], thickness*2,cv2.LINE_AA)
            else:
                cv2.line(img, (corners[start, 0], corners[start, 1]),(corners[end, 0], corners[end, 1]), colors[0], thickness,cv2.LINE_AA)
        except:
            pass
    return img
def showFace2Dbox(img,label_temp_all, cam_intrinsic, distortion_param, cam_rectify,origW,origH,pitch,label_canvas_w=None,label_canvas_h=None):
    if label_canvas_w is None:
        label_canvas_w = origW
    if label_canvas_h is None:
        label_canvas_h = origH
    legend_items = []
    legend_seen = set()
    for co in label_temp_all:
        try:
            label_det_cls,cx,cy,w,h =co[0], float(co[1])*label_canvas_w,float(co[2])*label_canvas_h,float(co[3])*label_canvas_w,float(co[4])*label_canvas_h
            box2d= [cx-0.5*w,cy-0.5*h,cx+0.5*w,cy+0.5*h]
            # 检查 label_det_cls 是否在 det_cls_index 中
            if label_det_cls not in det_cls_index:
                print(f"Warning: label_det_cls '{label_det_cls}' not in det_cls_index, skipping")
                continue

            label_cls = det_cls_index.index(label_det_cls)

            # 检查 label_cls 是否超出 colors 范围
            if label_cls >= len(colors):
                print(f"Warning: label_cls {label_cls} exceeds colors length {len(colors)}, using modulo")
                label_cls = label_cls % len(colors)

            color = colors[label_cls]
            if label_cls < 19:
                box_label = label_det_cls
                line_thickness = 2
            else:
                box_label = None
                line_thickness = 1
            plot_box(box2d, img, label=box_label, color=color, line_thickness=line_thickness)

            legend_key = (label_cls, label_det_cls)
            if legend_key not in legend_seen:
                legend_seen.add(legend_key)
                legend_items.append((label_cls, label_det_cls, color))
        except Exception as e:
            print(f"Error processing label: {e}, co={co}")
            continue
    return append_class_legend(img, legend_items)
def showFace3Dbox(img,label_temp_all, cam_intrinsic, distortion_param, cam_rectify,origW,origH,pitch):
    for co in label_temp_all:
        # try:
        #     label_det_cls,cx,cy,w,h =co[0], float(co[1])*origW,float(co[2])*origH,float(co[3])*origW,float(co[4])*origH
        #     box2d= [cx-0.5*w,cy-0.5*h,cx+0.5*w,cy+0.5*h]

        #     # 检查 label_det_cls 是否在 det_cls_index 中
        #     if label_det_cls not in det_cls_index:
        #         print(f"Warning: label_det_cls '{label_det_cls}' not in det_cls_index, skipping")
        #         continue

        #     label_cls = det_cls_index.index(label_det_cls)

        #     # 检查 label_cls 是否超出 colors 范围
        #     if label_cls >= len(colors):
        #         print(f"Warning: label_cls {label_cls} exceeds colors length {len(colors)}, using modulo")
        #         label_cls = label_cls % len(colors)

        #     plot_box(box2d, img, label=str('%s'%label_cls), color=colors[label_cls], line_thickness=1)
        # except Exception as e:
        #     print(f"Error processing label: {e}, co={co}")
        #     continue
        # if int(label_det_cls) < 11:
        #     show_label_color = 0
        # elif int(label_det_cls) > 11 :
        #     show_label_color = int(label_det_cls) - 11
        # plot_box(box2d, img, label=str('%s'%label_det_cls), color=colors[show_label_color], line_thickness=1)
        # # box2d_1=[[box2d[0],box2d[1]],[box2d[2],box2d[3]]]
        # # box2d_Rectify = cv2.undistortPoints(np.array(box2d_1).reshape(2,1,2), cam_intrinsic, distortion_param, None, cam_rectify).reshape((4,))
        # # plot_box(box2d_Rectify, img, label=str('%s'%label_det_cls), color=colors[label_det_cls], line_thickness=1)
        pts2ds = []
        if len(co)>7:
            x3d_ori,y3d_ori,z3d_ori,l3d,h3d,w3d,rotation_y = float(co[5]),float(co[6]),float(co[7]),float(co[8]),float(co[9]),float(co[10]),float(co[11])
            color = (0,0,0) if rotation_y <= 0 else (0,255,255)
            corners = cam_corners_front_rear(np.array([x3d_ori,y3d_ori,z3d_ori,l3d,h3d,w3d,rotation_y]),'whole',pitch)
            pts2ds = []
            for i in range(corners.shape[0]):
                x,y,z = corners[i]
                if len(distortion_param)==5:
                    image_coord = xyz_xy(x/z,y/z,distortion_param,cam_intrinsic)
                    # image_coord = cam2RectifyImg(x/z,y/z,distortion_param,cam_rectify)
                else:
                    image_coord = cam_img_kb(x,y,z,cam_intrinsic,distortion_param)
                pts2ds.append([image_coord[0],image_coord[1]])
            #####deal cutoff####
            x_left = corners[:,0]>=-corners[:,2]*cam_rectify[0][2]/cam_rectify[0][0]                       # (8,)
            x_right = corners[:,0]<=corners[:,2]*(origW-1-cam_rectify[0][2])/cam_rectify[0][0]
            have_in = ((x_left & x_right)[[0,1,4,5]]==True).sum()
            # po2ds = None
            if have_in >= 1 and have_in < 4:
                cut_pts2d,cam3d_corners_update= get_cutoff_corners(corners,pts2ds,cam_intrinsic,distortion_param,cam_rectify,origW,origH)
                # cut_pts2d,cut_rect,cut_rectify_box3d, po2ds= get_cutoff_corners(corners,pts2ds_new,cam_intrinsic,distortion_param,cam_rectify,imgW,imgH)
                if cut_pts2d is not None:
                    pts2ds = cut_pts2d
            sqe = [6,7,4,5,2,3,0,1]
            pts2ds = np.array(pts2ds)[sqe]
            img = drawPointBox(img, origW, origH,np.array(pts2ds), colors=[(0, 0, 255), color], thickness=1)
            if len(co)==51:
                anchortype_index = ['front','tail','left','right']
                value_3d_4face = co[18:50]
                for jj in range(len(value_3d_4face)):
                    value_3d_4face[jj] = float(value_3d_4face[jj])
                value_3d_4face = np.reshape(value_3d_4face,(4,-1))
                for anchortype in range(len(value_3d_4face)):
                    x3d,y3d,z3d,alpha,xc,yc,score,is_good = value_3d_4face[anchortype]
                    xc,yc = max(0,min(origW-1,float(xc)*origW)),max(0,min(origH-1,float(yc)*origH))
                    if len(distortion_param)==5:
                        x3d_new ,y3d_new = xy_xyz(xc,yc,distortion_param,cam_intrinsic)
                        # x3d_new ,y3d_new = cam2RectifyImg(x/z,y/z,distortion_param,cam_rectify)
                    else:
                        x3d_new ,y3d_new = img_cam_kb(np.array([xc,yc]).reshape((-1, 2)),cam_intrinsic,distortion_param)
                    infer_x3d = x3d_new * z3d
                    infer_y3d = y3d_new * z3d
                    rotation_y = alpha + np.arctan2(infer_x3d, z3d)
                    if is_good == -1:
                        continue
                    if rotation_y<0:
                        color = (0,0,0)
                    else:
                        color = (0,255,255)
                    corners = cam_corners_front_rear(np.array([infer_x3d,infer_y3d,z3d, l3d,h3d,w3d, rotation_y]),anchortype_index[anchortype],pitch)
                    pts2ds_new = []
                    for i in range(corners.shape[0]):
                        x,y,z = corners[i]
                        if len(distortion_param)==5:
                            image_coord = xyz_xy(x/z,y/z,distortion_param,cam_intrinsic)
                            # image_coord = cam2RectifyImg(x/z,y/z,distortion_param,cam_rectify)
                        else:
                            image_coord = cam_img_kb(x,y,z,cam_intrinsic,distortion_param)

                        pts2ds_new.append([image_coord[0],image_coord[1]])
                    #####deal cutoff####
                    x_left = corners[:,0]>=-corners[:,2]*cam_rectify[0][2]/cam_rectify[0][0]                       # (8,)
                    x_right = corners[:,0]<=corners[:,2]*(origW-1-cam_rectify[0][2])/cam_rectify[0][0]
                    have_in = ((x_left & x_right)[[0,1,4,5]]==True).sum()
                    # po2ds = None
                    if have_in >= 1 and have_in < 4:
                        cut_pts2d,cam3d_corners_update= get_cutoff_corners(corners,pts2ds_new,cam_intrinsic,distortion_param,cam_rectify,origW,origH)
                        if cut_pts2d is not None:
                            pts2ds_new = cut_pts2d
                    sqe = [6,7,4,5,2,3,0,1]
                    pts2ds = np.array(pts2ds_new)[sqe]
                    img = drawPointBox(img, origW, origH,np.array(pts2ds), colors=[(0, 0, 255),color], thickness=1)
                    if len(distortion_param)==5:
                        [xc,yc] = xyz_xy(infer_x3d/z3d,infer_y3d/z3d,distortion_param,cam_intrinsic)
                        # [xc,yc] = cam2RectifyImg(infer_x3d/z3d,infer_y3d/z3d,distortion_param,cam_rectify)
                    else:
                        [xc,yc] = cam_img_kb(infer_x3d,infer_y3d,z3d,cam_intrinsic,distortion_param)
                    img = cv2.circle(img, (int(xc),int(yc)), 4, colors[int(anchortype)],-1)
    return img       

def mono3dlabel_trans2train(calib_path,anno):
    det_cls_index = [
            "car", "suv", "pickup", "van", "bus", "medium_car",
            "truck", "tanker", "large_truck", "construction_vehicle",
            "special_vehicle", "unknown",
            'pedestrian', 'bicycle', "bicyclist",
            "motorcycle", "motorcyclist", "tricycle", "tricyclist",
            'traffic_light_bbox', 'traffic_light_bulb',
            'traffic_sign', 'animal', 'movable_object',
            'warning_triangle', 'traffic_cone', 'water_barrier', 'crash_barrel',
            'movable_barrier', 'bollard', 'sphere_bollard', 'cube_bollard',
            'cylinder_bollard', 'construction_barrier', 'other_barrier',
            'road_barrier_unknown', "wheel", "plate", "face"
        ]
    if os.path.exists(anno):
                    
        text_basename =  os.path.basename(anno)
        print(text_basename)
        text_split = text_basename.strip('_').split('_')

        calib_file = resolve_camera_calib_path(calib_path, anno)
        calib_param_vehicle, cam_intrinsic, distortion_param, cam_rectify, origW, origH = build_camera_config(calib_file)
        ego_to_cam = get_rot_mat_and_trans(calib_param_vehicle)

        objs = json.load(open(anno))['asso_list']
        label_temp_all = []
        for obj in objs:
            label_temp = []
            label_2d = obj['camera_mea']
            confidence = label_2d['confidence']
            try:
                label_radar = obj['radar_mea']
            except:
                label_radar=None
            label_lidar = obj['lidar_mea']
            # label_det_cls = det_cls_map[label_2d['cls']]
            ##############
            label_det_cls,label_det_subcls = label_2d['cls'],label_2d['subcls']
            if label_det_cls=='vehicle':
                label_det_cls=label_det_subcls
            label_det_cls = det_cls_index.index(label_det_cls)
            ###############
            if label_det_cls == -1:
                continue
            # sublabel = subcls.get(label_2d['subcls']) ###'kTricycle' : 6        # 三轮车
            box2d = [label_2d['det_pt_x'],label_2d['det_pt_y'],label_2d['det_pt_width'],label_2d['det_pt_height']] 
            box2d = [box2d[0] ,  box2d[1] , box2d[0] + box2d[2], box2d[1] + box2d[3]]

            label_temp=[label_det_cls,min((box2d[0]+box2d[2])/(2*origW),1), min((box2d[1]+box2d[3])/(2*origH),1),min((box2d[2]-box2d[0])/origW,1), min((box2d[3]-box2d[1])/origH,1)]

            # if label_radar is not None:
            #     radar_conf = label_radar['asso_conf']
            #     if radar_conf < 0.5:
            #         continue
                
            #     radar_x3d ,radar_y3d, radar_z3d= label_radar['x'],label_radar['y'],label_radar['z'] ###rear plane
            #     radarpoint = [radar_x3d ,radar_y3d, radar_z3d]
            #     radarpts2d , radarpts3d = Lidar2OrigImg(radarpoint,lidar_to_cam,distortion_param,cam_intrinsic, origW, origH)
            #     cv2.circle(img,(int(radarpts2d[0]),int(radarpts2d[1])),5,(255,0,0),2,cv2.LINE_8,0)
            #     cv2.putText(img, str('%.2f'%radarpts3d[2]), (int(radarpts2d[0]) + 5,int(radarpts2d[1]) - 5), 1, 1, (0, 0, 255))

            if (label_lidar is not None) and label_det_cls<=18:
                obj_lidar = [float(label_lidar[1]),float(label_lidar[2]),float(label_lidar[3]),float(label_lidar[4]),float(label_lidar[5]),float(label_lidar[6]),float(label_lidar[7])]
                l3d,w3d,h3d = obj_lidar[3],obj_lidar[4],obj_lidar[5]
                singleLW = int(float(label_lidar[11]))
                yaw_lidar = obj_lidar[6]

                Rz = np.array([    
                                [np.cos(yaw_lidar), -np.sin(yaw_lidar),   0],
                                [np.sin(yaw_lidar),  np.cos(yaw_lidar),   0],
                                [      0,                0, 1.0],
                            ])  #############lidar Z轴对应 cam  Y轴  顺时针
                    
                RotateZ =np.dot(ego_to_cam[:3, :3],Rz)  ##lidar yaw 累加旋转矩阵
                orientation = Quaternion(matrix=RotateZ)
                rotation_y = -orientation.yaw_pitch_roll[0] # convert the rot to our cam coordinate 

                campts2d , campts3d = Vehicle2OrigImg(np.array([obj_lidar[0],obj_lidar[1],obj_lidar[2]]), ego_to_cam, distortion_param, cam_intrinsic, origW, origH)
                campts2d_di, campts3d_di = Vehicle2OrigImg(np.array([obj_lidar[0],obj_lidar[1],obj_lidar[2] - 0.5 * h3d]),ego_to_cam, distortion_param, cam_intrinsic, origW, origH)
                # campts2d , campts3d = Lidar2OrigImg(np.array([obj_lidar[0],obj_lidar[1],obj_lidar[2]]), lidar_to_cam, distortion_param, cam_intrinsic, origW, origH)
                # campts2d_di, campts3d_di = Lidar2OrigImg(np.array([obj_lidar[0],obj_lidar[1],obj_lidar[2] - 0.5 * h3d]),lidar_to_cam, distortion_param, cam_intrinsic, origW, origH)
                box3d_cam = [campts3d[0], campts3d[1], campts3d[2],l3d,h3d,w3d,rotation_y]
                alpha = -np.arctan2(campts3d[0],campts3d[2]) + rotation_y
                if alpha<-np.pi:
                    alpha+=2*np.pi
                if alpha>np.pi:
                    alpha-=2*np.pi

                x3d_ori,y3d_ori,z3d_ori,l3d,h3d,w3d,rotation_y,alpha,singleLW =  campts3d[0], campts3d[1], campts3d[2],l3d,h3d,w3d,rotation_y,alpha,singleLW
                
                
                label_temp = label_temp + [x3d_ori,y3d_ori,z3d_ori,l3d,h3d,w3d,rotation_y,campts2d[0]/origW,campts2d[1]/origH,campts2d_di[0]/origW,campts2d_di[1]/origH,alpha]
                
                corners = cam_corners_front_rear(np.array(box3d_cam),'whole', pitch=0)
                if label_det_cls > 11:
                    label_temp = label_temp + [-1] + [confidence]
                else:
                    xyz3d_0 =  np.mean(corners[4:8,:],axis=0) ### Front_point
                    xyz3d_1 = np.mean(corners[0:4,:],axis=0) ### Back_point
                    xyz3d_2 = np.mean((corners[1,:],corners[2,:],corners[5,:],corners[6,:]),axis=0) ### Left_point
                    xyz3d_3 = np.mean((corners[0,:],corners[4,:],corners[3,:],corners[7,:]),axis=0) ### Right_point
                    xyz3d_4face = [xyz3d_0,xyz3d_1,xyz3d_2,xyz3d_3]
                    value_3d_4face = []

                    x_left = corners[:,0]>=-corners[:,2]*cam_rectify[0][2]/cam_rectify[0][0]                       # (8,)
                    x_right = corners[:,0]<=corners[:,2]*(origW-1-cam_rectify[0][2])/cam_rectify[0][0]
                    x_down = corners[:,1]<=corners[:,2]*(origH-1-cam_rectify[1][2])/cam_rectify[1][1]
                    have_in = ((x_left & x_right)[[0,1,4,5]]==True).sum()

                    #####################
                    x_front_1 = corners[4:8,0]>=-corners[4:8,2]*cam_rectify[0][2]/cam_rectify[0][0]   
                    x_front_2 = corners[4:8,0]<=corners[4:8,2]*(origW-1-cam_rectify[0][2])/cam_rectify[0][0]
                    have_in_front = ((x_front_1 & x_front_2)[[0,1,2,3]]==True).sum()

                    y_front = corners[4:8,1]<=corners[4:8,2]*(origH-5-cam_rectify[1][2])/cam_rectify[1][1]
                    y_front_center = xyz3d_0[1]<=xyz3d_0[2]*(origH-5-cam_rectify[1][2])/cam_rectify[1][1]

                    x_tail_1 = corners[0:4,0]>=-corners[0:4,2]*cam_rectify[0][2]/cam_rectify[0][0] 
                    x_tail_2 = corners[0:4,0]<=corners[0:4,2]*(origW-1-cam_rectify[0][2])/cam_rectify[0][0]

                    y_tail = corners[0:4,1]<=corners[0:4,2]*(origH-5-cam_rectify[1][2])/cam_rectify[1][1]
                    y_tail_center = xyz3d_1[1]<=xyz3d_1[2]*(origH-5-cam_rectify[1][2])/cam_rectify[1][1]

                    have_in_tail = ((x_tail_1 & x_tail_2)[[0,1,2,3]]==True).sum()
                    #####################
                    anchorpoint = [x3d_ori, y3d_ori, z3d_ori]
                    if have_in >= 1 and have_in < 4: ######切车
                        if y_front_center.sum() == 1 or y_tail_center.sum() == 1:
                            if have_in_tail < 4 and have_in_front==4:
                                anchorpoint = xyz3d_0 
                                alpha = -np.arctan2(anchorpoint[0],anchorpoint[2]) + rotation_y
                                if alpha<-np.pi:
                                    alpha+=2*np.pi
                                if alpha>np.pi:
                                    alpha-=2*np.pi
                                score = 1
                                if len(distortion_param)==5:
                                    [xc,yc] = xyz_xy(anchorpoint[0]/anchorpoint[2],anchorpoint[1]/anchorpoint[2],distortion_param,cam_intrinsic)
                                else:
                                    [xc,yc] = cam_img_kb(anchorpoint[0],anchorpoint[1],anchorpoint[2],cam_intrinsic,distortion_param)
                                xc,yc = xc/origW,yc/origH
                                value_3d_face = [anchorpoint[0],anchorpoint[1],anchorpoint[2],alpha,xc,yc,score,1]
                                value_3d_4face.append(value_3d_face)
                                value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                                value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                                value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                            elif have_in_tail == 4 and have_in_front<4:
                                anchorpoint = xyz3d_1 
                                alpha = -np.arctan2(anchorpoint[0],anchorpoint[2]) + rotation_y
                                if alpha<-np.pi:
                                    alpha+=2*np.pi
                                if alpha>np.pi:
                                    alpha-=2*np.pi
                                score = 1
                                if len(distortion_param)==5:
                                    [xc,yc] = xyz_xy(anchorpoint[0]/anchorpoint[2],anchorpoint[1]/anchorpoint[2],distortion_param,cam_intrinsic)
                                else:
                                    [xc,yc] = cam_img_kb(anchorpoint[0],anchorpoint[1],anchorpoint[2],cam_intrinsic,distortion_param)
                                xc,yc = xc/origW,yc/origH
                                value_3d_face = [anchorpoint[0],anchorpoint[1],anchorpoint[2],alpha,xc,yc,score,1]
                                value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                                value_3d_4face.append(value_3d_face)
                                value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                                value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                    else:
                        if 'G1M2' in text_basename and (x_down.sum() < 8 and rotation_y < 0 and y_tail.sum() < 4 and y_front_center.sum() == 1): ####G1M2车尾底边被截断###
                            anchorpoint = xyz3d_0 
                            alpha = -np.arctan2(anchorpoint[0],anchorpoint[2]) + rotation_y
                            if alpha<-np.pi:
                                alpha+=2*np.pi
                            if alpha>np.pi:
                                alpha-=2*np.pi
                            score = 1
                            if len(distortion_param)==5:
                                [xc,yc] = xyz_xy(anchorpoint[0]/anchorpoint[2],anchorpoint[1]/anchorpoint[2],distortion_param,cam_intrinsic)
                            else:
                                [xc,yc] = cam_img_kb(anchorpoint[0],anchorpoint[1],anchorpoint[2],cam_intrinsic,distortion_param)
                            xc,yc = xc/origW,yc/origH
                            value_3d_face = [anchorpoint[0],anchorpoint[1],anchorpoint[2],alpha,xc,yc,score,1]
                            value_3d_4face.append(value_3d_face)
                            value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                            value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                            value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                        elif 'G1M2' in text_basename and (x_down.sum() < 8 and rotation_y > 0 and y_front.sum() == 4 and y_tail_center.sum() == 1): ####G1M2车头底边被截断###
                            anchorpoint = xyz3d_1 
                            alpha = -np.arctan2(anchorpoint[0],anchorpoint[2]) + rotation_y
                            if alpha<-np.pi:
                                alpha+=2*np.pi
                            if alpha>np.pi:
                                alpha-=2*np.pi
                            score = 1
                            if len(distortion_param)==5:
                                [xc,yc] = xyz_xy(anchorpoint[0]/anchorpoint[2],anchorpoint[1]/anchorpoint[2],distortion_param,cam_intrinsic)
                            else:
                                [xc,yc] = cam_img_kb(anchorpoint[0],anchorpoint[1],anchorpoint[2],cam_intrinsic,distortion_param)
                            xc,yc = xc/origW,yc/origH
                            value_3d_face = [anchorpoint[0],anchorpoint[1],anchorpoint[2],alpha,xc,yc,score,1]
                            value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                            value_3d_4face.append(value_3d_face)
                            value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                            value_3d_4face.append([-1,-1,-1,-1,-1,-1,0,-1])
                        elif (rotation_y < 0 and y_front_center.sum() == 0) or (rotation_y > 0 and y_tail_center.sum() == 0):
                            value_3d_4face = []
                        else:
                            for xyz3d in xyz3d_4face:
                                angle = Calculate_vector_angle(np.array([xyz3d[0] - x3d_ori, xyz3d[2] - z3d_ori]), np.array([xyz3d[0], xyz3d[2]]))
                                score = angle2score(angle)
                                alpha = -np.arctan2(xyz3d[0],xyz3d[2]) + rotation_y
                                if alpha<-np.pi:
                                    alpha+=2*np.pi
                                if alpha>np.pi:
                                    alpha-=2*np.pi
                                if len(distortion_param)==5:
                                    [xc,yc] = xyz_xy(xyz3d[0]/xyz3d[2],xyz3d[1]/xyz3d[2],distortion_param,cam_intrinsic)
                                else:
                                    [xc,yc] = cam_img_kb(xyz3d[0],xyz3d[1],xyz3d[2],cam_intrinsic,distortion_param)
                                xc,yc = xc/origW,yc/origH
                                value_3d_face = [xyz3d[0],xyz3d[1],xyz3d[2],alpha,xc,yc,score,1]
                                value_3d_4face.append(value_3d_face)
                            value_3d_4face = np.array(value_3d_4face)
                            if singleLW == 0: #####L W Believable
                                value_3d_4face = value_3d_4face
                            elif singleLW == 1: #####L Believable
                                value_3d_4face[0:2,7] = -1
                            elif singleLW == 2: #####W Believable
                                value_3d_4face[2:4,7] = -1
                    label_temp.append(singleLW)
                    side_diff = np.min((abs(0-box2d[0]),abs(origW-box2d[2])))
                    value_3d_4face = np.array(value_3d_4face)
                    ################################
                    if 'G1M2' in text_basename or 'D4Q' in text_basename:
                        side_diff_threshold = 12
                    elif 'G1M3' in text_basename:
                        side_diff_threshold = 70
                    if len(value_3d_4face)!=0:
                        if have_in >= 1 and have_in < 4:
                        # if  np.sum(value_3d_4face[:,-1])==-2 and np.sum(value_3d_4face[:,-2]) == 1:
                            if side_diff>side_diff_threshold and abs(rotation_y) >= 0.785 and abs(rotation_y)<= 2.355: ##45
                                label_temp = label_temp[0:6]
                                label_temp[5]=-1
                                # print(annotations)
                        elif not(np.sum(value_3d_4face[:,-1])==-2 and np.sum(value_3d_4face[:,-2]) == 1) :
                            if side_diff<side_diff_threshold:
                                label_temp = label_temp[0:6]
                                label_temp[5]=-1
                    else:
                        label_temp = label_temp[0:6]
                        label_temp[5]=-1

                    if len(label_temp)>6:
                        for value_3d_face in value_3d_4face:
                            for value_3d in value_3d_face:
                                label_temp.append(value_3d)
                    label_temp.append(confidence)
                    
            else:
                label_temp.append(-1)
                label_temp.append(confidence)
            label_temp_all.append(label_temp)
        return label_temp_all

def select_texts_for_vis(textlists, select_mode, stride_step=1, sample_num_per_uuid=5):
    if select_mode == 'all':
        return textlists
    if select_mode == 'stride':
        return textlists[0::stride_step]
    if select_mode == 'per_uuid_random':
        uuid_to_texts = {}
        for text in textlists:
            uuid_dir = os.path.dirname(os.path.dirname(text))
            uuid_to_texts.setdefault(uuid_dir, []).append(text)

        selected_texts = []
        for uuid_dir in sorted(uuid_to_texts.keys()):
            uuid_texts = sorted(uuid_to_texts[uuid_dir])
            sample_count = min(sample_num_per_uuid, len(uuid_texts))
            if sample_count == len(uuid_texts):
                sampled_texts = uuid_texts
            else:
                sampled_texts = sorted(random.sample(uuid_texts, sample_count))
            selected_texts.extend(sampled_texts)
        return selected_texts
    raise ValueError(f'Unsupported select_mode: {select_mode}')

def process_single_label(args):
    """Worker function for processing a single label file"""
    text, showpath, origW, origH, count, save_mode, save_scale = args
    try:
        label_temp_all = []
        textname = os.path.basename(text)
        group_id = os.path.basename(os.path.dirname(os.path.dirname(text)))

        print(count, textname)
        path, name = os.path.split(text)
        calib_path = resolve_camera_calib_path(path.replace('labels','calib'), text)
        cam_intrinsic, distortion_param, cam_rectify, Vehiclepos, VehiclePitch, VehicleYaw, VehicleRoll = readcalib(calib_path)
        context = open(text,'r').readlines()
        for ll in context:
            data = ll.strip('\n').split(' ')
            label_temp = []
            for i in range(len(data)):
                label_temp.append(data[i])
            label_temp_all.append(label_temp)

        img_path = resolve_image_path(text)
        img = cv2.imread(img_path)

        if img is None:
            print(f"Error: Failed to read image from {img_path}")
            return False

        origH, origW = img.shape[:2]
        label_canvas_w, label_canvas_h = resolve_label_canvas_size(
            calib_path,
            origW,
            origH,
            label_temp_all,
            cam_intrinsic,
            distortion_param,
        )
        print(f"Image loaded successfully: shape={img.shape}")

        img2d = showFace2Dbox(
            img.copy(),
            label_temp_all,
            cam_intrinsic,
            distortion_param,
            cam_rectify,
            origW,
            origH,
            pitch=0,
            label_canvas_w=label_canvas_w,
            label_canvas_h=label_canvas_h,
        )
        img3d = showFace3Dbox(img.copy(), label_temp_all, cam_intrinsic, distortion_param, cam_rectify, origW, origH, pitch=0)

        if img2d is None or img3d is None:
            print(f"Error: showFace2Dbox or showFace3Dbox returned None")
            return False

        image_name = os.path.basename(text)[0:-4] + '.jpg'
        save_targets = []
        if save_mode == 'concat':
            img_show = cv2.vconcat([img2d, img3d])
            if save_scale != 1.0:
                img_show = cv2.resize(img_show, (0,0), fx=save_scale, fy=save_scale, interpolation=cv2.INTER_LINEAR)
            save_targets.append((os.path.join(showpath, 'concat', group_id, image_name), img_show))
        elif save_mode == 'overlay':
            img_overlay = showFace3Dbox(img2d.copy(), label_temp_all, cam_intrinsic, distortion_param, cam_rectify, origW, origH, pitch=0)
            if img_overlay is None:
                print("Error: showFace3Dbox returned None in overlay mode")
                return False
            if save_scale != 1.0:
                img_overlay = cv2.resize(img_overlay, (0,0), fx=save_scale, fy=save_scale, interpolation=cv2.INTER_LINEAR)
            save_targets.append((os.path.join(showpath, 'overlay', group_id, image_name), img_overlay))
        elif save_mode == 'split':
            if save_scale != 1.0:
                img2d = cv2.resize(img2d, (0,0), fx=save_scale, fy=save_scale, interpolation=cv2.INTER_LINEAR)
                img3d = cv2.resize(img3d, (0,0), fx=save_scale, fy=save_scale, interpolation=cv2.INTER_LINEAR)
            save_targets.append((os.path.join(showpath, '2D', group_id, image_name), img2d))
            save_targets.append((os.path.join(showpath, '3D', group_id, image_name), img3d))
        else:
            print(f"Error: unsupported save_mode {save_mode}")
            return False

        for output_path, output_img in save_targets:
            os.makedirs(os.path.dirname(output_path), exist_ok=True)
            result = cv2.imwrite(output_path, output_img)

            if not result:
                print(f"Error: cv2.imwrite failed for {output_path}")
                return False

            # 验证文件是否成功写入且大小不为0
            if os.path.exists(output_path):
                file_size = os.path.getsize(output_path)
                print(f"Successfully saved: {output_path}, size={file_size} bytes")
                if file_size == 0:
                    print(f"Warning: Output file size is 0!")
                    return False
            else:
                print(f"Error: Output file does not exist: {output_path}")
                return False

        return True
    except Exception as e:
        print(f"Error processing {text}: {str(e)}")
        traceback.print_exc()
        return False

if __name__ == '__main__': ####GT
    inpath = '/mnt/mono3d/ydong_data/Mono3d_camera2_gt_by_group_id'
    random.seed(2100)
    showpath_root = '/data1/dongying/Mono3d/D4Q2/data_visualization_for_check_camera2'
    save_mode = 'split'  # 'concat': 2D/3D上下拼接, 'overlay': 2D/3D叠加到同一张图, 'split': 分开保存到2D/3D目录
    save_scale = 0.5
    select_mode = 'stride' ###'all' 使用全部label 'stride'按步长抽取 'per_uuid_random',按每个 uuid 单独随机抽样.随机选 sample_num_per_uuid 个 label
    stride_step = 3 ##3
    sample_num_per_uuid = 5
    group_batch_size = 1000
    origH, origW = 1280, 1920 #2160, 3840
    num_processes = 24

    # test
    # group_batch_size = 10
    # num_processes = 4

    if save_mode not in ['concat', 'overlay', 'split']:
        raise ValueError(f'Unsupported save_mode: {save_mode}')

    os.makedirs(showpath_root, exist_ok=True)
    group_dirs = sorted(
        group_dir for group_dir in glob.glob(os.path.join(inpath, '*'))
        if os.path.isdir(group_dir)
    )

    # group_dirs = group_dirs[:20]

    # group_dirs = ['/mnt/mono3d/ydong_data/Mono3d_camera2_gt_by_group_id/019b45ab-8a37-7ae9-a448-6a8bc34f84b5']

    total_groups = len(group_dirs)
    if total_groups == 0:
        raise FileNotFoundError(f'No group directories found under {inpath}')

    total_batches = math.ceil(total_groups / group_batch_size)
    total_success_count = 0
    total_task_count = 0

    for batch_idx, start_idx in enumerate(range(0, total_groups, group_batch_size), 1):
        batch_group_dirs = group_dirs[start_idx:start_idx + group_batch_size]
        batch_showpath = os.path.join(showpath_root, f'batch_{batch_idx:03d}')
        os.makedirs(batch_showpath, exist_ok=True)

        args_list = []
        count = 0
        for group_dir in batch_group_dirs:
            textlists = sorted(glob.glob(os.path.join(group_dir, 'labels', '*.txt')))
            if not textlists:
                print(f"Skip group without labels: {group_dir}")
                continue

            selected_texts = select_texts_for_vis(
                textlists,
                select_mode,
                stride_step=stride_step,
                sample_num_per_uuid=sample_num_per_uuid,
            )
            for text in selected_texts:
                count += 1
                args_list.append((text, batch_showpath, origW, origH, count, save_mode, save_scale))

        if not args_list:
            print(f"Batch {batch_idx}/{total_batches}: no labels selected, skip")
            continue

        print(
            f"Starting batch {batch_idx}/{total_batches} with {len(batch_group_dirs)} groups, "
            f"{len(args_list)} files, save root: {batch_showpath}"
        )
        with Pool(processes=num_processes) as pool:
            results = pool.map(process_single_label, args_list)

        success_count = sum(results)
        total_success_count += success_count
        total_task_count += len(args_list)
        print(
            f"Completed batch {batch_idx}/{total_batches}: "
            f"{success_count}/{len(args_list)} files processed successfully"
        )

    print(f"All batches completed: {total_success_count}/{total_task_count} files processed successfully")

    
