├── images └── demo.png ├── data └── KITTI │ └── training │ ├── image_2 │ └── 000500.png │ ├── velodyne │ └── 000500.bin │ ├── label_2 │ └── 000500.txt │ └── calib │ └── 000500.txt ├── open3d_geometry ├── __pycache__ │ ├── open3d_box.cpython-36.pyc │ ├── open3d_arrow.cpython-36.pyc │ ├── open3d_utils.cpython-36.pyc │ └── open3d_coordinate.cpython-36.pyc ├── open3d_coordinate.py ├── open3d_box.py ├── open3d_utils.py └── open3d_arrow.py ├── README.md ├── visualize.py ├── PointCloudVis.py └── kitti_utils.py /images/demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chaomath/open3d-kitti-visualization/HEAD/images/demo.png -------------------------------------------------------------------------------- /data/KITTI/training/image_2/000500.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chaomath/open3d-kitti-visualization/HEAD/data/KITTI/training/image_2/000500.png -------------------------------------------------------------------------------- /data/KITTI/training/velodyne/000500.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chaomath/open3d-kitti-visualization/HEAD/data/KITTI/training/velodyne/000500.bin -------------------------------------------------------------------------------- /open3d_geometry/__pycache__/open3d_box.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chaomath/open3d-kitti-visualization/HEAD/open3d_geometry/__pycache__/open3d_box.cpython-36.pyc -------------------------------------------------------------------------------- /open3d_geometry/__pycache__/open3d_arrow.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chaomath/open3d-kitti-visualization/HEAD/open3d_geometry/__pycache__/open3d_arrow.cpython-36.pyc -------------------------------------------------------------------------------- /open3d_geometry/__pycache__/open3d_utils.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chaomath/open3d-kitti-visualization/HEAD/open3d_geometry/__pycache__/open3d_utils.cpython-36.pyc -------------------------------------------------------------------------------- /open3d_geometry/__pycache__/open3d_coordinate.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chaomath/open3d-kitti-visualization/HEAD/open3d_geometry/__pycache__/open3d_coordinate.cpython-36.pyc -------------------------------------------------------------------------------- /open3d_geometry/open3d_coordinate.py: -------------------------------------------------------------------------------- 1 | import open3d as o3d 2 | 3 | def create_coordinate(size=2.0, origin=[0, 0, 0]): 4 | mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=2.0, origin=[0, 0, 0]) 5 | return mesh_frame -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # open3d kitti visualization 2 | 3 | kitti training data visiualization with [open3d](http://www.open3d.org/) 4 | 5 | ## Dependencies 6 | 7 | - python >=3.0 8 | - numpy 9 | - open3d 10 | 11 | 12 | ## Visualization 13 |

