Skip to content

Instantly share code, notes, and snippets.

@nnop
Created October 31, 2020 03:41
Show Gist options
  • Save nnop/916a2542a345887f3d18051579ac9dfa to your computer and use it in GitHub Desktop.
Save nnop/916a2542a345887f3d18051579ac9dfa to your computer and use it in GitHub Desktop.
3d to cylinder projection
#!/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()
#!/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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment