3d to cylinder projection

camera.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Copyright (C) 2020, Momenta Co. Ltd.
# All rights reserved.

import logging
import math
from copy import deepcopy

import numpy as np
from scipy.spatial.transform import Rotation as R


def rotate_mat(axis, radian):
    return expm(np.cross(np.eye(3), axis / np.linalg.norm(axis) * radian))


def extrinsic(cam_param):
    R_w2c = np.array(cam_param['rotation_matrix']).reshape(3, 3)
    T_w2c = np.array([cam_param["cam_to_front"],
                     (cam_param["cam_to_left"] -
                      cam_param["cam_to_right"]) / 2.0,
                     -cam_param['camera_height']])
    T_w2c = R_w2c.dot(T_w2c)
    R_l2c = np.eye(4)
    R_l2c[:3, :3] = R_w2c
    R_l2c[:3, 3]  = T_w2c
    extrinsic_list = R_w2c.reshape(-1).tolist()
    extrinsic_list.extend(T_w2c.reshape(-1).tolist())
    return R_l2c, extrinsic_list


def eulerAnglesToRotationMatrix(theta):
    R_x = np.array([[1, 0, 0],
                    [0, math.cos(theta[0]), -math.sin(theta[0])],
                    [0, math.sin(theta[0]), math.cos(theta[0])]])
    R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
                    [0, 1, 0],
                    [-math.sin(theta[1]), 0, math.cos(theta[1])]])
    R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
                    [math.sin(theta[2]), math.cos(theta[2]), 0],
                    [0, 0, 1]])
    R = np.dot(R_z, np.dot(R_y, R_x))
    return R


def regular_angle(angle):
    if angle > np.pi:
        angle -= 2 * np.pi
    elif angle < -np.pi:
        angle += 2 * np.pi
    return angle


def rotate_upright(R_l2c):
    ext_cam_body = np.linalg.inv(R_l2c)
    camhlu_to_cam_R = np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]])
    camhlu_to_body_R = ext_cam_body[:3, :3].dot(camhlu_to_cam_R)
    rot = R.from_dcm(camhlu_to_body_R)
    euler = rot.as_euler('zyx')
    if abs(euler[1]) + abs(euler[2]) > 0.8 * np.pi:
        yaw = regular_angle(euler[0] + np.pi)
    else:
        yaw = regular_angle(euler[0])
    upright_ext_camhlu_body_R = eulerAnglesToRotationMatrix([0, 0, yaw])
    upright_ext_cam_body_R = upright_ext_camhlu_body_R.dot(
        np.linalg.inv(camhlu_to_cam_R))
    R_c2l = np.zeros((4, 4))
    R_c2l[:3, :3] = upright_ext_cam_body_R
    R_c2l[:, 3] = ext_cam_body[:, 3]
    R_c2l[3, 3] = 1
    return np.linalg.inv(R_c2l)


def cylinder_intrinsic(hfov_degree, width, fov_u, fov_d):
    cx = width * 0.5
    fx = width / (hfov_degree / 180.0 * math.pi)
    fy = fx
    upper_height = math.tan(fov_u * math.pi / 180.0) * fy
    cy = int(upper_height * 2) * 0.5
    return fx, fy, cx, cy


class LidarHelper(object):
    def __init__(self, x, y, z, h, d, w, yaw, pitch, roll, 
                 vx=None, vy=None, vz=None, track_id=-1):
        self.x = float(x)
        self.y = float(y)
        self.z = float(z)
        self.d = float(d)  # height
        self.h = float(h)  # width
        self.w = float(w)  # length
        self.yaw   = float(yaw)
        self.pitch = float(pitch)
        self.roll  = float(roll)
        self.vx = vx
        self.vy = vy
        self.vz = vz
        self.track_id = int(track_id)