14 | 15 | 16 | ## Dataset preparation 17 | Download KITTI dataset and create some directories first: 18 | 19 | ```plain 20 | └── KITTI_DATASET_ROOT 21 | ├── training <-- 7481 train data 22 | | ├── image_2 <-- for visualization 23 | | ├── calib 24 | | ├── label_2 25 | | ├── velodyne 26 | | └── velodyne_reduced <-- empty directory 27 | └── testing <-- 7580 test data 28 | ├── image_2 <-- for visualization 29 | ├── calib 30 | ├── velodyne 31 | └── velodyne_reduced <-- empty directory 32 | ``` 33 | 34 | 35 | ## Demo 36 | ``` 37 | $ python visiualize.py 38 | ``` 39 | 40 | If you has your own data path, you can run like this: 41 | ``` 42 | $ python visiualize.py --data=KITTI_DATASET_ROOT --frame=500 43 | ``` 44 | -------------------------------------------------------------------------------- /data/KITTI/training/label_2/000500.txt: -------------------------------------------------------------------------------- 1 | Car 0.00 0 -1.59 594.51 174.51 638.61 216.22 1.56 1.65 3.69 0.19 1.65 29.11 -1.58 2 | Car 0.87 0 -1.54 1067.59 187.01 1241.00 374.00 1.40 1.65 4.18 4.27 1.52 4.56 -0.82 3 | Car 0.00 0 -1.29 838.52 163.91 1111.22 356.67 1.59 1.65 3.94 4.15 1.53 7.96 -0.82 4 | Car 0.00 2 -1.11 763.86 173.53 985.22 284.59 1.48 1.73 4.06 4.35 1.50 11.65 -0.76 5 | Car 0.00 2 -0.99 728.04 166.95 903.14 255.35 1.63 1.61 3.96 4.36 1.53 15.17 -0.71 6 | Car 0.00 2 -0.96 721.62 175.30 850.40 238.46 1.49 1.63 3.36 4.61 1.57 18.89 -0.72 7 | Car 0.00 2 -0.90 696.96 174.28 821.96 224.09 1.41 1.60 4.09 4.68 1.47 22.46 -0.70 8 | Car 0.00 1 1.67 525.35 177.56 546.43 192.20 1.37 1.64 3.92 -7.22 1.85 70.45 1.57 9 | Car 0.00 0 1.76 461.34 181.95 503.86 210.95 1.38 1.47 3.62 -6.47 1.86 36.77 1.58 10 | Car 0.00 1 -0.83 667.67 172.26 754.59 206.74 1.49 1.59 4.09 4.65 1.48 33.13 -0.69 11 | Car 0.00 2 -0.89 650.91 173.55 720.64 200.23 1.40 1.63 4.05 4.20 1.45 39.83 -0.79 12 | Van 0.00 2 -0.92 639.38 168.48 696.03 194.49 1.88 1.82 4.64 4.39 1.58 54.51 -0.84 13 | Car 0.00 2 1.77 462.38 175.94 493.50 196.90 1.67 1.68 4.49 -11.00 1.95 60.26 1.59 14 | Car 0.00 1 1.78 440.29 176.19 460.63 192.15 1.47 1.29 2.95 -15.10 1.80 68.33 1.56 15 | DontCare -1 -1 -10 575.81 167.55 587.57 181.50 -1 -1 -1 -1000 -1000 -1000 -10 16 | DontCare -1 -1 -10 550.86 174.97 569.97 182.16 -1 -1 -1 -1000 -1000 -1000 -10 17 | -------------------------------------------------------------------------------- /data/KITTI/training/calib/000500.txt: -------------------------------------------------------------------------------- 1 | P0: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 0.000000000000e+00 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 2 | P1: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.875744000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 3 | P2: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 4.485728000000e+01 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.163791000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.745884000000e-03 4 | P3: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.395242000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.199936000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.729905000000e-03 5 | R0_rect: 9.999239000000e-01 9.837760000000e-03 -7.445048000000e-03 -9.869795000000e-03 9.999421000000e-01 -4.278459000000e-03 7.402527000000e-03 4.351614000000e-03 9.999631000000e-01 6 | Tr_velo_to_cam: 7.533745000000e-03 -9.999714000000e-01 -6.166020000000e-04 -4.069766000000e-03 1.480249000000e-02 7.280733000000e-04 -9.998902000000e-01 -7.631618000000e-02 9.998621000000e-01 7.523790000000e-03 1.480755000000e-02 -2.717806000000e-01 7 | Tr_imu_to_velo: 9.999976000000e-01 7.553071000000e-04 -2.035826000000e-03 -8.086759000000e-01 -7.854027000000e-04 9.998898000000e-01 -1.482298000000e-02 3.195559000000e-01 2.024406000000e-03 1.482454000000e-02 9.998881000000e-01 -7.997231000000e-01 8 | 9 | -------------------------------------------------------------------------------- /open3d_geometry/open3d_box.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | 4 | from .open3d_arrow import create_arrow # the dot '.' is essential 5 | from .open3d_utils import box_dim2corners, get_box_arrow 6 | 7 | 8 | def create_box_from_corners(corners, color=None): 9 | ''' 10 | corners: 8 corners(x, y, z) 11 | corners: array = 8*3 12 | # 7 -------- 6 13 | # /| /| 14 | # 4 -------- 5 . 15 | # | | | | 16 | # . 3 -------- 2 17 | # |/ |/ 18 | # 0 -------- 1 19 | ''' 20 | # 12 lines in a box 21 | lines = [[0, 1], [1, 2], [2, 3], [3, 0], 22 | [4, 5], [5, 6], [6, 7], [7, 4], 23 | [0, 4], [1, 5], [2, 6], [3, 7]] 24 | if color == None: 25 | color = [1, 0, 0] # red 26 | colors = [color for i in range(len(lines))] 27 | line_set = o3d.geometry.LineSet() 28 | line_set.points = o3d.utility.Vector3dVector(corners) 29 | line_set.lines = o3d.utility.Vector2iVector(lines) 30 | line_set.colors = o3d.utility.Vector3dVector(colors) 31 | 32 | return line_set 33 | 34 | 35 | def create_box_from_dim(dim, color=None): 36 | ''' 37 | dim: list(8) [h, w, l, x, y, z, yaw] 38 | ''' 39 | box_corners = box_dim2corners(dim) 40 | box = create_box_from_corners(box_corners, color) 41 | return box 42 | 43 | 44 | def create_box_from_dim_with_arrow(dim, color=None): 45 | ''' 46 | dim: list(8) [h, w, l, x, y, z, yaw] 47 | ''' 48 | 49 | box = create_box_from_dim(dim, color) 50 | 51 | vec = get_box_arrow(dim) 52 | a_start = [vec[0], vec[1], vec[2]] 53 | a_end = [vec[3], vec[4], vec[5]] 54 | arrow = create_arrow(a_start, a_end, color) 55 | 56 | return box, arrow 57 | 58 | -------------------------------------------------------------------------------- /visualize.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import os 3 | import numpy as np 4 | 5 | from kitti_utils import load_kitti_calib, read_objs2velo, colors_list 6 | from PointCloudVis import PointCloudVis 7 | 8 | 9 | def vis_kitti(data_path, frame): 10 | 11 | # get data 12 | lidar_path = os.path.join(data_path, 'velodyne') 13 | label_path = os.path.join(data_path, 'label_2') 14 | calib_path = os.path.join(data_path, 'calib') 15 | 16 | lidar_file = os.path.join(lidar_path, '%06d.bin'%frame) 17 | label_file = os.path.join(label_path, '%06d.txt'%frame) 18 | calib_file = os.path.join(calib_path, '%06d.txt'%frame) 19 | 20 | # get calibration 21 | calib = load_kitti_calib(calib_file) 22 | 23 | # get points and boxes 24 | boxes_velo, objs_type = read_objs2velo(label_file, calib['Tr_velo2cam']) 25 | points = np.fromfile(lidar_file, dtype=np.float32).reshape(-1, 4) 26 | 27 | #-------------------------------------------------------------- 28 | # get colors for each box 29 | #-------------------------------------------------------------- 30 | box_colors = [] 31 | if len(objs_type) == 0: 32 | box_colors = [[1, 0, 0] for i in range(boxes_velo.shape[0])] # red 33 | else: 34 | box_colors = [colors_list[int(i)] for i in objs_type] 35 | 36 | # draw boxes with arrows 37 | PointCloudVis.draw_points_with_boxes(points, boxes_velo, box_colors) 38 | 39 | 40 | 41 | if __name__ == "__main__": 42 | 43 | parser = argparse.ArgumentParser() 44 | parser.add_argument("--data", "--d", type=str, default="./data/KITTI/training", help="kitti data path") 45 | parser.add_argument("--frame", "--f", type=int, default=500, help="frame of the data") 46 | opt = parser.parse_args() 47 | 48 | data_path = opt.data 49 | frame = opt.frame 50 | 51 | vis_kitti(data_path, frame) 52 | -------------------------------------------------------------------------------- /open3d_geometry/open3d_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def rotz(t): 5 | ''' Rotation about the z-axis. ''' 6 | c = np.cos(t) 7 | s = np.sin(t) 8 | return np.array([[c, -s, 0], 9 | [s, c, 0], 10 | [0, 0, 1]]) 11 | 12 | 13 | def get_box_arrow(dim): 14 | h = dim[0] 15 | # w = dim[1] 16 | l = dim[2] 17 | x = dim[3] 18 | y = dim[4] 19 | z = dim[5] 20 | yaw = dim[6] 21 | 22 | # get direction arrow 23 | dx = l/2.0*np.cos(yaw) 24 | dy = l/2.0*np.sin(yaw) 25 | # a_start = [x, y, z+h] 26 | # a_end = [x+dx, y+dy, z+h] 27 | # arrow = [a_start, a_end] 28 | arrow = [x, y, z+h, x+dx, y+dy, z+h] # [x0, y0, z0, x1, y1, z1], point0--->point1 29 | return arrow 30 | 31 | 32 | def box_dim2corners(dim): 33 | ''' 34 | dim: [h, w, l, x, y, z, yaw] 35 | 36 | 8 corners: np.array = n*8*3(x, y, z) 37 | # 7 -------- 6 38 | # /| /| 39 | # 4 -------- 5 . 40 | # | | | | 41 | # . 3 -------- 2 42 | # |/ |/ 43 | # 0 -------- 1 44 | 45 | ^ x(l) 46 | | 47 | | 48 | | 49 | y(w) | 50 | <-----------O 51 | ''' 52 | h = dim[0] 53 | w = dim[1] 54 | l = dim[2] 55 | x = dim[3] 56 | y = dim[4] 57 | z = dim[5] 58 | yaw = dim[6] 59 | 60 | # 3d bounding box corners 61 | Box = np.array([[-l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2], 62 | [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2], 63 | [0, 0, 0, 0, h, h, h, h]]) 64 | 65 | R = rotz(yaw) 66 | corners_3d = np.dot(R, Box) # corners_3d: (3, 8) 67 | 68 | corners_3d[0,:] = corners_3d[0,:] + x 69 | corners_3d[1,:] = corners_3d[1,:] + y 70 | corners_3d[2,:] = corners_3d[2,:] + z 71 | 72 | return np.transpose(corners_3d) 73 | -------------------------------------------------------------------------------- /open3d_geometry/open3d_arrow.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import open3d as o3d 4 | 5 | 6 | def vector_magnitude(vec): 7 | """ 8 | Calculates a vector's magnitude. 9 | Args: 10 | - vec (): 11 | """ 12 | magnitude = np.sqrt(np.sum(vec**2)) 13 | return(magnitude) 14 | 15 | 16 | def calculate_zy_rotation_for_arrow(vec): 17 | """ 18 | Calculates the rotations required to go from the vector vec to the 19 | z axis vector of the original FOR. The first rotation that is 20 | calculated is over the z axis. This will leave the vector vec on the 21 | XZ plane. Then, the rotation over the y axis. 22 | 23 | Returns the angles of rotation over axis z and y required to 24 | get the vector vec into the same orientation as axis z 25 | of the original FOR 26 | 27 | Args: 28 | - vec (): 29 | """ 30 | # Rotation over z axis of the FOR 31 | gamma = np.arctan(vec[1]/vec[0]) 32 | Rz = np.array([[np.cos(gamma),-np.sin(gamma),0], 33 | [np.sin(gamma),np.cos(gamma),0], 34 | [0,0,1]]) 35 | # Rotate vec to calculate next rotation 36 | vec = Rz.T@vec.reshape(-1,1) 37 | vec = vec.reshape(-1) 38 | # Rotation over y axis of the FOR 39 | beta = np.arctan2(vec[0], vec[2]) 40 | Ry = np.array([[np.cos(beta),0,np.sin(beta)], 41 | [0,1,0], 42 | [-np.sin(beta),0,np.cos(beta)]]) 43 | return(gamma,beta) 44 | 45 | 46 | def get_arrow(scale=10): 47 | """ 48 | get an arrow in for Open3D 49 | """ 50 | cone_height = scale*0.2 51 | cylinder_height = scale*0.8 52 | cone_radius = scale/10 53 | cylinder_radius = scale/20 54 | mesh_frame = o3d.geometry.TriangleMesh.create_arrow(cone_radius=0.4, 55 | cone_height=cone_height, 56 | cylinder_radius=0.1, 57 | cylinder_height=cylinder_height) 58 | return(mesh_frame) 59 | 60 | 61 | def create_arrow(origin=[0,0,0],end=None,color = None, vec=None): 62 | """ 63 | Creates an arrow from an origin point to an end point, 64 | or create an arrow from a vector vec starting from origin. 65 | Args: 66 | - end (): End point. [x,y,z] 67 | - vec (): Vector. [i,j,k] 68 | """ 69 | scale = 10; beta = 0; gamma = 0 70 | T = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) 71 | T[:3,-1] = origin 72 | if end is not None: 73 | vec = np.array(end) - np.array(origin) 74 | elif vec is not None: 75 | vec = np.array(vec) 76 | if end is not None or vec is not None: 77 | scale = vector_magnitude(vec) 78 | gamma,beta = calculate_zy_rotation_for_arrow(vec) 79 | mesh = get_arrow(scale) 80 | # mesh.transform(T) 81 | mesh.rotate([0,beta,0],center=False) 82 | mesh.rotate([0,0,gamma],center=False) 83 | mesh.translate(origin) 84 | 85 | # add color 86 | if color == None: 87 | mesh.paint_uniform_color([1.0, 0, 0]) 88 | else: 89 | mesh.paint_uniform_color(color) 90 | return(mesh) 91 | -------------------------------------------------------------------------------- /PointCloudVis.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | 4 | from open3d_geometry.open3d_box import create_box_from_dim_with_arrow 5 | from open3d_geometry.open3d_coordinate import create_coordinate 6 | 7 | class PointCloudVis: 8 | 9 | @staticmethod 10 | def draw_points(points): 11 | ''' 12 | points: numpy array-->(n*4) or (n*3)-->(x, y, z, intensity) 13 | ''' 14 | points = points[:, :3] 15 | 16 | colors = [[0.5, 0.5, 0.5] for i in range(points.shape[0])] 17 | pc = o3d.geometry.PointCloud() 18 | 19 | pc.points = o3d.utility.Vector3dVector(points) 20 | pc.colors = o3d.utility.Vector3dVector(colors) 21 | 22 | vis = o3d.visualization.Visualizer() 23 | vis.create_window() 24 | vis.add_geometry(pc) 25 | vis.get_render_option().point_size = 2 26 | vis.run() 27 | vis.destroy_window() 28 | 29 | @staticmethod 30 | def draw_pcd(pcd): 31 | vis = o3d.visualization.Visualizer() 32 | vis.create_window() 33 | vis.add_geometry(pcd) 34 | vis.get_render_option().point_size = 2 35 | vis.run() 36 | vis.destroy_window() 37 | 38 | 39 | @staticmethod 40 | def draw_points_with_boxes(points, boxes, box_colors=[]): 41 | ''' 42 | points: (m, 4) or (m, 3) [x, y, z, intensity] 43 | boxes: np.array = n*7 (h, w, l, x, y, z, yaw) 44 | box_colors: list(n) color of each box 45 | ''' 46 | 47 | #-------------------------------------------------------------- 48 | # get colors for each box 49 | #-------------------------------------------------------------- 50 | if len(box_colors) == 0: 51 | box_colors = [[1, 0, 0] for i in range(boxes.shape[0])] # red 52 | 53 | #-------------------------------------------------------------- 54 | # create boxes 55 | #-------------------------------------------------------------- 56 | boxes_o3d = [] 57 | for i in range(boxes.shape[0]): 58 | dim = boxes[i] 59 | color = box_colors[i] 60 | box_o3d, arrow = create_box_from_dim_with_arrow(dim, color) 61 | boxes_o3d.append(box_o3d) 62 | boxes_o3d.append(arrow) 63 | 64 | #-------------------------------------------------------------- 65 | # coordinate frame 66 | #-------------------------------------------------------------- 67 | coordinate_frame = create_coordinate(size=2.0, origin=[0, 0, 0]) 68 | 69 | #-------------------------------------------------------------- 70 | # point cloud 71 | #-------------------------------------------------------------- 72 | points = points[:, :3] 73 | point_color = [0.5, 0.5, 0.5] 74 | point_colors = [point_color for i in range(points.shape[0])] # points color 75 | pc = o3d.geometry.PointCloud() 76 | pc.points = o3d.utility.Vector3dVector(points) 77 | pc.colors = o3d.utility.Vector3dVector(point_colors) 78 | 79 | #-------------------------------------------------------------- 80 | # draw geometry in open3d 81 | #-------------------------------------------------------------- 82 | vis = o3d.visualization.Visualizer() 83 | vis.create_window() 84 | 85 | vis.add_geometry(coordinate_frame) 86 | vis.add_geometry(pc) 87 | 88 | [vis.add_geometry(element) for element in boxes_o3d] 89 | 90 | vis.get_render_option().point_size = 2 91 | vis.run() 92 | vis.destroy_window() 93 | -------------------------------------------------------------------------------- /kitti_utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | 4 | # classes 5 | class_list = ['Car', 'Van' , 'Truck' , 'Pedestrian' , 'Person_sitting' , 'Cyclist' , 'Tram', 'Misc'] 6 | colors_list = [[1, 0, 0], # car------------------>red 7 | [0.5, 1, 0], # Van 8 | [0, 1, 0.5], # Truck 9 | [0, 1, 0], # Pedestrian----------->green 10 | [0.2, 0.5, 0], # Person_sitting 11 | [0, 0, 1], # Cyclist-------------->blue 12 | [0.7, 0.7, 0.3], # Tram 13 | [0.2, 0.5, 0.7]] # Misc 14 | 15 | 16 | def load_kitti_calib(calib_file): 17 | """ 18 | load projection matrix 19 | """ 20 | with open(calib_file) as fi: 21 | lines = fi.readlines() 22 | assert (len(lines) == 8) 23 | 24 | obj = lines[0].strip().split(' ')[1:] 25 | P0 = np.array(obj, dtype=np.float32) 26 | obj = lines[1].strip().split(' ')[1:] 27 | P1 = np.array(obj, dtype=np.float32) 28 | obj = lines[2].strip().split(' ')[1:] 29 | P2 = np.array(obj, dtype=np.float32) 30 | obj = lines[3].strip().split(' ')[1:] 31 | P3 = np.array(obj, dtype=np.float32) 32 | obj = lines[4].strip().split(' ')[1:] 33 | R0 = np.array(obj, dtype=np.float32) 34 | obj = lines[5].strip().split(' ')[1:] 35 | Tr_velo_to_cam = np.array(obj, dtype=np.float32) 36 | obj = lines[6].strip().split(' ')[1:] 37 | Tr_imu_to_velo = np.array(obj, dtype=np.float32) 38 | 39 | return {'P2': P2.reshape(3, 4), 40 | 'R0': R0.reshape(3, 3), 41 | 'Tr_velo2cam': Tr_velo_to_cam.reshape(3, 4)} 42 | 43 | 44 | def project_cam2velo(cam, Tr): 45 | T = np.zeros([4, 4], dtype=np.float32) 46 | T[:3, :] = Tr 47 | T[3, 3] = 1 48 | T_inv = np.linalg.inv(T) 49 | lidar_loc_ = np.dot(T_inv, cam) 50 | lidar_loc = lidar_loc_[:3] 51 | return lidar_loc.reshape(1, 3) 52 | 53 | def ry_to_rz(ry): 54 | angle = -ry - np.pi / 2 55 | 56 | if angle >= np.pi: 57 | angle -= np.pi 58 | if angle < -np.pi: 59 | angle = 2*np.pi + angle 60 | 61 | return angle 62 | 63 | 64 | 65 | class KittiObject(object): 66 | ''' kitti 3d object label ''' 67 | def __init__(self, label_file_line): 68 | data = label_file_line.split(' ') 69 | data[1:] = [float(x) for x in data[1:]] 70 | 71 | # extract label, truncation, occlusion 72 | self.type = data[0] # 'Car', 'Pedestrian', ... 73 | self.truncation = data[1] # truncated pixel ratio [0..1] 74 | self.occlusion = int(data[2]) # 0=visible, 1=partly occluded, 2=fully occluded, 3=unknown 75 | self.alpha = data[3] # object observation angle [-pi..pi] 76 | 77 | # extract 2d bounding box in 0-based coordinates 78 | self.xmin = data[4] # left 79 | self.ymin = data[5] # top 80 | self.xmax = data[6] # right 81 | self.ymax = data[7] # bottom 82 | self.box2d = np.array([self.xmin,self.ymin,self.xmax,self.ymax]) 83 | 84 | # extract 3d bounding box information 85 | self.h = data[8] # box height 86 | self.w = data[9] # box width 87 | self.l = data[10] # box length (in meters) 88 | self.t = (data[11],data[12],data[13]) # location (x,y,z) in camera coord. 89 | self.ry = data[14] # yaw angle (around Y-axis in camera coordinates) [-pi..pi] 90 | 91 | def __str__(self): 92 | str0 = ('Type, truncation, occlusion, alpha: %s, %d, %d, %f\n' % \ 93 | (self.type, self.truncation, self.occlusion, self.alpha)) 94 | str1 = ('2d bbox (x0,y0,x1,y1): %f, %f, %f, %f\n' % \ 95 | (self.xmin, self.ymin, self.xmax, self.ymax)) 96 | str2 = ('3d bbox h,w,l: %f, %f, %f\n' % \ 97 | (self.h, self.w, self.l)) 98 | str3 = ('3d bbox location, ry: (%f, %f, %f), %f\n' % \ 99 | (self.t[0],self.t[1],self.t[2],self.ry)) 100 | 101 | return (str0 + str1 + str2 + str3) 102 | 103 | 104 | def get_obj_type(obj_str): 105 | obj_type = -1 106 | for i in range(len(class_list)): 107 | if obj_str == class_list[i]: 108 | obj_type = i 109 | return obj_type 110 | 111 | 112 | def read_objs2velo(label_file, Tr_velo2cam): 113 | ''' 114 | Tr_velo2cam: (3, 4) 115 | ''' 116 | 117 | lines = [line.rstrip() for line in open(label_file)] 118 | objs_velo = [] 119 | objs_type = [] 120 | for line in lines: 121 | obj = KittiObject(line) 122 | if obj.type == 'DontCare': 123 | continue 124 | obj_type = get_obj_type(obj.type) 125 | h = obj.h 126 | w = obj.w 127 | l = obj.l 128 | x = obj.t[0] 129 | y = obj.t[1] 130 | z = obj.t[2] 131 | ry = obj.ry 132 | 133 | rz = ry_to_rz(ry) # ry in camera, rz in velo 134 | 135 | pos_cam = np.ones([4, 1]) 136 | pos_cam[0] = x 137 | pos_cam[1] = y 138 | pos_cam[2] = z 139 | pos_velo = project_cam2velo(pos_cam, Tr_velo2cam) # pos_velo: (1,3) 140 | x_velo = pos_velo[0][0] 141 | y_velo = pos_velo[0][1] 142 | z_velo = pos_velo[0][2] 143 | obj_velo = [h, w, l, x_velo, y_velo, z_velo, rz, obj_type] 144 | objs_type.append(obj_type) 145 | 146 | objs_velo.append(obj_velo) 147 | objs_velo = np.array(objs_velo) #(n, 8) 148 | 149 | return objs_velo, objs_type 150 | --------------------------------------------------------------------------------