forked from arsjindal/Lidar-Points-Representation
-
Notifications
You must be signed in to change notification settings - Fork 0
/
lidar.py
123 lines (93 loc) · 3.43 KB
/
lidar.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
112
113
114
115
116
117
118
119
120
121
122
123
# -*- coding: utf-8 -*-
"""
Created on Sun Mar 29 22:15:15 2020
@author: ravit
"""
import numpy as np
import pdb
import numpy as np
import matplotlib.pyplot as plt
proj_H = 64
proj_W = 2048
proj_fov_up = 3
proj_fov_down = -25.0
points = np.zeros((0, 3), dtype=np.float32) # [m, 3]: x, y, z
remissions = np.zeros((0, 1), dtype=np.float32) # [m ,1]: remission
proj_pgm = np.full((proj_H, proj_W, 5), -1,
dtype=np.float32)
proj_pgm1 = np.full((proj_H, proj_W, 5), -1,
dtype=np.float32)
# for each point, where it is in the range image
proj_x = np.zeros((0, 1), dtype=np.float32) # [m, 1]: x
proj_y = np.zeros((0, 1), dtype=np.float32) # [m, 1]: y
scan = np.fromfile("data/000000.bin",dtype= np.float32)
scan = scan.reshape((-1, 4))
points = scan[:, 0:3] # get xyz
remissions = scan[:, 3] # get remission
fov_up = proj_fov_up / 180.0 * np.pi # field of view up in rad
fov_down = proj_fov_down / 180.0 * np.pi # field of view down in rad
fov = abs(fov_down) + abs(fov_up) # get field of view total in rad
# get depth of all points
depth = np.linalg.norm(points, 2, axis=1)
# get scan components
scan_x = points[:, 0]
scan_y = points[:, 1]
scan_z = points[:, 2]
# get angles of all points
yaw = -np.arctan2(scan_y, scan_x)
pitch = np.arcsin(scan_z / depth)
# pdb.set_trace()
# get projections in image coords
proj_x = 0.5 * (yaw / np.pi + 1.0) # in [0.0, 1.0]
proj_y = 1.0 - (pitch + abs(fov_down)) / fov # in [0.0, 1.0]
'''
required_range = np.logical_and(yaw>=-np.pi/4, yaw <= np.pi/4)
proj_x = proj_x[required_range]
proj_y = proj_y[required_range]
points = points[required_range]
scan_x = scan_x[required_range]
scan_y = scan_y[required_range]
scan_z = scan_z[required_range]
depth = depth[required_range]
remissions = remissions[required_range]
'''
"""
# scale to image size using angular resolution
proj_x *= proj_W # in [0.0, W]
proj_y *= proj_H # in [0.0, H]
# round and clamp for use as index
proj_x = np.floor(proj_x)
proj_x = np.minimum(proj_W - 1, proj_x)
proj_x = np.maximum(0, proj_x).astype(np.int32) # in [0,W-1]
proj_x_copy = np.copy(proj_x) # store a copy in orig order
proj_y = np.floor(proj_y)
proj_y = np.minimum(proj_H - 1, proj_y)
proj_y = np.maximum(0, proj_y).astype(np.int32) # in [0,H-1]
proj_y_copy = np.copy(proj_y) # stope a copy in original order"""
# scale to image size using angular resolution
proj_x *= proj_W # in [0.0, W]
proj_y *= proj_H # in [0.0, H]
# round and clamp for use as index
proj_x = np.floor(proj_x)
proj_x = np.minimum(proj_W - 1, proj_x)
proj_x = np.maximum(0, proj_x).astype(np.int32) # in [0,W-1]
proj_x_copy = np.copy(proj_x) # store a copy in orig order
proj_y = np.floor(proj_y)
proj_y = np.minimum(proj_H - 1, proj_y)
proj_y = np.maximum(0, proj_y).astype(np.int32) # in [0,H-1]
proj_y_copy = np.copy(proj_y) # stope a copy in original order
"""
# assing to images
#polar_grid_map = np.concatenate((points,remissions,depth),axis = 2)
proj_pgm[proj_y,proj_x,0:3] = points
proj_pgm[proj_y,proj_x,3] = remissions
proj_pgm[proj_y,proj_x,4] = depth"""
proj_pgm[proj_y,proj_x,0:3] = points
proj_pgm[proj_y,proj_x,3] = remissions
proj_pgm[proj_y,proj_x,4] = depth
#proj_pgm = proj_pgm[:,767:1279,:]
plt.imshow(proj_pgm[:,768:1281,4])
plt.show()
"""
plt.imshow(proj_pgm1[:,:,4])
plt.show()"""