Suggestion - add color to the depth map

[tomer-grin]

Hey,
you can add RGB to the depth map using the next lines:

# Get intrinsic parameters
height, width, _ = rgb.shape
K = intrinsic_from_fov(height, width, 90)  # +- 45 degrees
K_inv = np.linalg.inv(K)

# Get pixel coordinates
pixel_coords = pixel_coord_np(width, height)  # [3, npoints]

# Apply back-projection: K_inv @ pixels * depth
cam_coords = K_inv[:3, :3] @ pixel_coords * depth.flatten()
colors = rgb.reshape(-1, rgb.shape[-1])
# back-projection using native for-loop.
# Uncomment block to test this
# cam_coords = np.zeros((height * width, 3))
# u0 = K[0, 2]
# v0 = K[1, 2]
# fx = K[0, 0]
# fy = K[1, 1]
# i = 0
# # Loop through each pixel in the image
# for v in range(height):
#     for u in range(width):
#         # Apply equation in fig 3
#         x = (u - u0) * depth[v, u] / fx
#         y = (v - v0) * depth[v, u] / fy
#         z = depth[v, u]
#         cam_coords[i] = (x, y, z)
#         i += 1
# cam_coords = cam_coords.T


# Limit points to 150m in the z-direction for visualisation
colors = colors[np.where(cam_coords[2] <= 150)[0]]
cam_coords = cam_coords[:, np.where(cam_coords[2] <= 150)[0]]
# Visualize
pcd_cam = o3d.geometry.PointCloud()
pcd_cam.points = o3d.utility.Vector3dVector(cam_coords.T[:, :3])
pcd_cam.colors = o3d.utility.Vector3dVector(colors / 255.)

[tomer-grin]

Added as PR
#20