      
# coding:utf-8
import os
import glob
import json
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
colors = [(0, 255, 0),(255, 0, 0),(0, 0, 255),(240, 32, 160), (128, 128, 128),
          (204, 161, 141),(101, 134, 179),(0, 255, 255),(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)]

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,        # 垃圾车
}

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)
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 showFace3Dbox(img,label_temp_all, cam_intrinsic, distortion_param, cam_rectify,origW,origH,pitch):
    for co in label_temp_all:
        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]
        plot_box(box2d, img, label=str('%s'%label_det_cls), color=colors[label_det_cls], 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)>6:
            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)==50:
                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_map = { 'kVehicle':0,
    #                 'kPed':1,
    #                 'kBike':2,
    #                 'kCyclist':3, 
    #                 'kCone':4,
    #                 'kRoadBarrier':4,
    #                 'kPedHead':5,
    #                 'kSmallTrafficSign': 6,
    #                 'kWarningTriangle' :6,
    #                 'kBigTrafficSign':7,
    #                 'kVehiclePlate' :8,
    #                 'kVehicleWheel':9,     
    #                 'kTrafficLight':10,      
    #                 'kTrafficLightBulb' :11,
    #                 'kTrafficLightDigit':12
    #                 }

    # det_cls_map = {
    #     "vehicle": 0,
    #     "head": 5,
    #     "plate": 8,
    #     "pedestrian": 1,
    #     "wheel": 9,
    #     "guideboard": 7,
    #     "bicycle": 2,
    #     "rider": 3,
    #     "roadpile": 4,
    #     "tsr": 6,
    #     "rickshaw": 0,
    #     }

    det_cls_map ={
        "vehicle": 0,
        "head": 5,
        "bicycle": 2,
        "wheel": 9,
        "plate": 8,
        "rider": 3,
        "pedestrian": 1,
        "tsr": 6,
        "guideboard": 7,
        "roadpile": 4,
        "waterbarrier": 4,
        "tl_border": 10,
        "tl_wick": 11,
        "sphericalstone": 4,
        "rickshaw": 0,
        "roadblock": 4,
        "cone": 4,
        "crashbarrel": 4,
        "tl_num": 12,
        "carton": -1,
        }

    # det_cls_map = { 'vehicle':0,
    #                 'pedestrian':1,
    #                 'bicycle':2,
    #                 'rider':3, 
    #                 'roadpile':4, 'warningtriangle':4, 'cone':4, 'crashbarrel':4,'waterbarrier':4, 'sphericalstone':4,'roadblock':4,
    #                 'head':5,
    #                 'tsr': 6,
    #                 'guideboard':7,
    #                 'plate' :8,
    #                 'wheel':9,     
    #                 'tl_border':10,      
    #                 'tl_wick' :11,
    #                 'tl_num':12,
    #                 'trailer':0,
    #                 'rickshaw':0,
    #                 'tinycar':0,
    #                 'carton':-1,'tire':-1,'barriergate':-1
    #                 }

    # det_cls_map = [
    #         "car",  "tinycar", "bus", "van", "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('_')
        # try:
        #     calib_name = calib_path +  '/' + 'calib.json'
        #     calib_params = json.load(open(calib_name, "r"))
        # except:
        #     calib_name = calib_path + '/' + 'calib_pandar128_to_cam.json'
        #     calib_params = json.load(open(calib_name, "r"))
        
        # cam_name = 'camera4' 
        # calib_param = calib_params[cam_name]
        # lidar_to_cam = np.array(calib_param["Extrinsic"])
        # cam_intrinsic = np.array(calib_param["Intrinsic"])
        # distortion_param = np.array(calib_param["Distortion"])

        # if 'D4Q' in text_basename  and cam_name == 'camera4':
        #     origH,origW = 2160,3840  
        #     cam_rectify = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(cam_intrinsic,distortion_param,[origW,origH],np.eye(3),balance=1) ####
        # elif 'G1M2' in text_basename  and cam_name == 'camera4':
        #     origH,origW = 1080,1920  
        #     cam_rectify, _ = cv2.getOptimalNewCameraMatrix(cam_intrinsic,distortion_param, (origW,origH), 1, (origW,origH))
        # elif 'G1M3' in text_basename  and cam_name == 'camera4':
        #     origH,origW = 1080,1920  
        #     cam_rectify = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(cam_intrinsic,distortion_param,[origW,origH],np.eye(3),balance=1)
        # elif 'D4Q' in text_basename and cam_name == 'camera2':
        #     origH,origW = 1280,1920
        #     cam_rectify, _ = cv2.getOptimalNewCameraMatrix(cam_intrinsic,distortion_param, (origW,origH), 1, (origW,origH))
        # else:
        #     print('error cam')

        cam_name = 'camera4' 
        calib_param_vehicle = json.load(open(calib_path +  '/' +  '/L2_calib/camera4.json','r'))
        ego_to_cam = get_rot_mat_and_trans(calib_param_vehicle)
        cam_intrinsic = np.array([calib_param_vehicle['focal_u'], 0, calib_param_vehicle['cu'], 0, calib_param_vehicle['focal_v'], calib_param_vehicle['cv'], 0, 0, 1], dtype=np.float64).reshape(3, 3)
        distortion_param = np.array(calib_param_vehicle['distort_coeffs'])
        if 'G1M2' in text_basename  and cam_name == 'camera4':
            origH,origW = 1080,1920  
            cam_rectify, _ = cv2.getOptimalNewCameraMatrix(cam_intrinsic,distortion_param, (origW,origH), 1, (origW,origH))
        elif 'G1M3' in text_basename  and cam_name == 'camera4':
            origH,origW = 1080,1920  
            cam_rectify = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(cam_intrinsic,distortion_param,[origW,origH],np.eye(3),balance=1)

        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']]
            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<=3:
                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]
                
                # cy = (box2d[1]+box2d[3])/2.0
                # if len(distortion_param)==5:
                #     image_coord = xyz_xy(x3d_ori/z3d_ori , y3d_ori/z3d_ori, distortion_param,cam_intrinsic)
                #     x3d_new ,y3d_new = xy_xyz(image_coord[0],cy,distortion_param,cam_intrinsic)
                # else:
                #     image_coord = cam_img_kb(x3d_ori,y3d_ori,z3d_ori,cam_intrinsic,distortion_param)
                #     x3d_new ,y3d_new = img_cam_kb(np.array([image_coord[0],cy]).reshape((-1, 2)),cam_intrinsic,distortion_param)
                # infer_y3d = y3d_new * z3d_ori
                # box3d_cam[1] = infer_y3d
                corners = cam_corners_front_rear(np.array(box3d_cam),'whole', pitch=0)
                if label_det_cls !=0:
                    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
    # if Labelshow:
    #     outpath = '/data1/xdzhu/Data/Mono3D_G1M2/mono3d_show_for_changepoint_0221_0311/'
    #     if not os.path.exists(outpath):
    #         os.makedirs(outpath)
    #     img = cv2.imread(anno.replace('annotations','images')[0:-5] + '.jpg' )
    #     img = showFace3Dbox(img,label_temp_all, cam_intrinsic, distortion_param, cam_rectify,origW,origH,pitch)
    #     ###
    #     # map_1, map_2 = cv2.initUndistortRectifyMap(cam_intrinsic, distortion_param, np.eye(3), cam_rectify, [1920,1080],cv2.CV_32FC1)
    #     # undistorted_image = cv2.remap(img, map_1, map_2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
    #     # undistorted_image = showFace3Dbox(undistorted_image,label_temp_all, cam_intrinsic, distortion_param, cam_rectify,origW,origH)
    #     ###
    #     cv2.imwrite(outpath + '/' + os.path.basename(anno)[0:-5] + '.jpg',img)


