-
Notifications
You must be signed in to change notification settings - Fork 7
/
lidar_utils.py
27 lines (23 loc) · 1.03 KB
/
lidar_utils.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
from camera_utils import draw_rect
import numpy as np
def project_point_cloud(image, point_cloud, intrinsic_mat, width, height, draw_each_nth=5):
""" Projects the lidar measurements onto the screen and draws them.
Since the points are just X,Y,Z coordinates relative to the lidar, this can be done by a simple projection.
Note that this assumes that the camera and the lidar have exactly the same position and rotation!!
"""
num_samples, dim = point_cloud.shape
assert dim == 3, "Point cloud should have shape (?, 3) (X, Y, Z)"
pos2d = np.dot(intrinsic_mat, point_cloud.T)
for j in range(0, num_samples, draw_each_nth):
cur_pos2d = pos2d[..., j]
cur_pos2d = np.array([
cur_pos2d[0] / cur_pos2d[2],
cur_pos2d[1] / cur_pos2d[2],
cur_pos2d[2]
])
depth = cur_pos2d[2]
if 1000 > depth > 0:
x_2d = width - cur_pos2d[0]
y_2d = height - cur_pos2d[1]
draw_rect(image, (y_2d, x_2d), int(20/depth))
return image