class CylinderCameraHelper:
    ''' to be documented '''
    
    @staticmethod
    def convert_fisheye_to_cylinder(fisheye_param, image_h, image_w):
        cylinder_param = deepcopy(fisheye_param)
        cylinder_param['image_height'] = image_h
        cylinder_param['image_width']  = image_w
        R_l2c = np.eye(4)
        R_l2c[:3, :3] = np.array(cylinder_param['extrinsic'][:9]).reshape(3, 3)
        R_l2c[:3, 3]  = np.array(cylinder_param['extrinsic'][9:12])
        cylinder_param['extrinsic'] = rotate_upright(R_l2c).reshape(-1).tolist()
        fx, fy, cx, cy = cylinder_intrinsic(
            hfov_degree=180, width=image_w, fov_u=30, fov_d=60) # HACK: magic number
        cylinder_param['intrinsic'] = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
        return cylinder_param
    
    @staticmethod
    def lidar_to_camera(cam_param, lidar):
        R_l2c = np.array(cam_param['extrinsic']).reshape((4, 4))
        center_coor = np.array([lidar.x, lidar.y, lidar.z])
        corners = [
            np.array([ lidar.w/2,  lidar.h/2,  lidar.d/2]),
            np.array([-lidar.w/2,  lidar.h/2,  lidar.d/2]),
            np.array([-lidar.w/2, -lidar.h/2,  lidar.d/2]),
            np.array([ lidar.w/2, -lidar.h/2,  lidar.d/2]),
            np.array([ lidar.w/2,  lidar.h/2, -lidar.d/2]),
            np.array([-lidar.w/2,  lidar.h/2, -lidar.d/2]),
            np.array([-lidar.w/2, -lidar.h/2, -lidar.d/2]),
            np.array([ lidar.w/2, -lidar.h/2, -lidar.d/2])
        ]
        euler_angle = np.array([lidar.roll, lidar.pitch, lidar.yaw])
        rotation_matrix = eulerAnglesToRotationMatrix(euler_angle)
        center3d = np.array(
            [center_coor[0], center_coor[1], center_coor[2], 1])
        center3d = R_l2c.dot(center3d)[:3]

        corners3d = []
        for corner in corners:
            corner_coor = rotation_matrix.dot(corner) + center_coor
            corner3d = np.array(
                [corner_coor[0], corner_coor[1], corner_coor[2], 1])
            corner3d = R_l2c.dot(corner3d)[:3]
            corners3d.append(corner3d.tolist())
        return center3d, corners3d

    @staticmethod
    def camera_to_image(cam_param, p3d):
        valid = False
        if p3d[2] < 0 or p3d[0]**2 + p3d[2]**2 < 1e-6:
            return valid, [0, 0]

        fx = cam_param['intrinsic'][0]
        fy = cam_param['intrinsic'][4]
        cx = cam_param['intrinsic'][2]
        cy = cam_param['intrinsic'][5]

        theta_x = math.atan2(p3d[0], p3d[2])
        yr = p3d[1] / math.sqrt(p3d[0]**2 + p3d[2]**2)
        p2d = np.array([fx * theta_x + cx, fy * yr + cy])

        p2d[0] = min(cam_param['image_width'], max(0, p2d[0]))
        p2d[1] = min(cam_param['image_height'], max(0, p2d[1]))
        return True, p2d.tolist()

    @staticmethod
    def image_to_camera(cam_param, p2d):
        fx = cam_param['intrinsic'][0]
        fy = cam_param['intrinsic'][4]
        cx = cam_param['intrinsic'][2]
        cy = cam_param['intrinsic'][5]

        xd = p2d[0] - cx
        yd = p2d[1] - cy
        theta_x = xd / fx
        yr = yd / fy
        xr = math.sin(theta_x)
        zr = math.cos(theta_x)
        length_inv = 1 / math.sqrt(xr**2 + yr**2 + zr**2)
        p3d = np.array([length_inv * xr, length_inv * yr, length_inv * zr])
        return p3d.tolist()

main.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import random
import os.path as osp
import logging
import json
from glob import glob
import numpy as np
import matplotlib
# matplotlib.use('Agg')
import matplotlib.pyplot as plt
import cv2

logging.basicConfig(format='[%(filename)s:%(lineno)d] %(message)s', level=logging.INFO)

import camera
from vis import draw_text, draw_bbox, text_args


def get_times(pattern):
  files = glob(pattern)
  times = np.array([np.int64(osp.basename(p).split('.')[0]) for p in files])
  return times

def get_nearest_time(t, times):
  return times[np.abs(np.int64(t) - times).argmin()]

class FramesHolder:
  def __init__(self, data_dir, file_ext, exact=False, delay_thresh=1000):
    self.dir = data_dir
    self.ext = file_ext
    self.exact = exact
    self.delay_thresh = delay_thresh
    if not self.exact:
      self.times = get_times(osp.join(self.dir, '*.' + self.ext))

  def get_nearest_file(self, t):
    t = np.int64(t)
    t_nearest = t if self.exact else get_nearest_time(t, self.times)
    if abs(t_nearest - t) > self.delay_thresh:
      logging.warning('diff ({}, {}) > delay_thresh: {}, {}'.format(t, t_nearest, self.delay_thresh, self.dir))
      return None
    match_p = osp.join(self.dir, '{}.{}'.format(t_nearest, self.ext))
    assert osp.isfile(match_p), match_p
    return match_p, t_nearest - t