def analysis_depth(depth_path):
    depth_data = np.load(depth_path)

    return depth_data['arr_0']

def process_annotation_file(anno_file):
    """
    处理单个JSON标注文件，转换为TXT标签格式
    参数:
        anno_file: 单个JSON标注文件的完整路径
    返回:
        tuple: (anno_file, success: bool, message: str)
    """
    try:
        # 拆分路径和文件名，替代字符串切片，更健壮
        anno_dir, anno_name = os.path.split(anno_file)
        
        # 构建labels目录路径（替换annotations为labels）
        label_dir = anno_dir.replace('annotations_20260320', 'labels_20260320')
        # 确保目录存在，不存在则创建（exist_ok=True避免重复创建报错）
        os.makedirs(label_dir, exist_ok=True)
        
        # （可选）图片存在性校验，如需启用可取消注释
        # img_path = anno_file.replace('annotations', 'images/camera4')[:-5] + '.png'
        # if not os.path.exists(img_path):
        #     return (anno_file, True, "跳过：对应图片不存在")
        
        # 构建标定文件路径
        calib_dir = anno_dir.replace('annotations_20260320', 'calib')
        
        # 调用转换函数生成标签列表
        label_lists = mono3dlabel_trans2train(calib_dir, anno_file)
        
        # 拼接标签字符串
        label_str = ''
        for ll in label_lists:
            # 优化字符串拼接：用join更高效
            ll_str = ' '.join([str(item) for item in ll[:-1]]) + ' ' + str(ll[-1]) + '\n'
            label_str += ll_str
        
        # 构建输出TXT文件路径
        txt_filename = anno_name[:-5] + '.txt'  # 去掉.json后缀，加.txt
        txt_filepath = os.path.join(label_dir, txt_filename)
        
        # 写入标签文件（使用with语句自动关闭文件，避免手动close遗漏）
        with open(txt_filepath, 'w+', encoding='utf-8') as fw:
            fw.write(label_str)
        
        return (anno_file, True, "处理成功")
    
    except Exception as e:
        # 捕获所有异常，记录详细错误信息
        error_detail = f"{str(e)}\n{traceback.format_exc()}"
        return (anno_file, False, f"处理失败：{error_detail}")

