提交更新

script/anno_process.py

+from genericpath import exists
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+import open3d as o3d
+import os
+import json
+import copy
+import shutil
+import cv2
+import random
+from easydict import EasyDict
+import yaml
+import logging
+import datetime
+
+global col_dict
+col_dict = {"little": [0, 1, 1],
+            "mid": [1, 0, 1],
+            "big": [1, 0, 0],
+            "cyclist": [0, 1, 0],
+            "pedestrian": [1, 1, 0]}
+
+
+def get_cross_prod_mat(pVec_Arr):
+    qCross_prod_mat = np.array([
+        [0, -pVec_Arr[2], pVec_Arr[1]],
+        [pVec_Arr[2], 0, -pVec_Arr[0]],
+        [-pVec_Arr[1], pVec_Arr[0], 0],
+    ])
+    return qCross_prod_mat
+ 
+ 
+def caculate_align_mat(pVec_Arr):
+    scale = np.linalg.norm(pVec_Arr)
+    pVec_Arr = pVec_Arr / scale
+
+    # must ensure pVec_Arr is also a unit vec.
+    z_unit_Arr = np.array([0, 0, 1])
+    z_mat = get_cross_prod_mat(z_unit_Arr)
+ 
+    z_c_vec = np.matmul(z_mat, pVec_Arr)
+    z_c_vec_mat = get_cross_prod_mat(z_c_vec)
+ 
+    if np.dot(z_unit_Arr, pVec_Arr) == -1:
+        qTrans_Mat = -np.eye(3, 3)
+    elif np.dot(z_unit_Arr, pVec_Arr) == 1:
+        qTrans_Mat = np.eye(3, 3)
+    else:
+        qTrans_Mat = np.eye(3, 3) + z_c_vec_mat + np.matmul(z_c_vec_mat,
+                                                            z_c_vec_mat) / (1 + np.dot(z_unit_Arr, pVec_Arr))
+ 
+    qTrans_Mat *= scale
+    return qTrans_Mat
+
+
+def get_arrow(label, begin=[0,0,0], vec_Arr=[0,0,1] ):
+
+    sc = np.linalg.norm(vec_Arr)
+    vec_Arr = vec_Arr / sc
+
+    scale = 1.5
+    mesh_arrow = o3d.geometry.TriangleMesh.create_arrow(
+        cone_height=0.5 * scale ,
+        cone_radius=0.12 * scale,
+        cylinder_height=0.8 * scale,
+        cylinder_radius=0.06 * scale
+    )
+
+    c = col_dict[label]
+    mesh_arrow.paint_uniform_color(c)
+    mesh_arrow.compute_vertex_normals()
+
+    rot_mat = caculate_align_mat(vec_Arr)
+    mesh_arrow.rotate(rot_mat, center=np.array([0, 0, 0]))
+    mesh_arrow.translate(np.array(begin)) 
+    return mesh_arrow
+
+def get_line_arrow(label, begin=[0,0,0], vec_Arr=[0,0,1] ):
+  sc = np.linalg.norm(vec_Arr)
+  vec_Arr = vec_Arr / sc
+  c =  np.array([1, 0, 0])
+  arrow_lines = [[0, 1]]
+  coordinates = np.vstack((begin, begin + vec_Arr))
+  colors = [c for _ in range(len(arrow_lines))] 
+  line_arrow = o3d.geometry.LineSet()
+  line_arrow.lines  = o3d.utility.Vector2iVector(arrow_lines)
+  line_arrow.colors = o3d.utility.Vector3dVector(colors)
+  line_arrow.points = o3d.utility.Vector3dVector(coordinates)
+  return line_arrow
+
+def rads2coordinates(radius, angles):
+  height = -6
+  xs = np.sin(angles) * radius
+  ys = np.cos(angles) * radius
+  zs = np.ones((len(xs))) * height
+  return np.vstack((xs, ys, zs)).T
+
+def draw_circle(radius, resolution=40, color=None):
+  angles = np.arange(resolution) * 2 * np.pi / resolution
+  coordinates = rads2coordinates(radius, angles)
+  circle_lines = [[x, x + 1] for x in np.arange(resolution)]
+  circle_lines[-1] = [resolution - 1, 0]
+  
+  colors = [colormap[color] for _ in range(len(circle_lines))] 
+  circle = o3d.geometry.LineSet()
+  circle.lines  = o3d.utility.Vector2iVector(circle_lines)
+  circle.colors = o3d.utility.Vector3dVector(colors)
+  circle.points = o3d.utility.Vector3dVector(coordinates)
+  return circle
+
+def draw_T_geometry(geometry):
+
+  if geometry is None: 
+    return None
+
+  if len(geometry) == 9:
+    num_corners = 12
+
+  c = np.array([127 / 255, 0 / 255, 255 / 255])                            # purpule
+  T_1, T_2, T_3, T_4, T_5, T_6, T_7, T_8, T_9 = geometry
+  geometry_lines = [[x, x + 1] for x in np.arange(num_corners)]
+  geometry_lines[-1] = [num_corners - 1, 0]
+  corners = np.array([[T_3, T_1], [T_4, T_1], [T_4, T_6], [T_5, T_6], [T_5, T_8], [0, T_8],
+             [0, T_9], [T_5, T_9],  [T_5, T_7], [T_4, T_7], [T_4, T_2], [T_3, T_2]])
+  coordinates = np.hstack((corners, -5.9 * np.ones((len(corners), 1))))
+  colors = [c for _ in range(len(geometry_lines))] 
+  T_geometry = o3d.geometry.LineSet()
+  T_geometry.lines  = o3d.utility.Vector2iVector(geometry_lines)
+  T_geometry.colors = o3d.utility.Vector3dVector(colors)
+  T_geometry.points = o3d.utility.Vector3dVector(coordinates)
+  return T_geometry
+
+def draw_ground_plane():
+  radius = 80
+  create_box = o3d.geometry.TriangleMesh.create_box(width= 2 * radius, height= 2 * radius, depth=0.05)
+  create_box.paint_uniform_color([1, 1, 151/255])
+  create_box.translate(np.array([-radius, -radius, -6]))
+  return create_box
+
+
+
+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 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平面       即kitti 坐标系
+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])                                    # cross product of two vectors results in normal vector
+    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, 0, 0
+  new_heading = euler[0]
+  new_heading_plane = copy.deepcopy(six_planes[ret_ind])
+  
+  return True, new_heading, new_heading_plane
+
+def merge_new_config(config, new_config):
+    for key, val in new_config.items():
+        if not isinstance(val, dict):
+            config[key] = val
+            continue
+        if key not in config:
+            config[key] = EasyDict()
+        merge_new_config(config[key], val)
+    return config
+
+def read_yaml(path):
+    # with open(path,  'r') as f:
+    with open(path,  'r', encoding='utf-8') as f:
+        new_config = yaml.load(f, Loader=yaml.FullLoader)
+    config = EasyDict()
+    merge_new_config(config=config, new_config=new_config)
+    return config
+
+
+def read_rotate_boxes(objects, rotMat):
+  bboxes = []
+  cats = []
+  for obj in objects:
+    item = obj['box3D']
+
+    center = [item['translation']['x'],item['translation']['y'],item['translation']['z']]
+    center = np.array(center, np.float32)
+    center = np.transpose(np.dot(rotMat[:3, :3], np.transpose(center))) + rotMat[:3, 3]                     # convert center
+    quart = [item['rotation']['x'],item['rotation']['y'],item['rotation']['z'],item['rotation']['w']]       # annotation format is quanternion
+
+    origin_rot = R.from_quat(quart)
+    origin_rot_mat = origin_rot.as_matrix()
+    new_rot_mat = np.dot(rotMat[:3, :3], origin_rot_mat)                                                    # combine ground calibration & kitti conversion & annotation conversion
+    tmp = R.from_matrix(new_rot_mat)
+    quart = tmp.as_quat()
+
+    size = [item['size']['x'], item['size']['y'], item['size']['z']]
+    size = np.array(size, np.float32)
+    corners_3d = my_compute_box_3d(center, quart, size)                                                     # get the bbox in kitti representation
+    category = obj['category']
+
+    assert_flag, heading, heading_plane = my_compute_heading_plane(quart, corners_3d)                       # this quart cooresponds full transformation matrix
+    # if assert_flag == False:
+    #   return False, [], []
+    #特别注意，下面的heading和heading_plane是虚拟lidar坐标系下的heading
+    bboxes.append([category, corners_3d, heading, heading_plane, center, quart, size])
+    cats.append(category)
+
+  return True, bboxes, cats
+
+
+def T_ROAD_P_AREA_mask_points_by_range(points, limit_range):
+  '''
+    ENSURE THAT THE point cloud IS PROPELY ROTATED!! (GROUND CALIBRATION AND Z-ROTATED)
+    limit_range = [T_1, T_2, T_3, T_4, T_5, T_6, T_7, T_8, T_9]
+  '''
+  mask_far =   (points[:, 0] >= limit_range[3]) & (points[:, 0] <= limit_range[2]) & (points[:, 1] >= limit_range[1]) & (points[:, 1] <= limit_range[0])
+  mask_close = (points[:, 0] >= 0)              & (points[:, 0] <= limit_range[4]) & (points[:, 1] >= limit_range[8]) & (points[:, 1] <= limit_range[7])
+  mask_mid =   (points[:, 0] >= limit_range[4]) & (points[:, 0] <= limit_range[3]) & (points[:, 1] >= limit_range[6]) & (points[:, 1] <= limit_range[5])
+  mask = mask_far + mask_mid + mask_close
+  return mask
+
+def gen_o3d_3dbboxes(corners_, cat):
+  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 = [col_dict[cat] for _ in range(len(bbox_lines))] 
+  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
+
+def read_pcd_with_intensity(pcd_path):
+    fp = open(pcd_path,'r')
+    pc = []
+    intens = []
+    for line in fp.readlines()[11:]:
+        x = float(line.split(' ')[0])
+        y = float(line.split(' ')[1])
+        z = float(line.split(' ')[2])
+        pc.append([x, y, z])
+        if len(line.split(' ')) > 3:
+            intensity = int(line.split(' ')[3])
+            intens.append(intensity / 255.0)
+
+    points = np.asarray(pc)
+    points = np.hstack((points, np.ones((len(points), 1))))
+
+    has_intensity = False
+    if len(intens) > 0:
+        intens = np.asarray(intens)
+        points[:, 3] = intens
+        has_intensity = True
+    
+    if has_intensity: 
+        print("=> intensity exists")
+    else:
+        print("=> no intensity")
+
+    return points, has_intensity
+
+def read_pcd(path):
+    pcd = o3d.io.read_point_cloud(path)
+    return np.asarray(pcd.points)
+
+def parse_pandarmind_pcd_rotated(pandar_pcd_path, rotMat, limit_range):
+
+  pcd = o3d.io.read_point_cloud(pandar_pcd_path)
+  bg_col = np.array([192/255, 192/255, 192/255])
+  bg_col = np.tile(bg_col, (len(pcd.points), 1))
+  pcd.colors =o3d.utility.Vector3dVector(bg_col)
+
+  converted_pcd = copy.deepcopy(pcd)
+
+  points_i = read_pcd(pandar_pcd_path)
+  print("=> read_pcd points shape", points_i.shape)
+  points = points_i[:, :3]
+
+  # do rotation
+  rot_points = np.dot(points, rotMat[:3, :3].T) + rotMat[:3, 3]
+
+  tmp = np.ones((len(rot_points), 1))
+  xyzi = np.hstack((rot_points, tmp))
+
+  if limit_range is not None: 
+    mask = T_ROAD_P_AREA_mask_points_by_range(xyzi, limit_range)
+    pcdpoints = xyzi[mask]
+  else:
+    pcdpoints = xyzi
+
+  converted_pcd.points = o3d.utility.Vector3dVector(pcdpoints[:,:3])
+
+  return xyzi, pcd, converted_pcd
+
+#将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()
+
+def write_geometry(file_path, geometry):
+  fp = open(file_path,'w')
+  line = " ".join([str(x) for x in geometry])
+  fp.write(line)
+  fp.close()
+
+#将新虚拟坐标系的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
+
+def read_json(jsn_path):
+  anno = {}
+  with open(jsn_path, 'r',encoding='utf-8',errors='ignore') as fp:
+    jsn = json.load(fp)
+
+  jsn = jsn["annotations"]
+  for an in jsn:
+    anno[an["fileuri"]] = an["labels_box3D"]
+  return anno
+
+rotMat = {}
+
+def _jp(x, y, z=None):
+  if not z:
+    return os.path.join(x, y)
+  return os.path.join(x, y, z)
+
+def _md(x):
+  os.makedirs(x, exist_ok=True)
+  print("=> creating ", x)
+
+def assert_pcdnames(subfolders):
+
+  for _, ff in enumerate(subfolders):
+    ff = ff.strip()
+    if "." in ff or "logs" == ff:
+      continue
+    assert "_" in ff and len(ff) == 3, "name: %s, not regular" % ff
+    
+def create_logger(log_file=None, rank=0, log_level=logging.INFO):
+    logger = logging.getLogger(__name__)
+    logger.setLevel(log_level if rank == 0 else 'ERROR')
+    formatter = logging.Formatter('%(asctime)s  %(levelname)5s  %(message)s')
+    console = logging.StreamHandler()
+    console.setLevel(log_level if rank == 0 else 'ERROR')
+    console.setFormatter(formatter)
+    logger.addHandler(console)
+    if log_file is not None:
+        file_handler = logging.FileHandler(filename=log_file)
+        file_handler.setLevel(log_level if rank == 0 else 'ERROR')
+        file_handler.setFormatter(formatter)
+        logger.addHandler(file_handler)
+    return logger    
+
+
+def verify_valid_annos_for_37degree(bboxes, cats, info_logger):
+  pass
+  
+
+def write_dataset(vfolder, name_prefix, xyzi, groundtruth, geometry):
+
+  img = np.zeros([2048, 2448, 3],dtype=np.uint8)
+  pcd_bin_path = _jp(vfolder, name_prefix + '.bin')
+
+  label_2_path = pcd_bin_path.replace('velodyne','label_2').replace('.bin','.txt')
+  write_label_2(label_2_path,groundtruth)
+
+  with open(pcd_bin_path,'bw') as fp:
+    xyzi.astype(np.float32).tofile(fp)
+  
+  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')
+  shutil.copy(calib_template_path, calib_path)
+
+  geometry_path = pcd_bin_path.replace('velodyne','geometry').replace('.bin','.txt')
+  write_geometry(geometry_path, geometry)
+
+
+def run_main(root_path, jsn_path, pcdfolder_name, to_save_dataset_root, KITTI_PROCESS, RANDOM_CHECK, jsonname, VIS_GROUND_PLANE):
+  
+  '''
+  rootpath  
+          |
+          |___ xxx.json
+          |___ 2_1 ...
+          |___ 2_2 ...
+          |___ 3_1
+                  |___ 3_1.yaml
+                  |___ img ...
+                  |___ pcd
+                          |___ 1624412629.057949000.pcd
+                          |___ ...
+  '''
+  os.makedirs(_jp(root_path, 'logs'), exist_ok=True)
+  log_file = _jp(root_path, 'logs', 'anno_process_log_%s.txt' % datetime.datetime.now().strftime('%Y%m%d-%H%M%S'))
+  info_logger = create_logger(log_file)
+  info_logger.info('**********************anno_process Start logging**********************')
+  
+
+  if KITTI_PROCESS:
+    RANDOM_CHECK = False
+    vfolder = _jp(to_save_dataset_root, "training/velodyne")
+    _md(vfolder)
+    _md(_jp(to_save_dataset_root, "training/label_2"))
+    _md(_jp(to_save_dataset_root, "training/image_2"))
+    _md(_jp(to_save_dataset_root, "training/calib"))
+    _md(_jp(to_save_dataset_root, "training/geometry"))
+
+
+  anno = read_json(jsn_path)
+  folders = os.listdir(root_path)           # 2_1, 2_2, 3_1....
+  # assert_pcdnames(folders)
+
+  info_logger.info('KITTI_PROCESS: %d' % int(KITTI_PROCESS))
+  info_logger.info('RANDOM_CHECK: %d' % int(RANDOM_CHECK))
+  info_logger.info('RANDOM_CHECK = 1 means only 10 pcds are shown / traversed')
+  info_logger.info("********* found following folders *********")
+
+  for x in folders: 
+    info_logger.info(x)
+  
+  dasdaqwr = 0
+  dasdaqwr_l = []
+  dasdaqwr_2 = []
+  num = 0 
+  num_dir = 0
+
+  for _, child_dir in enumerate(folders):
+    loc_str = child_dir
+    if child_dir != jsonname: continue
+    if child_dir == "logs": continue
+    if child_dir.endswith(".json"): continue
+    num_dir += 1
+    cfg_path = _jp(root_path, child_dir, loc_str + ".yaml")
+
+    cfg = read_yaml(cfg_path)
+    ORIGIN2KITTI = np.array(cfg.POINTPILLARS.ORIGIN2KITTI)
+    PC_T_RANGE =  np.array(cfg.POINTPILLARS.PC_T_RANGE)
+    if len(PC_T_RANGE) ==10:
+      PC_T_RANGE = PC_T_RANGE[:9]
+    assert len(PC_T_RANGE) == 9
+    
+    info_logger.info("=> visiting %s.." % child_dir)
+    
+    pcd_folder = os.path.join(root_path, child_dir, pcdfolder_name)   # "2_3/pcd"
+    pcd_list = os.listdir(pcd_folder)
+    child_ind = start_ind[child_dir]
+
+    if RANDOM_CHECK:
+      random.shuffle(pcd_list)
+
+    for pcd_ind, pcd_file in enumerate(pcd_list):
+      
+      if RANDOM_CHECK:
+        if pcd_ind == NUM_REVIEW:   # check only first 10 annotations of each folder when we want REVIEW
+          break 
+      pcd_abs_file = _jp(pcd_folder, pcd_file)                    # "2_3/pcd/1232321321.12312312.pcd"
+      anno_key =  child_dir + "/" + pcdfolder_name + "/" + pcd_file
+      # if anno_key != "5_1/pcd/1629663169.047605000.pcd": continue
+      name_prefix = child_dir + "_" + str(child_ind)
+      child_ind += 1
+
+      progress = "%d / %d " %(pcd_ind, NUM_REVIEW) if RANDOM_CHECK else "%d / %d " %(pcd_ind, len(pcd_list)) 
+      print('=> pcd_file: ', pcd_file)
+      print("=> pcd_ind / total_len: ", progress)
+      print("=> anno_key: ", anno_key)
+      print('=> generate ', child_ind)
+      
+      if anno_key not in anno:
+        print('=> the anno is null : ', anno_key)
+        dasdaqwr += 1
+        dasdaqwr_l.append(anno_key)
+        continue
+      
+      # read annotation and transfrom 
+      
+      bbox_flag, bboxes, cats = read_rotate_boxes(anno[anno_key], ORIGIN2KITTI)  # ORIGIN2KITTI is the combi of ground calibration and kitti conversion
+
+      # verify_valid_annos_for_37degree(bboxes, cats, info_logger)
+      if bbox_flag == False:
+        dasdaqwr += 1
+        dasdaqwr_2.append(anno_key)
+        continue
+      
+
+      # transform the pcd
+      xyzi, pcd, converted_pcd = parse_pandarmind_pcd_rotated(pcd_abs_file, ORIGIN2KITTI, None)
+
+      if not KITTI_PROCESS:
+        merge_geos = [converted_pcd]
+
+      groundtruth = []
+
+      for i, bbox in enumerate(bboxes):
+        if not KITTI_PROCESS:
+          merge_geos += [gen_o3d_3dbboxes(bbox[1], cats[i])]
+          front_plane = [bbox[1][1], bbox[1][5], bbox[1][6]]
+          heading_point = (front_plane[0] + front_plane[2]) / 2
+          arrow_vector = np.cross(front_plane[0] - front_plane[1], front_plane[2] - front_plane[1]) 
+
+          # mesh_arrow = get_arrow(cats[i], heading_point, arrow_vector)
+          line_arrow = get_line_arrow(cats[i], heading_point, arrow_vector)
+          merge_geos += [line_arrow]
+
+        #下面将new virtual heading转换为camera坐标系下的heading
+        gt_heading = lidar_heading_to_camera(bbox[2])
+        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 not KITTI_PROCESS:
+        axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=3, origin=[0, 0, 0])
+        pc_geometry = draw_T_geometry(PC_T_RANGE)
+        merge_geos += [axis_pcd, pc_geometry]
+
+        perception_range = draw_circle(radius=80, resolution=70, color="shallowblue")
+        annotation_range = draw_circle(radius=100, resolution=70, color="shalloworange")
+        merge_geos += [perception_range, annotation_range]
+
+        if VIS_GROUND_PLANE:
+          ground_plane = draw_ground_plane()
+          merge_geos += [ground_plane]
+
+        wn = progress + "key: " + anno_key + " num obj: %d" % len(bboxes)
+        o3d.visualization.draw_geometries(geometry_list=merge_geos,window_name=wn)
+      
+      if KITTI_PROCESS:
+        num += 1
+        write_dataset(vfolder, name_prefix, xyzi, groundtruth, PC_T_RANGE)
+
+    if KITTI_PROCESS:
+      info_logger.info("********* FINISHED *********")
+      info_logger.info("total %d samples not created " % dasdaqwr)
+      info_logger.info("********* following is not found in anno json: *********")
+
+      for x in dasdaqwr_l: 
+        info_logger.info(x)
+      info_logger.info("********* following is skipped due to wrong direction label: *********")
+
+      for x in dasdaqwr_2: 
+        info_logger.info(x)
+      info_logger.info("=> DONE")
+      
+      info_logger.info("=> Total %d samples from %d folders are processed" % (num, num_dir))
+
+def assert_string(x):
+  x = x.strip()
+  assert len(x) > 0
+
+
+def color_value(r, g, b):
+  return np.array([r / 255, g / 255, b / 255])
+
+if __name__ == '__main__':
+
+  '''
+  rootpath  
+          |
+          |___ xxx.json
+          |___ 2_1 ...
+          |___ 2_2 ...
+          |___ 3_1
+                  |___ 3_1.yaml
+                  |___ img ...
+                  |___ pcd
+                          |___ 1624412629.057949000.pcd
+                          |___ ...
+  '''
+
+  global colormap, calib_template_path, start_ind, NUM_REVIEW
+
+  # naming: year + date + int(locstr) + 0000
+  start_ind = {'1_1': 210818110000, 
+             '7_1': 210818710000,
+             '2_1': 210831210000,
+             '2_2': 210831220000, 
+             '2_3': 210907230000, 
+             '2_4': 210907240000, 
+             '3_1': 210831310000,
+             '3_2': 210831320000, 
+             '4_1': 210907410000, 
+             '4_2': 210831420000, 
+             '6_3': 210831630000, 
+             '8_1': 210831810000, 
+             '9_1': 210831910000, 
+             '5_2': 210903520000, 
+             
+             '6_1': 210907610000, 
+             '6_2': 210907620000, 
+             '9_2': 210907920000, 
+             '10_2': 2109071020000, 
+             }
+
+  colormap = {
+        "shalloworange": color_value(255, 178, 102),
+        "shallowblue": color_value(0, 255, 255),
+      }
+  KITTI_PROCESS = 0
+  RANDOM_CHECK = True
+  
+  NUM_REVIEW = 20
+
+  to_save_dataset_root = r"D:\wtdata\kitti_out_batch_2"
+  calib_template_path = r'C:\Users\touch\OneDrive\arbeit\code\general_tools\hs\calib.txt'
+  rootpath = r"D:\wtdata\batch_2"
+  VIS_GROUND_PLANE= 0
+
+  assert_string(rootpath)
+  
+  # to_do_list_b1 = ['2_1','2_2', '3_1','3_2', '4_2', '5_1', '5_3', '6_3', '8_1', '9_1', '5_2']
+  to_do_list_b2 = ['2_3','2_4', '4_1','6_1', '6_2', '9_2','10_1', '10_2', "5_2"]
+  
+  for jsonname in to_do_list_b2[::-1]:
+    # if jsonname != "4_1": continue
+    # if jsonname != "6_1": continue
+    assert_string(jsonname)
+    if KITTI_PROCESS:
+      assert jsonname in start_ind
+      assert_string(to_save_dataset_root)
+
+    jsn_path = _jp(rootpath, jsonname + ".json")
+    pcdfolder_name='pcd'     
+
+    run_main(rootpath, jsn_path, pcdfolder_name, to_save_dataset_root, KITTI_PROCESS, RANDOM_CHECK, jsonname, VIS_GROUND_PLANE)
+
+

script/trans_data.py

@@ -516,7 +516,15 @@ if __name__ == '__main__':
         angle = car_yaw_cal(79.89299572540227,bbox[2])
         mapInfoExport = get_map_data(exportCenterBL[0],exportCenterBL[1],angle)
         print("call get ex data isInMap = ",mapInfoExport)
-        if mapInfo[0] != 1 or mapInfoExport[0] != 1:
+        if mapInfoExport[0] != 1:
+          continue;
+        laneAngle = mapInfoExport[10];
+        detaAngel = laneAngle - angle;
+        while detaAngel > 180:
+          detaAngel -= 360
+        while detaAngel < -180:
+          detaAngel +=- 360
+        if( abs(detaAngel) > 30)
           continue;
         flag = in_hull(xyz,bbox[1])
         pickcloud = xyz[flag]