# 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 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_diff6: 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)