def main():
    """
    主函数：初始化进程池，分发处理任务
    """
    # 1. 获取所有待处理的JSON文件并排序
    JSON_ROOT_PATH = '/data1/dongying/Mono3d/G1M3/Testdata_0129/'

    json_pattern = os.path.join(JSON_ROOT_PATH, '019b4e2c-f464-7aea-af9f-e61135bb3eed', 'annotations_20260320', '*.json')
    annotations = glob.glob(json_pattern)
    annotations.sort()
    
    # 2. 基础校验
    total_files = len(annotations)
    if total_files == 0:
        print("⚠️ 未找到任何JSON标注文件，程序退出")
        return
    print(f"📌 共发现 {total_files} 个JSON标注文件待处理")
    
    # 3. 设置进程数：IO密集型任务设为CPU核心数的2-4倍，避免进程数过多导致调度开销
    # num_processes = min(cpu_count() * 2, total_files)
    num_processes = 1
    print(f"🚀 启动 {num_processes} 个进程并行处理...")
    
    # 4. 启动进程池处理任务
    with Pool(processes=num_processes) as pool:
        # 使用imap_unordered实时获取处理结果，便于监控进度
        results = []
        for idx, result in enumerate(pool.imap_unordered(process_annotation_file, annotations), 1):
            file_path, success, msg = result
            # 打印进度和状态
            status = "✅" if success else "❌"
            print(f"进度: {idx}/{total_files} | {status} {os.path.basename(file_path)} - {msg}")
            results.append(result)
    
    # 5. 统计最终结果
    success_count = sum(1 for r in results if r[1])
    fail_count = total_files - success_count
    print("\n" + "="*60)
    print(f"📊 处理完成 | 成功：{success_count} | 失败：{fail_count}")
    
    # 6. 打印失败文件列表（便于排查）
    if fail_count > 0:
        failed_files = [r[0] for r in results if not r[1]]
        print(f"❌ 失败文件列表：{failed_files}")

