提交python文件

script/to_kitti_Nodes.py

+import os
+import open3d as o3d
+import numpy as np
+import glob
+import json
+import math
+import random
+from scipy.spatial.transform import Rotation as R
+import shutil as sh
+import cv2
+import copy
+
+def compute_pitch(A,B,C):
+  origin_direction = [0,0,1]
+  nor = [A,B,C]
+  angle = np.arccos(np.dot(origin_direction,nor) / np.linalg.norm(nor))
+  print(angle,angle* 180/ np.pi)
+  return angle
+
+#绕z轴旋转：欧拉角到旋转矩阵
+def rotz(t):
+  """Rotation about the z-axis."""
+  c = np.cos(t)
+  s = np.sin(t)
+  return np.array([[c, -s,  0],
+                    [s,  c,  0],
+                    [0,  0,  1]])
+
+def compute_rot(A,B,C):
+  origin_direction = np.array([0,0,1])
+  nor = np.array([A,B,C])
+  mod = np.linalg.norm(nor)
+  nor = nor / mod
+  angle = np.arccos(np.dot(nor,origin_direction))
+  print(angle,angle* 180/ np.pi)
+
+  p_rot = np.cross(nor,origin_direction)
+  p_rot = p_rot / np.linalg.norm(p_rot)
+
+  sin_ang = np.sin(angle)
+  cos_ang = np.cos(angle)
+  rotMat = np.zeros([4,4])
+  rotMat[0,0] = cos_ang + p_rot[0] * p_rot[0] * (1 - cos_ang)
+  rotMat[0,1] = p_rot[0] * p_rot[1] * ( 1 - cos_ang - p_rot[2] * sin_ang)
+  rotMat[0,2] = p_rot[1] * sin_ang + p_rot[0] * p_rot[2] * (1-cos_ang)
+
+  rotMat[1,0] = p_rot[2] * sin_ang + p_rot[0] * p_rot[1] * (1 - cos_ang)
+  rotMat[1,1] = cos_ang + p_rot[1] * p_rot[1] * (1 - cos_ang)
+  rotMat[1,2] = -p_rot[0] * sin_ang + p_rot[1] * p_rot[2] * ( 1 - cos_ang)
+
+  rotMat[2,0] = -p_rot[1] * sin_ang + p_rot[0] * p_rot[2] * (1 - cos_ang)
+  rotMat[2,1] = p_rot[0] * sin_ang + p_rot[1] * p_rot[2] * ( 1 - cos_ang)
+  rotMat[2,2] = cos_ang + p_rot[2] * p_rot[2] * (1 - cos_ang)
+
+  rotMat[3,3] = 1
+  return angle,rotMat
+
+# convert the ground of the origin point cloud to an horizontal plane
+rotMat = {}
+start_ind = {'N2_1':14000, 'N2_2':16000, 'N2_3':18000, 'N2_4':20000 \
+  , 'N3_1':22000, 'N3_2':24000, 'N3_3':26000, 'N4_1':28000, 'N4_2':30000 \
+    , 'N5_1':32000, 'N5_2':34000, 'N5_3':36000, 'N6_1':38000 \
+      , 'N6_2':40000, 'N6_3':42000, 'N8_1':44000, 'N8_2':46000, 'N9_1':48000 \
+        , 'N9_2':50000, 'N10_1':52000, 'N10_2':54000}
+print(start_ind.keys())
+
+def tmp_func():
+  ext_theta =  90 / 180.0 * np.pi
+  kitti_Rot = np.zeros([4,4])
+  kitti_Rot[:3,:3] = rotz(ext_theta)
+  kitti_Rot[0,3] = 5
+  kitti_Rot[1,3] = -22
+  kitti_Rot[3,3] = 1
+  print(kitti_Rot)
+  inverse_tmp = np.linalg.inv(kitti_Rot)
+  print(inverse_tmp)
+#tmp_func()
+
+def gen_rotMat():
+  global rotMat
+
+  #生成N7_1这段数据的全局转换矩阵
+  A,B,C,D = 5.10151553e-04,1.30740918e-03,1.89664435e-01,9.81847968e-01
+  #A,B,C,D = -9.39865821e-04,  1.04783823e-03,  1.85715376e-01,  9.82602574e-01
+  ang,hori_rotMat = compute_rot(A,B,C)
+  print(ang,hori_rotMat)
+
+  #ext_theta =  0.5 * np.pi
+  ext_theta =  90 / 180.0 * np.pi
+  kitti_Rot = np.zeros([4,4])
+  kitti_Rot[:3,:3] = rotz(ext_theta)
+  kitti_Rot[2,3] = -1
+  kitti_Rot[3,3] = 1
+  print(kitti_Rot)
+
+  tmp = np.dot(kitti_Rot,hori_rotMat)
+  rotMat['N7_1'] = tmp
+  print(tmp)
+  inverse_tmp = np.linalg.inv(tmp)
+  print(inverse_tmp)
+
+  N2_1_mat = np.array([[0.05045908306549516, -0.997741728436212, 0.04433197799906369, 0.0],
+    [0.9959209468126726, 0.05359234819149573, 0.07259013648610471, 0.0], [-0.0748020630460102,
+      0.04048831377621809, 0.9963761076077744, 0.2859260540868016], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N2_1'] = N2_1_mat
+
+  N2_2_mat = np.array([[0.07510626206747571, -0.9946697513726462, 0.07064796601834904, 0.0],
+    [0.9968676257061473, 0.07313389683723709, -0.03010597868179836, 0.0], [0.024778745271707994,
+      0.07268781767035316, 0.997046887034447, -0.7000000000000002], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N2_2'] = N2_2_mat
+
+  N2_3_mat = np.array([[0.07568437565247627, -0.9968308870840116, -0.02449607800031248,
+      0.0], [0.9971317454228593, 0.07565174868629727, 0.002257252902847979, 0.0],
+    [-0.0003969282768393359, -0.024596655789107878, 0.9996973776958381, 0.9000000000000004],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N2_3'] = N2_3_mat
+
+  N2_4_mat = np.array([[0.07516802438907474, -0.996779910756179, 0.0279209172905782, 0.0],
+    [0.9961881280280445, 0.07630731297561882, 0.042265914901034, 0.0], [-0.044260385057339445,
+      0.024637441006419476, 0.9987161833149752, 0.09999999999999964], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N2_4'] = N2_4_mat
+
+  N3_1_mat = np.array([[0.10282893175566048, -0.9927983688656276, 0.06146226136206685, 0.0],
+    [0.9946815801416514, 0.10226437626202323, -0.01226994191701453, 0.0], [0.0058961784994428,
+      0.06239708427073043, 0.9980339868729993, 0.7844127162859094], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N3_1'] = N3_1_mat
+
+  N3_2_mat = np.array([[0.9995180204153945, 0.028485544660163925, -0.012341013341895022,
+      10.0], [-0.02812481342444661, 0.9991986491976914, 0.02847901528053492, 0.0],
+    [0.013142364122603362, -0.02811820027887063, 0.9995182064766736, 0.7000000000000002],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N3_2'] = N3_2_mat
+
+  N4_1_mat = np.array([[-0.444341735964156, -0.8947739313681952, 0.044045810518876544, 0.0],
+    [0.8953720752482885, -0.4419458502434798, 0.054705688169885476, 0.0], [-0.029483360492542885,
+      0.06374540921880009, 0.9975305781065533, -0.09999999999999964], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N4_1'] = N4_1_mat
+
+  N4_2_mat = np.array([[0.9988523295913587, 0.04671359708300838, -0.010576555085760117,
+      10.0], [-0.04648606911938285, 0.9987018755893003, 0.020823281975752502, 0.0],
+    [0.011535555805583356, -0.02030772124045653, 0.9997272265024476, 0.7999999999999998],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N4_2'] = N4_2_mat
+
+  N5_1_mat = np.array([[-0.08986377281989212, -0.9954705408424266, 0.031030704946254054,
+      0.0], [0.9958706079834863, -0.09021596031025624, -0.010139657780214443, 0.0],
+    [0.012893195460257717, 0.029991379097749554, 0.9994669993004465, 0.5747271756415682],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N5_1'] = N5_1_mat
+  
+  N5_2_mat = np.array([[-0.09764111729780293, -0.9944741458664936, 0.038566636046517395,
+      0.0], [0.9945996324606676, -0.09887670911792698, -0.03154310547669095, 0.0],
+    [0.035182144930937416, 0.03527845797532399, 0.9987580523234553, 1.2727123965813156],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N5_2'] = N5_2_mat
+
+  N5_3_mat = np.array([[0.11376454710271973, -0.9928370794231558, 0.03649878826770976, 0.0],
+    [0.9918680257870178, 0.11560991762950971, 0.05321810187391049, 0.0], [-0.05705652674214411,
+      0.030147647805359817, 0.9979156638152981, -0.404451678835378], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N5_3'] = N5_3_mat
+
+  N6_1_mat = np.array([[0.9994973004805413, -0.03086596626934255, 0.007241440351907325,
+      10.0], [0.03082860898771179, 0.9995110809882194, 0.0052149640117460745, 0.0],
+    [-0.007398864777328993, -0.004989098918726587, 0.9999601820532585, 0.9000000000000004],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N6_1'] = N6_1_mat
+
+  N6_2_mat = np.array([[0.9988722862129534, -0.04214125427326351, -0.021869396973478106,
+      10.0], [0.0409985649537926, 0.99789214163878, -0.05030299521200135, 0.0], [
+      0.023943130694148426, 0.04934965393845363, 0.9984945358632253, 0.9000000000000004],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N6_2'] = N6_2_mat
+
+  N6_3_mat = np.array([[-0.04889424680162646, -0.9987854830493128, -0.006075481845301646,
+      0.0], [0.9987878973799329, -0.048858057728727665, -0.005968772264798985, 0.0],
+    [0.00566468684698101, -0.00635995636205657, 0.9999637304812602, -0.6553131698852468],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N6_3'] = N6_3_mat
+
+  N8_1_mat = np.array([[0.06742395876958965, -0.9977243948218384, 0.00020435565303055666,
+      0.0], [0.9976344970690704, 0.06741513105551401, -0.013439135482274529, 0.0],
+    [0.013394776652850061, 0.001109991965790168, 0.9999096698583608, 0.7999999999999998],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N8_1'] = N8_1_mat
+
+  N9_1_mat = np.array([[-0.19809481806349166, -0.9794499176397107, 0.03789857374567175,
+      0.0], [0.9799804151970427, -0.19712000934332657, 0.02796583177233762, 0.0],
+    [-0.01992056441529652, 0.04267974639159458, 0.998890189340813, 0.9980420980703562],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N9_1'] = N9_1_mat
+
+  N9_2_mat = np.array([[-0.03297378854159967, -0.9961526066188378, 0.08119552694397052,
+      0.0], [0.9993813319576447, -0.03186786767058145, 0.014879258875841538, 0.0],
+    [-0.01223448420563244, 0.0816359194020958, 0.9965871231656551, -0.11066447945547608],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N9_2'] = N9_2_mat
+
+  N10_1_mat = np.array([[-0.10138137931389381, -0.9920819078535333, 0.07413031794148582,
+      0.0], [0.9946640529021992, -0.10251230933181928, -0.011603805394292926, 0.0],
+    [0.019111195477614955, 0.07255835269044347, 0.9971810505932539, -0.09999999999999964],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N10_1'] = N10_1_mat
+
+  N10_2_mat = np.array([[0.051858864388487746, -0.9973430294431006, 0.05116189798663601,
+      0.0], [0.9980146776291314, 0.049924120800806875, -0.0383964243008054, 0.0],
+    [0.03574019335646457, 0.05305152008684534, 0.9979519902256409, 1.259040914953271],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N10_2'] = N10_2_mat
+
+gen_rotMat()
+
+#根据标注的3d框的center、quart、size计算3d bbox的8个顶点
+#此处计算的是原始lidar坐标系下的坐标
+def my_compute_box_3d(center, quart, size):
+  # P' = T * R * S * p
+  #其中p为box3d局部坐标，P'为变换后的世界坐标，假设初始box3d局部坐标系与lidar世界坐标系重合
+  #标准的box边长为1，正方体
+  x_corners = [-1,1,1,-1,-1,1,1,-1]
+  y_corners = [1,1,-1,-1,1,1,-1,-1]
+  z_corners = [1,1,1,1,-1,-1,-1,-1]
+
+  tmp = np.vstack([x_corners, y_corners, z_corners]) / 2
+  corners_3d = np.ones([4,8])
+  corners_3d[:3,:] = tmp
+
+  S = np.diag([size[0],size[1],size[2],1])
+  rot = R.from_quat(quart)
+  TR = rot.as_matrix()
+  Trans = np.zeros([4,4])
+  Trans[:3,:3] = TR
+  Trans[0,3] = center[0]
+  Trans[1,3] = center[1]
+  Trans[2,3] = center[2]
+
+  tmp4x4 = np.dot(S,corners_3d)
+  world_corners_3d = np.dot(Trans,tmp4x4)
+
+  return np.transpose(world_corners_3d[:3,:])
+
+#根据quart和计算出的原始坐标系的corners_3d来
+#计算转换到新虚拟坐标系下的heading和heading平面
+def my_compute_heading_plane(quart,corners_3d):
+  rot = R.from_quat(quart)
+  euler = rot.as_euler('zxy', degrees=False) #航向角
+  #点的方向按照逆时针顺序给出，保证算出的法向量垂直于平面向外
+  six_planes = [[corners_3d[1],corners_3d[5],corners_3d[6],corners_3d[2]],\
+                [corners_3d[0],corners_3d[4],corners_3d[5],corners_3d[1]],\
+                [corners_3d[3],corners_3d[7],corners_3d[4],corners_3d[0]],\
+                [corners_3d[2],corners_3d[6],corners_3d[7],corners_3d[3]],\
+                [corners_3d[0],corners_3d[1],corners_3d[2],corners_3d[3]],\
+                [corners_3d[7],corners_3d[6],corners_3d[5],corners_3d[4]] ]
+  normals = []
+  for it in six_planes:
+    it = np.array(it)
+    nor = np.cross(it[1] - it[2],it[0] - it[1])
+    normals.append(nor)
+
+  origin_direction = [1,0,0]
+  ret_ind = -1
+  for ind,nor in enumerate(normals):
+    direction = np.arccos(np.dot(origin_direction,nor) / np.linalg.norm(nor))
+    if abs(direction - abs(euler[0])) < 0.5:
+      ret_ind = ind
+      #print('debug:',direction,ind)
+      break
+  
+  try:
+    assert(ret_ind == 0)
+  except:
+    print("wrong annotation")
+    return False, [], []
+  #new_heading = origin_lidar_heading_to_new_virtual(euler[0])
+  new_heading = euler[0]
+
+  new_heading_plane = copy.deepcopy(six_planes[ret_ind])
+  '''
+  nhp_len = len(new_heading_plane)
+  for ii in range(nhp_len):
+    #TODO cur lidar -> virtual kitti lidar
+    new_heading_plane[ii][0] = new_heading_plane[ii][0] - 15.0
+    new_heading_plane[ii][1] = new_heading_plane[ii][1] - 7.0
+  '''
+  
+  return True,new_heading, new_heading_plane
+
+#open3d的带快捷键，切换不同点云帧
+def custom_draw_geometry_with_key_callback(pcd, file_path):
+  def save_pcd(vis):
+    #io.write_point_cloud(file_path, pcd, write_ascii=True)
+    #print(file_path)
+    return False
+
+  key_to_callback = {}
+  key_to_callback[ord("S")] = save_pcd
+  o3d.visualization.draw_geometries_with_key_callbacks(pcd, key_to_callback)
+
+#读入原始lidar坐标系的标注信息
+#将原始lidar坐标系转化为标准kitti坐标系
+#转换方式：
+#       所有corners_3d、center坐标按照“ln(x) = -l(y);ln(y) = l(x);ln(z) = l(z)”来转换
+#       新的虚拟new_heading = np.pi + heading   if -np.pi < heading < 0;
+#       新的虚拟new_heading = heading - np.pi   if 0 < heading < np.pi;
+########
+#保持不变的信息：
+#       category、size
+def read_all_3d_bboxes(box3d,rotMat):
+  bboxes = []
+  for item in box3d:
+    center = [item['box3D']['translation']['x'],item['box3D']['translation']['y'],item['box3D']['translation']['z']]
+    center = np.array(center, np.float32)
+    center = np.transpose(np.dot(rotMat[:3,:3],np.transpose(center))) + rotMat[:3,3]
+    
+    quart = [item['box3D']['rotation']['x'],item['box3D']['rotation']['y'],item['box3D']['rotation']['z'],item['box3D']['rotation']['w']]
+
+    origin_rot = R.from_quat(quart)
+    origin_rot_mat = origin_rot.as_matrix()
+    new_rot_mat = np.dot(rotMat[:3,:3],origin_rot_mat)
+    tmp = R.from_matrix(new_rot_mat)
+    quart = tmp.as_quat()
+
+    size = [item['box3D']['size']['x'],item['box3D']['size']['y'],item['box3D']['size']['z']]
+    size = np.array(size, np.float32)
+    if size[0] < 0.4 or size[1] < 0.4 or size[2] < 0.4:
+      continue
+    #计算原始lidar坐标系中的corners_3d
+    corners_3d = my_compute_box_3d(center, quart, size)
+
+    category = item['category']
+    assert_flag, heading, heading_plane = my_compute_heading_plane(quart,corners_3d)
+    if assert_flag == False:
+      return False, []
+
+    #以上为原始lidar坐标系的数据，下面要把bbox转成虚拟lidar坐标系的数据
+    #TODO cur lidar -> virtual kitti lidar
+    #new x = old x - 15   new y = old y - 7
+    #corners_3d[:,0] = corners_3d[:,0] - 15.0
+    #corners_3d[:,1] = corners_3d[:,1] - 7.0
+
+    #TODO cur lidar -> virtual kitti lidar
+    #center[0] = center[0] - 15.0
+    #center[1] = center[1] - 7.0
+    
+    #特别注意，下面的heading和heading_plane是虚拟lidar坐标系下的heading
+    bboxes.append([category, corners_3d, heading, heading_plane, center, quart, size])
+
+  return True, bboxes
+
+#根据corners_3d来生成open3d渲染要用的12条边
+def gen_o3d_3dbboxes(corners_):
+  bbox_lines = [[0, 1], [1, 2], [2, 3], [3, 0], [4, 5], [5, 6], [6, 7], [7, 4], [0, 4], [1, 5], [2, 6], [3, 7]]
+  colors = [[1, 0, 0] for _ in range(len(bbox_lines))] #red
+
+  bbox = o3d.geometry.LineSet()
+  bbox.lines  = o3d.utility.Vector2iVector(bbox_lines)
+  bbox.colors = o3d.utility.Vector3dVector(colors)
+  bbox.points = o3d.utility.Vector3dVector(corners_)
+  return bbox
+
+#按照bin格式来解析点云文件
+#解析过程中将原始lidar坐标系转换为新虚拟lidar坐标系
+#转换方式：
+#        ln(x) = -l(y);ln(y) = l(x);ln(z) = l(z)
+#        intensity = intensity / 255.0
+def parse_pandarmind_bin(pandar_bin_path):
+  points = np.fromfile(pandar_bin_path, dtype=np.float32,count=-1).reshape([-1,4])
+  
+  origin_data = []
+  pc = []
+  for ind in range(points.shape[0]):
+    #TODO cur lidar -> virtual kitti lidar
+    x = points[ind][1] * -1 + 5  #将原点移到铁丝网后5米，这样可以把剔除掉的对象拉回来
+    y = points[ind][0] * 1 + 22 #将原点右移22米
+    z = points[ind][2]
+
+    intensity = points[ind][3]
+    origin_data.append([x,y,z,intensity/255.0])
+    pc.append([x,y,z])
+
+  origin_data = np.array(origin_data,dtype=np.float32)
+  pc = np.array(pc,dtype=np.float32)
+  pcd = o3d.geometry.PointCloud()
+  pcd.points = o3d.utility.Vector3dVector(pc)
+  return origin_data, pcd
+
+#按照文本来解析pcd点云文件
+#解析过程中将原始lidar坐标系转换为新虚拟lidar坐标系
+#转换方式：
+#        ln(x) = -l(y);ln(y) = l(x);ln(z) = l(z)
+#        intensity = intensity / 255.0
+def parse_pandarmind_pcd(pandar_pcd_path,rotMat):
+  '''
+  pandar_fp = open(pandar_pcd_path,'r')
+  origin_data = []
+  pc = []
+  for line in pandar_fp.readlines()[11:]:
+    #TODO cur lidar -> virtual kitti lidar
+    x = float(line.split(' ')[1]) * -1 + 5  #将原点移到铁丝网后5米，这样可以把剔除掉的对象拉回来
+    y = float(line.split(' ')[0]) * 1 + 22 #将原点右移22米
+    z = float(line.split(' ')[2])
+
+    intensity = int(line.split(' ')[3])
+    origin_data.append([x,y,z,intensity/255.0])
+    pc.append([x,y,z])
+
+  origin_data = np.array(origin_data,dtype=np.float32)
+  pc = np.array(pc,dtype=np.float32)
+  pcd = o3d.geometry.PointCloud()
+  pcd.points = o3d.utility.Vector3dVector(pc)
+  return origin_data, pcd
+  '''
+  pcd = o3d.io.read_point_cloud(pandar_pcd_path)
+  converted_pcd = copy.deepcopy(pcd)
+  xyz = np.array(pcd.points)
+
+  # convert points to horizontal plane
+  xyz_h = copy.deepcopy(xyz)
+  xyz_h = np.transpose(np.dot(rotMat[:3,:3], np.transpose(xyz_h))) + rotMat[:3,3]
+  xyzi = np.zeros([xyz_h.shape[0],4])
+  xyzi[:,:3] = xyz_h[:,:]
+  converted_pcd.points = o3d.utility.Vector3dVector(xyzi[:,:3])
+  '''
+  xyzi[:,0] = xyz[:,0] - 15
+  xyzi[:,1] = xyz[:,1] - 7
+  xyzi[:,2] = xyz[:,2]
+  pcd.points = o3d.utility.Vector3dVector(xyzi[:,:3])
+  '''
+
+  return xyzi,pcd,converted_pcd
+
+#原始坐标系下的heading转换到虚拟kitti坐标系下的heading
+def origin_lidar_heading_to_new_virtual(origin_lidar_heading):
+  #TODO cur lidar -> virtual kitti lidar
+  if -1 * np.pi < origin_lidar_heading and origin_lidar_heading <= 0.5 * np.pi:
+    return 0.5 * np.pi + origin_lidar_heading
+  else:
+    return origin_lidar_heading - 1.5 * np.pi
+
+def virtual_heading_to_origin_lidar(virtual_heading):
+  if -0.5 * np.pi < virtual_heading and virtual_heading < np.pi:
+    return virtual_heading - 0.5 * np.pi
+  else:
+    return virtual_heading + 1.5 * np.pi
+
+#将新虚拟坐标系的heading转换到虚拟camera坐标系下的heading
+def lidar_heading_to_camera(lidar_heading):
+  gt_heading = 1000.0
+  if lidar_heading > - np.pi and lidar_heading < 0.5 * np.pi:
+    gt_heading = -1 * lidar_heading - 0.5 * np.pi
+  else:
+    gt_heading = 1.5 * np.pi - lidar_heading
+  
+  return gt_heading
+
+#将label写入文件
+def write_label_2(file_path,groundtruth):
+  fp = open(file_path,'w')
+  for gt in groundtruth:
+    line = "{} 0.0 0 0.0 0 0 50 50 {:.3f} {:.3f} {:.3f} {:.3f} {:.3f} {:.3f} {:.4f} {:.2f}\n" \
+      .format(gt[0],gt[8],gt[9],gt[10],gt[11],gt[12],gt[13],gt[14],gt[15])
+    fp.write(line)
+  fp.close()
+
+if __name__ == '__main__':
+  #遍历所有pcd
+  root_path = 'G:\\datasets\\car_city\\nodes\\'
+
+  children_dir = ['N2_1', 'N2_2', 'N2_3', 'N2_4', 
+                 'N3_1', 'N3_2', 
+                 'N4_1', 'N4_2', 
+                 'N5_1', 'N5_2', 'N5_3', 
+                 'N6_1', 'N6_2', 'N6_3', 
+                 'N8_1',
+                 'N9_1', 'N9_2', 
+                 'N10_1', 'N10_2']
+  #children_dir = ['N10_2']
+  training_ratio = 0.999
+  total_ind = 0
+  
+  for child_ind,child_dir in enumerate(children_dir):
+    pcd_list = glob.glob(os.path.join(root_path,child_dir)+'\\pcd\\*.pcd')
+
+    #读标注的json文件
+    anno = {}
+    jsn_file = os.path.join(os.path.join(root_path,child_dir), child_dir + ".json")
+    with open(jsn_file, 'r',encoding='utf-8',errors='ignore') as fp:
+      jsn = json.load(fp)
+      annos = jsn["annotations"]
+      for an in annos:
+        fileuri = an['fileuri'].split("/")[0] + "\\" + an['fileuri'].split("/")[-1]
+        anno[fileuri] = an['labels_box3D']
+
+    #random.shuffle(pcd_list)
+    to_save_dataset_root = 'G:\\datasets\\car_city\\datasets\\N19\\'
+    avg = []
+    child_ind = start_ind[child_dir]
+    for pcd_ind,pcd_file in enumerate(pcd_list):
+      vis = False
+      #if pcd_file == "G:\\datasets\\car_city\\nodes\\N5_1\\pcd\\1629663169.047605000.pcd":
+      if int(pcd_file.split(".")[0].split("\\")[-1]) % 4 == 0:
+        vis = True
+        print('handle ', pcd_file)
+      
+      key = pcd_file.split('\\')[-3][1:] + '\\' + pcd_file.split('\\')[-1]
+
+      if key not in anno:
+        print('the anno is null : ', pcd_file)
+        continue
+      print(key)
+      #读入原始lidar坐标系的标注信息
+      #将原始lidar坐标系转化为标准kitti坐标系
+      bbox_flag,bboxes = read_all_3d_bboxes(anno[key], rotMat[child_dir])
+      if bbox_flag == False or len(bboxes) < 1:
+        continue
+
+      # 地面转成水平面，地面点的z值几乎差不多
+      xyzi,pcd,converted_pcd = parse_pandarmind_pcd(pcd_file, rotMat[child_dir])
+      #merge_geos = [pcd]
+      #to_save_path = "N7_1_horizon.pcd"
+      #o3d.io.write_point_cloud(to_save_path, converted_pcd, write_ascii=True)
+
+      merge_geos = [converted_pcd]
+
+      groundtruth = []
+      for bbox in bboxes:
+        #渲染bbox
+        merge_geos += [gen_o3d_3dbboxes(bbox[1])]
+        heading_point = (bbox[3][0] + bbox[3][2]) / 2
+        heading_pcd = o3d.geometry.PointCloud()
+        heading_pcd.points = o3d.utility.Vector3dVector(np.array([heading_point]))
+        colors = [[1, 0, 0]]
+        heading_pcd.colors = o3d.utility.Vector3dVector(colors)
+        #渲染车辆形式方向的点
+        merge_geos += [heading_pcd]
+
+        #下面将new virtual heading转换为camera坐标系下的heading
+        gt_heading = lidar_heading_to_camera(bbox[2])
+        #字段1:type，3d物体类别
+        #字段2:truncated，代表物体是否在图像内，取值为0～1之间的浮点数，截断指数
+        #字段3:occuluded，代表物体是否被遮挡，取值为整数0，1，2，3表示被遮挡的程度，0：完全可见 1：小部分遮挡 2：大部分遮挡 3：完全遮挡
+        #字段4:alpha，物体的观察角度，范围：[-π,π]，是在相机坐标系下，以相机原点为中心，相机原点到物体中心的连线为半径，将物体绕相机y轴旋转至相机z轴，此时物体方向与相机x轴的夹角
+        #字段5-8:2D标注框，xmin，ymin，xmax，ymax
+        #字段9-11:3维物体的尺寸：表示该车的高度，宽度，和长度，单位为米，千万注意顺序
+        #字段12-14:该车的3D中心在相机坐标下的xyz坐标，并且该中心在车辆下表面的中心，千万注意是相机坐标系下的坐标，
+        # 千万注意是在车辆下表面的中心
+        #字段15:表示车体朝向，绕相机坐标系y轴的弧度值
+        # 注意在lidar坐标系中竖直方向是z轴向上，所以在lidar坐标系中，绕z轴顺时针旋转的朝向为负，逆时针为正
+        # 注意在camera坐标系中竖直方向是y轴向下，所以在camera坐标系中，绕y轴顺时针旋转的朝向为正，逆时针为负
+        gt = [bbox[0],0.0,0,0.0,0,0,50,50,bbox[6][2],bbox[6][1],bbox[6][0],\
+            -1 * bbox[4][1],-1 * bbox[4][2] + bbox[6][2]/2, bbox[4][0],gt_heading,1.0]
+        groundtruth.append(gt)
+      
+      if vis:
+        #axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=[0, 0, 0])
+        #merge_geos += [axis_pcd]
+        #pcd.paint_uniform_color([1.00, 0, 0])
+        #merge_geos += [pcd]
+        #custom_draw_geometry_with_key_callback(merge_geos, pcd_file)
+        vis = False
+      name_prefix = str(child_ind)
+      total_ind += 1
+      child_ind += 1
+      print('generate ', child_ind)
+      
+      img = np.zeros([2048,2448,3],dtype=np.uint8)
+      training_num = int(len(pcd_list) * training_ratio)
+      #按照kitti格式将数据保存为标注的格式
+      if pcd_ind < training_num: # for training
+        pcd_bin_path = os.path.join(to_save_dataset_root, 'training\\velodyne\\' + name_prefix + '.bin')
+        with open(pcd_bin_path,'bw') as fp:
+          xyzi.astype(np.float32).tofile(fp)
+
+        label_2_path = pcd_bin_path.replace('velodyne','label_2').replace('.bin','.txt')
+        write_label_2(label_2_path,groundtruth)
+
+        image_2_path = pcd_bin_path.replace('velodyne','image_2').replace('.bin','.png')
+        cv2.imwrite(image_2_path,img)
+
+        calib_path = pcd_bin_path.replace('velodyne','calib').replace('.bin','.txt')
+        sh.copy('calib.txt',calib_path)
+      else:
+        pcd_bin_path = os.path.join(to_save_dataset_root, 'testing\\velodyne\\' + name_prefix + '.bin')
+        with open(pcd_bin_path,'bw') as fp:
+          xyzi.astype(np.float32).tofile(fp)
+
+        label_2_path = pcd_bin_path.replace('velodyne','label_2').replace('.bin','.txt')
+        write_label_2(label_2_path,groundtruth)
+
+        image_2_path = pcd_bin_path.replace('velodyne','image_2').replace('.bin','.png')
+        cv2.imwrite(image_2_path,img)
+
+        calib_path = pcd_bin_path.replace('velodyne','calib').replace('.bin','.txt')
+        sh.copy('calib.txt',calib_path)
+      
+      
\ No newline at end of file

script/trans_data.py

+
+import os
+
+if __name__ == '__main__':
+
+    jsn_file = "./data_aug_samples/N10_1.json"
+    with open(jsn_file, 'r',encoding='utf-8',errors='ignore') as fp:
+      jsn = json.load(fp)
+      print(jsn)
+	
\ No newline at end of file