1243 lines
62 KiB
Python
Executable File
1243 lines
62 KiB
Python
Executable File
|
||
# 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)
|
||
|
||
|