def show_lidar(info3d, cam_param, ax):
  df = []
  # process each object
  i_obj = 0
  for obj in info3d['children']:
    lidar_info = camera.LidarHelper(x=obj['x'], y=obj['y'], z=obj['z'],
                                    h=obj['width'], d=obj['height'], w=obj['length'],
                                    yaw=obj['yaw'], pitch=obj['pitch'], roll=obj['roll'],
                                    track_id=obj['uuid'])
    center3d, corners3d = camera.CylinderCameraHelper.lidar_to_camera(cam_param, lidar_info)
    corners3d = np.array(corners3d)
    color = np.random.random(3)
    all_pt2d = []
    for i, j in [(0, 1), (1, 2), (2, 3), (3, 0), (4, 5), (5, 6), (6, 7), (7, 4), (0, 4), (1, 5), (2, 6), (3, 7)]:
      line2d = []
      for a in np.linspace(0, 1, 30):
        p3d = corners3d[i] * a + corners3d[j] * (1 - a)
        valid, p2d = camera.CylinderCameraHelper.camera_to_image(cam_param, p3d)
        if valid:
          line2d.append(p2d)
      if line2d:
        all_pt2d.append(line2d[0])
        all_pt2d.append(line2d[-1])
        line2d = np.array(line2d)
        ax.plot(line2d[:, 0], line2d[:, 1], '-', color=color, lw=1)
    # draw number
    if all_pt2d:
      pt2d = random.choice(all_pt2d)
      draw_text(ax, pt2d[0], pt2d[1], i_obj, color=color, **text_args)
      i_obj += 1
      df.append({ 'id': i_obj,
                  'type': obj['data']['type'][0],
                  'score': obj['score'],
                  'tag': obj['tag'],
                  'uuid': obj['uuid'],
                  })
  return pd.DataFrame(df)

if __name__ == "__main__":
  im_p = '/data1/output/L_fix/bag/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/fisheye_B_dewarp/1602648542800684.360lidar.jpg'
  t = osp.basename(im_p).split('.')[0]
  im = cv2.imread(im_p)[..., ::-1]
  print('im:', im_p)

  cam_param_p = '/data1/output/L_fix/bag/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/calib_source/cameraB195.json'
  bag_2d_holder = FramesHolder('/data1/output/L_fix/bag/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/2d_results/Rear195/', 'json')
  bag_3d_holder = FramesHolder('/data1/output/L_fix/bag/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/msd_score_lidar_ap/', 'pcd.tar.json', delay_thresh=1e8)
  track_holder = FramesHolder('/data1/output/L_fix/bag/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/msd_score_lidar_ap/', 'pcd.tar.json.new', delay_thresh=1e8)
  align_holder = FramesHolder('/data1/output/L_fix/align/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/fisheye_B_lidar/', 'pcd.tar.json', delay_thresh=1e8)

  # camera param
  with open(cam_param_p) as f:
    cam_param = json.load(f)
  R_l2c_fisheye, extrinsic_list = camera.extrinsic(cam_param)
  cam_param['extrinsic'] = extrinsic_list
  cylinder_param = camera.CylinderCameraHelper.convert_fisheye_to_cylinder(
      cam_param, image_h=im.shape[0], image_w=im.shape[1])

  M, N = 2, 2

  bag2d_p, delay2d = bag_2d_holder.get_nearest_file(t)
  print('2d:', delay2d, bag2d_p)
  #    -9 /data1/output/L/bag/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/2d_results/Rear195/1602648543900660.json
  with open(bag2d_p) as f:
    info2d = json.load(f)
  df2d = pd.DataFrame(info2d)
  # show
  ax = plt.subplot(M, N, 1)
  ax.imshow(im)
  ax.set_title('2d: {}'.format(delay2d))
  for i, car in enumerate(info2d):
    color = np.random.random(3)
    draw_bbox(ax, car, color=color)
    draw_text(ax, car['left'], car['top'], i, color=color, **text_args)

  bag3d_p, delay3d = bag_3d_holder.get_nearest_file(t)
  print('3d:', delay3d, bag3d_p)
  # 49361 /data1/output/L/bag/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/msd_score_lidar_ap/1602648543950030.pcd.tar.json
  with open(bag3d_p) as f:
    info3d = json.load(f)
  # show
  ax = plt.subplot(M, N, 2)
  ax.imshow(im)
  ax.set_title('3d: {}'.format(delay3d))
  df3d = show_lidar(info3d, cylinder_param, ax)

  track_p, delay_track = track_holder.get_nearest_file(t)
  print('track:', delay_track, track_p)
  # 49361 /data1/output/L/bag/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/msd_score_lidar_ap/1602648543950030.pcd.tar.json.new
  with open(track_p) as f:
    info_track = json.load(f)
  ax = plt.subplot(M, N, 3)
  ax.imshow(im)
  ax.set_title('tracking: {}'.format(delay_track))
  df_track = show_lidar(info_track, cylinder_param, ax)

  align_p, delay_align = align_holder.get_nearest_file(t)
  print('align:', delay_align, align_p)
  # 0 /data1/output/L/align/PLCAP291_event_filter_diff_3d2d_vehicle_20201014-120907_0/fisheye_B_lidar/1602648543900669.pcd.tar.json
  with open(align_p) as f:
    info_align = json.load(f)
  ax = plt.subplot(M, N, 4)
  ax.imshow(im)
  ax.set_title('align: {}'.format(delay_align))
  df_align = show_lidar(info_align, cylinder_param, ax)