自动驾驶坐标转换（代码）

    for json_file in tqdm(json_files):
        assert json_file.endswith('.json'), f'json_file: {json_file}'
        bmk_data = json.load(refile.smart_open(json_file, 'r'))
    
        frame_data = bmk_data['frame_data']
        # print('gt frame num: {}'.format(len(frame_data)))
    
        lidar2ego_v = bmk_data['calibrated_sensors']['lidar2ego']
        lidar2ego = np.eye(4)
        lidar2ego[:3, :3] = Quaternion(list(lidar2ego_v['rotation'].values())).rotation_matrix
        lidar2ego[:3, 3] = np.transpose(np.array(list(lidar2ego_v['translation'].values())))
        
        lidar2cam_v = bmk_data['calibrated_sensors'][camera]['extrinsic']
        lidar2cam = np.eye(4)
        lidar2cam[:3, :3] = Quaternion(list(lidar2cam_v['rotation'].values())).rotation_matrix
        lidar2cam[:3, 3] = np.transpose(np.array(list(lidar2cam_v['translation'].values())))
        
        intrinsic = bmk_data['calibrated_sensors'][camera]['intrinsic']
        camera_k = np.array(intrinsic['K'])
    
        calibrate_info = {'lidar2ego':lidar2ego, 'lidar2cam':lidar2cam, 'camera_k':camera_k} # 只构造一份, 防止内存太大

      lidar2ego_v = bmk_data['calibrated_sensors']['lidar2ego']

    {'rotation': {'w': -0.7071067811865474, 'x': 0, 'y': 0, 'z': 0.7071067811865477}, 'translation': {'x': 0, 'y': 0, 'z': -0.33000001311302185}}

    from pyquaternion import Quaternion

    lidar2ego = np.eye(4)
    lidar2ego[:3, :3] = Quaternion(list(lidar2ego_v['rotation'].values())).rotation_matrix

    >> list(lidar2ego_v['rotation'].values())
    >> [-0.7071067811865474, 0, 0, 0.7071067811865477]

    Quaternion(list(lidar2ego_v['rotation'].values())).rotation_matrix

    array([[-4.47411937e-16,  1.00000000e+00,  0.00000000e+00],
       [-1.00000000e+00, -4.67705010e-16,  0.00000000e+00],
       [ 0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])

     lidar2cam_v = bmk_data['calibrated_sensors'][camera]['extrinsic']
     lidar2cam = np.eye(4)
     lidar2cam[:3, :3] = Quaternion(list(lidar2cam_v['rotation'].values())).rotation_matrix
     lidar2cam[:3, 3] = np.transpose(np.array(list(lidar2cam_v['translation'].values())))

     intrinsic = bmk_data['calibrated_sensors'][camera]['intrinsic']
     camera_k = np.array(intrinsic['K'])

    def convert_points2grid(self, points, bev_range, map_resolution):
        # HACK: 后续考虑对Z 也做offsets的学习
        x_max, x_min, y_max, y_min = bev_range    # 150,0,15,-15
        xyz = deepcopy(points)
        xyz[:, 0] = (x_max - points[:, 0]) / map_resolution[0]    #0.75
        xyz[:, 1] = (y_max - points[:, 1]) / map_resolution[1]    #0.15
        return xyz

    def draw_bev_seg_point(self, img_bev, seg_points,attr, color=(255,0,0)):
        xmax, xmin, ymax, ymin, zmax, zmin = self.map_range
        for i, pts in enumerate(copy.deepcopy(seg_points)):
            new_pts = np.array(pts)[:2]
            new_pts[0] = (xmax - new_pts[0]) // self.map_resolution[0]
            new_pts[1] = (ymax - new_pts[1]) // self.map_resolution[1]
            img_bev = cv2.circle(img_bev, np.int32(new_pts[::-1]), 12, color, -1)
            # cv2.putText(img_bev, str(attribs[i]), np.int32(new_pts[::-1]) , cv2.FONT_HERSHEY_COMPLEX, 0.5, color, 1)
        return img_bev

    		 x_max, x_min, y_max, y_min = bev_range    # 150,0,15,-15
    		 map_resolution = [0.75,0.15]
        bev_v =  (gt_xyz[:,1] - (ymin + map_resolution[1]/ 2.)) / map_resolution[1]    # data gt is ego: ego->bev
        bev_u =  (gt_xyz[:,0] -(xmin. + map_resolution[0]/ 2.)) /map_resolution[0]       # bev_uv to bev_map
        bev_uv = np.stack((bev_u,bev_v),axis=-1)

     gt_xyz[:,0] = 150 - gt_xyz[:,0] * 0.75    # data gt is bev: bev->ego
     gt_xyz[:,1] = 15 - gt_xyz[:,1] * 0.15     # gt_xyz ego

     def proj_3dto2d(self, bev_lanes,ego2cam,camK):
     uvs = []
     vis_bev_lanes = []
     for i, bev_lane in enumerate(bev_lanes):
         bev_lane = np.array(bev_lane).reshape(-1, 3)
         xyz = np.stack([bev_lane[:, 0], bev_lane[:, 1], bev_lane[:, 2], np.ones(bev_lane.shape[0])])  # [4, n]
         cam_coord_xyz = ego2cam @ xyz
         cam_coord_xyz = cam_coord_xyz[:, cam_coord_xyz[2] > 0][:3]
         u_v = cam_K @ cam_coord_xyz
         u_v = u_v[:2, :] / u_v[2, :]
         uvs.append(u_v)
         vis_bev_lanes.append(xyz.T[:, :3])
     return uvs, vis_bev_lanes

    def bev_to_rv(bev_lane, cam2ego, camera_k):
    nr_points = bev_lane.shape[0]
    bev_lane_v = np.ones((nr_points, 4))
    bev_lane_v[:, :3] = bev_lane
    bev_lane_v = bev_lane_v.T
    
    bev_lane_v = np.linalg.inv(cam2ego) @ bev_lane_v
    bev_lane_v[:3, :] /= bev_lane_v[3, :]
    bev_lane_v = bev_lane_v[:, bev_lane_v[2, :] > 0]
    rv_lane_v = (camera_k @ bev_lane_v[:3, :]).T
    rv_lane = rv_lane_v / rv_lane_v[:, 2:]
    rv_lane = rv_lane[rv_lane[:, 2] > 0][:, :2]
    return rv_lane

    def proj_3dto2d(lidar2camera_proj, ldiar2ego_proj, points3d, h, w):
    proj = lidar2camera_proj @ np.linalg.inv(ldiar2ego_proj)
    xyz = np.stack([points3d[:, 0], points3d[:, 1], points3d[:, 2], np.ones(points3d.shape[0])])  # [4, n]
    cam_coord_xyz = proj @ xyz
    
    u_v = cam_coord_xyz[:, cam_coord_xyz[2] > 0][:3]
    u_v = u_v[:2, :] / u_v[2, :]
    uv_mask_u = np.logical_and(u_v[1] >= 0, u_v[1] < h)
    uv_mask_v = np.logical_and(u_v[0] >= 0, u_v[0] < w)
    uv_mask = np.logical_and(uv_mask_u, uv_mask_v)
    u_v_new = u_v.T[uv_mask]
    return u_v_new

    def proj_3dto2d_fovformask(lidar2camera_proj, ldiar2ego_proj, points3d, w, h):
    proj = lidar2camera_proj @ np.linalg.inv(ldiar2ego_proj)
    xyz = np.stack([points3d[:, 0], points3d[:, 1], points3d[:, 2], np.ones(points3d.shape[0])])  # [4, n]
    cam_coord_xyz = proj @ xyz # (4, n)
    
    # 过滤掉depth < 0 的点
    points3d = points3d
    # [cam_coord_xyz[2] > 0]
    u_v = cam_coord_xyz[:3]
    # [:, cam_coord_xyz[2] > 0][:3]
    u_v = u_v[:2, :] / u_v[2, :]
    
    # 过滤掉u_v 不在范围内的点
    uv_mask_u = np.logical_and(u_v[1] >= 0, u_v[1] < h)
    uv_mask_v = np.logical_and(u_v[0] >= 0, u_v[0] < w)
    uv_mask = np.logical_and(uv_mask_u, uv_mask_v)
    uv_mask = np.logical_and(uv_mask, cam_coord_xyz[2] > 0)
    points3d_cp = copy.deepcopy(points3d.tolist())
    points3d= points3d[uv_mask]
    u_v = u_v[:, :]
    
    return u_v.T, points3d, uv_mask

      def _inter3d_once(lane_spec, interp_dist):
      if len(lane_spec) < 2:
          return lane_spec
      lane_spec = lane_spec.tolist()
      lane_spec.sort(key=lambda _x: _x[0])
      lane_spec = np.array(lane_spec)
      x, y, z = lane_spec[:, 0], lane_spec[:, 1], lane_spec[:, 2]
      x_inter = np.append(np.arange(min(x), max(x), interp_dist), max(x))
      y_inter = np.interp(x_inter, x, y)
      z_inter = np.interp(x_inter, x, z)
      lane_inter = np.concatenate(
          [x_inter.reshape(-1, 1), y_inter.reshape(-1, 1), z_inter.reshape(-1, 1)],
          axis=1,
      )
      return lane_inter

    _inter3d_once(pts, interp_dist=min(map_resolution) / 2.0)

    def cal_dis(pt1, pt2):
    dis = (pt1[0] - pt2[0]) ** 2 +\
               (pt1[1] - pt2[1]) ** 2 +\
               (pt1[2] - pt2[2]) *
    return math.sqrt(dis)
    
    def interp_3d(lane_3d):
    def generate_multi_points_among_two_points(pointA, pointB):
        if pointA[0] == pointB[0]:  # 避免除0
            return [pointA]
        dis = cal_dis(pointB, pointA)
        num_gen = int(dis // 0.15)
        delta = (pointB - pointA) / num_gen
        grid_xyz = [pointA]
        for step in range(1, num_gen):
            grid_xyz.append(pointA + step * delta)
        # grid_xyz.append(pointB)
        return grid_xyz
    
    # 每两个点之间插n个点， 最后再稀疏回去
    new_lane_points = []
    for i in range(len(lane_3d) - 1):
        grid_xyz = generate_multi_points_among_two_points(
            lane_3d[i], lane_3d[i + 1]
        )
        new_lane_points += grid_xyz
    new_lane_points.append(lane_3d[-1])
    return np.array(new_lane_points)

     def points_filter_by_range(lanes, x_max, x_min, y_max, y_min):
     if len(lanes) == 0:
         return lanes, []
     mask1 = np.bitwise_and(lanes[:, 0] >= x_min, lanes[:, 0] < x_max)
     mask2 = np.bitwise_and(lanes[:, 1] >= y_min, lanes[:, 1] < y_max)
     mask = np.bitwise_and(mask1, mask2)
     lanes = lanes[mask]
     return lanes, mask

    def undistort(img, cam_intrin):
    h, w = img.shape[:2]
    k = np.array(cam_intrin["K"]).reshape((3, 3))
    d = np.array(cam_intrin["D"])
    mode = cam_intrin["distortion_model"]
    if mode == "pinhole":       # 针孔相机 
        mapx, mapy = cv2.initUndistortRectifyMap(k, d, None, k, (w, h), 5)
    elif mode == "fisheye":     # 鱼眼相机
        mapx, mapy = cv2.fisheye.initUndistortRectifyMap(k, d, None, k, (w, h), 5)
    return cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)

    import matplotlib as mpl
    import matplotlib.pyplot as plt
    from PIL import Image
    
    def draw_lanes3d(self, all_lane_points):
    """
    visual all lane points in 3D ax plot, in the final plot, the coordinates have changed to normal coordinates
    input:
    all_lane_points: [lane_num, ]  all_lane_points[0]: [N,3]
    save_path: the dir path of saved image
    """
    xmax, xmin, ymax, ymin, zmax, zmin = self.map_range
    mpl.rcParams["legend.fontsize"] = 10
    fig = plt.figure(figsize=(16, 16), dpi=90)
    ax = fig.gca(projection="3d")
    plt.xlim((ymin, ymax))
    plt.ylim((xmin, xmax))
    ax.set_zlim((zmin, zmax))
    # all_lane_points = np.array(all_lane_points)
    for i in range(len(all_lane_points)):
        lane_points = all_lane_points[i]
        lane_points = np.array(lane_points)
        keep_idx1 = np.bitwise_and(lane_points[:, 0] >= xmin, lane_points[:, 0] <= xmax)
        keep_idx2 = np.bitwise_and(lane_points[:, 1] >= ymin, lane_points[:, 1] < ymax)
        lane_points = lane_points[np.bitwise_and(keep_idx1, keep_idx2)]
        x = lane_points[:, 0]
        y = lane_points[:, 1]
        z = lane_points[:, 2]
        color = colors[np.mod(i, len(colors))]
        ax.plot(-y, x, z, color=color, linewidth=3, label="3D Lane %d" % i)  # normal coordinates
        ax.plot(np.ones(x.shape) * (-15), x, z, color=color, linestyle="-.", linewidth=3, label="3D Lane %d" % i)
    
    
    
    fig.canvas.draw()
    image_from_plot = np.frombuffer(fig.canvas.tostring_rgb(), dtype=np.uint8)
    image_from_plot = image_from_plot.reshape(fig.canvas.get_width_height()[::-1] + (3,))
    return image_from_plot

     xyz_vis_img = self.draw_lanes3d(vis_bev_lanes)
     cv2.imwrite("3d_lane.jpg",xyz_vis_img )

    #img_bev= np.zeros((200,200,3),dtype=np.uint8) + 64
    def draw_circle(self, img_bev, 3d_points, attribs, color=(255,0,0)):
    xmax, xmin, ymax, ymin, zmax, zmin = self.map_range
    for i, pts in enumerate(copy.deepcopy(3d_points)):
        new_pts = np.array(pts)[:2]
        new_pts[0] = (xmax - new_pts[0]) // self.map_resolution
        new_pts[1] = (ymax - new_pts[1]) // self.map_resolution
        img_bev = cv2.circle(img_bev, np.int32(new_pts[::-1]), 5, color, 5)
        # cv2.putText(img_bev, str(attribs[i]), np.int32(new_pts[::-1]) , cv2.FONT_HERSHEY_COMPLEX, 0.5, color, 1)
    return img_bev

    def draw_img(ego2img,showimage,gt_xyz,color):
    
    points_uv = get_points_uv(ego2img,gt_xyz,showimage)               
    if points_uv.shape[0] == 0:
        return showimage
    
      
    showimage = cv2.polylines(showimage, [points_uv.astype(np.int64)], False, color, 3)
    
    
    #showimage = cv2.putText(showimage, show_attr, (points_uv[points_uv.shape[0] // 5].astype(np.int64)), cv2.FONT_HERSHEY_COMPLEX,0.75, (0, 255, 255), 1)   
      
    return showimage
    
    def get_points_uv(ego2img,gt_xyz,showimage):
    points_uvz = ego2img @ gt_xyz.T
    points_uvz = points_uvz.T
    points_uvz[:,0] = points_uvz[:,0] / points_uvz[:,2]
    points_uvz[:,1] = points_uvz[:,1] / points_uvz[:,2]
    points_uv = points_uvz[:,0:2]
    mask = (points_uv[:,0] >= 0) & \
            (points_uv[:,1] >= 0) & \
            (points_uv[:,0] < showimage.shape[1]) & \
            (points_uv[:,1] < showimage.shape[0])
    points_uv = points_uv[mask]
    return points_uv.cpu().numpy()