-
Notifications
You must be signed in to change notification settings - Fork 11
/
make-3d-testing.py
18 lines (16 loc) · 800 Bytes
/
make-3d-testing.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
def point_cloud(self, depth):
"""Transform a depth image into a point cloud with one point for each
pixel in the image, using the camera transform for a camera
centred at cx, cy with field of view fx, fy.
depth is a 2-D ndarray with shape (rows, cols) containing
depths from 1 to 254 inclusive. The result is a 3-D array with
shape (rows, cols, 3). Pixels with invalid depth in the input have
NaN for the z-coordinate in the result.
"""
rows, cols = depth.shape
c, r = np.meshgrid(np.arange(cols), np.arange(rows), sparse=True)
valid = (depth > 0) & (depth < 255)
z = np.where(valid, depth / 256.0, np.nan)
x = np.where(valid, z * (c - self.cx) / self.fx, 0)
y = np.where(valid, z * (r - self.cy) / self.fy, 0)
return np.dstack((x, y, z))