if __name__ == "__main__":
    # 多进程必须在该判断内执行，避免Windows系统递归创建进程
    # 需确保mono3dlabel_trans2train函数可被子进程导入/访问
    main()

# if __name__ == '__main__':
#     ##calib path ####
#     jsonpath = '/mnt/smb/xdzhu_data/Mono3d/Mono3d_4face_2m_g1m3/discard_erro/G1M3_FDL2232/'
#     annotations = glob.glob(jsonpath + '20251201/*/annotations/*.json')
#     annotations.sort()
#     for anno in annotations:
#         # context = anno.split('/')
#         # tag = context[-3]
#         path,annoname = os.path.split(anno)
#         labelpath = path.replace('annotations','labels')
#         calib_path = path.replace('annotations','calib')
#         calib_path = calib_path.replace('discard_erro','driving_png_20260202')
#         if not os.path.exists(labelpath):
#             os.mkdir(labelpath)
#         imgpath = anno.replace('annotations','images')[0:-5] + '.png' 
#         if not os.path.exists(imgpath):
#             continue
#         jsonname = os.path.basename(anno)
#         labellists = mono3dlabel_trans2train(calib_path,anno)
#         # strs = ''
#         # for ll in labellists:
#         #     for i in range(len(ll) - 1):
#         #         strs += str(ll[i]) + ' '
#         #     strs += str(ll[-1]) + '\n'
#         # fw = open(labelpath + '/' + jsonname[:-5] + '.txt','w+')
#         # fw.write(strs)
#         # fw.close()

# if __name__ == '__main__': ####GT
#     ##calib path ####
#     jsonpath = '/mnt/smb/xdzhu_data/Mono3d/Mono3d_4face_2m_g1m3/driving_png_20260207/G1M3_FDL2232/20251210/019b092a-4bb3-7e06-af6b-99b53dd0a18c/'
#     labelpath = '/mnt/smb/xdzhu_data/Mono3d/Mono3d_4face_2m_g1m3/driving_png_20260207/G1M3_FDL2232/20251210/019b092a-4bb3-7e06-af6b-99b53dd0a18c/'
#     annotations = glob.glob(jsonpath + '/annotations/*.json')
#     annotations.sort()
#     for anno in annotations:
#         path,annoname = os.path.split(anno)
#         labelpath = path.replace('annotations','labels')
#         if not os.path.exists(labelpath):
#             os.mkdir(labelpath)
#         # imgpath = anno.replace('annotations','images/camera4')[0:-5] + '.png' 
#         # if not os.path.exists(imgpath):
#         #     continue
#         calib_path = path.replace('annotations','calib')
#         jsonname = os.path.basename(anno)
#         labellists = mono3dlabel_trans2train(calib_path,anno)
#         strs = ''
#         for ll in labellists:
#             for i in range(len(ll) - 1):
#                 strs += str(ll[i]) + ' '
#             strs += str(ll[-1]) + '\n'
#         fw = open(labelpath + '/' + jsonname[:-5] + '.txt','w+')
#         fw.write(strs)
#         fw.close()

#############################show#######################################
    # jsonpath = '/data1/xdzhu/Data/Mono3D_G1M2/Train_Data/'
    # showpath = '/data1/xdzhu/Data/Mono3D_G1M2_test/'
    # annotations = glob.glob(jsonpath + '20250222/annotations/*.json')
    # annotations.sort()
    # for anno in annotations:
    #     if 'G1M2_AU4010_20250222_seq_33_camera4_001954_405946' not in anno :
    #         continue
    #     print(anno)
    #     mono3dlabel_trans2train(anno,showpath)    

    