-
Notifications
You must be signed in to change notification settings - Fork 0
/
viz_util.py
111 lines (94 loc) · 3.58 KB
/
viz_util.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
import numpy as np
import mayavi.mlab as mlab
from PIL import Image, ImageDraw
import os
import cv2
import matplotlib.pyplot as plt
from objects import *
img_dir = '000002.png'
cal_dir = '000002c.txt'
velo_dir = '000002.bin'
label = '000002.txt'
def in_hull(p, hull):
from scipy.spatial import Delaunay
if not isinstance(hull,Delaunay):
hull = Delaunay(hull)
return hull.find_simplex(p)>=0
def extract_pc_in_box3d(pc, box3d):
''' pc: (N,3), box3d: (8,3) '''
box3d_roi_inds = in_hull(pc[:,0:3], box3d)
return pc[box3d_roi_inds,:], box3d_roi_inds
def project_to_image(pts_3d, P):
''' Project 3d points to image plane.
Usage: pts_2d = projectToImage(pts_3d, P)
input: pts_3d: nx3 matrix
P: 3x4 projection matrix
output: pts_2d: nx2 matrix
P(3x4) dot pts_3d_extended(4xn) = projected_pts_2d(3xn)
=> normalize projected_pts_2d(2xn)
<=> pts_3d_extended(nx4) dot P'(4x3) = projected_pts_2d(nx3)
=> normalize projected_pts_2d(nx2)
'''
n = pts_3d.shape[0]
pts_3d_extend = np.hstack((pts_3d, np.ones((n,1))))
#print(('pts_3d_extend shape: ', pts_3d_extend.shape))
pts_2d = np.dot(pts_3d_extend, np.transpose(P)) # nx3
pts_2d[:,0] /= pts_2d[:,2]
pts_2d[:,1] /= pts_2d[:,2]
return pts_2d[:,0:2]
def roty(t):
''' Rotation about the y-axis. '''
c = np.cos(t)
s = np.sin(t)
return np.array([[c, 0, s],
[0, 1, 0],
[-s, 0, c]])
def compute_box_3d(obj, P, multi=1):
''' Takes an object and a projection matrix (P) and projects the 3d
bounding box into the image plane.
Returns:
corners_2d: (8,2) array in left image coord.
corners_3d: (8,3) array in in rect camera coord.
'''
# compute rotational matrix around yaw axis
R = roty(obj.ry)
# 3d bounding box dimensions
l = obj.l;
w = obj.w;
h = obj.h;
# 3d bounding box corners
x_corners = [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2];
y_corners = [0, 0, 0, 0, -h, -h, -h, -h];
z_corners = [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2];
# rotate and translate 3d bounding box
corners_3d = np.dot(R, multi*np.vstack([x_corners, y_corners, z_corners]))
# print corners_3d.shape
corners_3d[0, :] = corners_3d[0, :] + obj.t[0];
corners_3d[1, :] = corners_3d[1, :] + obj.t[1];
corners_3d[2, :] = corners_3d[2, :] + obj.t[2];
# print 'cornsers_3d: ', corners_3d
# only draw 3d bounding box for objs in front of the camera
if np.any(corners_3d[2, :] < 0.1):
corners_2d = None
return corners_2d, np.transpose(corners_3d)
# project the 3d bounding box into the image plane
corners_2d = project_to_image(np.transpose(corners_3d), P)
# print 'corners_2d: ', corners_2d
return corners_2d, np.transpose(corners_3d)
def read_label(label_filename):
lines = [line.rstrip() for line in open(label_filename)]
objects = [Object3d(line) for line in lines]
return objects
if __name__=='__main__':
from objects import *
DATAPATH ="C:/Users/czy/Desktop/data/kitti/training/"
id = '%06d'%2
velo = np.fromfile(DATAPATH + 'velodyne/' +id+'.bin', dtype=np.float).reshape(-1,4)
img = Image.open(DATAPATH + 'image_2/' + id + '.png')
imgcv = cv2.imread(DATAPATH + 'image_2/' +id + '.png')
cal = Calibration(DATAPATH + 'calib/' +id + 'c.txt')
objs = read_label(DATAPATH + 'label_2/' +id + '.txt')
fig = mlab.figure()
fig = draw_lidar_fig(fig, velo, objs, cal)
mlab.show()
draw_3dbox_in_rgb(imgcv,cal,objs)