| 7 | |
| 8 | |
| 9 | def from_depth_to_point(rgb, depth, mask, intrinsic, depth_intrinsic, |
| 10 | extrinsic): |
| 11 | h2, w2, _ = rgb.shape |
| 12 | h, w = depth.shape |
| 13 | depth_intrinsic = np.linalg.inv(depth_intrinsic) |
| 14 | y, x = np.meshgrid(np.arange(h), np.arange(w), indexing='ij') |
| 15 | x = x.reshape((1, -1))[:, :] |
| 16 | y = y.reshape((1, -1))[:, :] |
| 17 | z = depth.reshape((1, -1))[:, :] |
| 18 | rgb_resized = cv2.resize(rgb, (w, h)) |
| 19 | color = rgb_resized.reshape(-1, 3) / 255.0 |
| 20 | p_2d = np.vstack([x, y, np.ones_like(x)]) |
| 21 | pc = depth_intrinsic[:3, :3] @ p_2d |
| 22 | pc = pc * z |
| 23 | pc = np.concatenate([pc, np.ones_like(x)], axis=0) |
| 24 | pc = extrinsic @ pc |
| 25 | z_mask = pc[2, :] < 1.8 |
| 26 | mask = np.logical_and(mask, z_mask) |
| 27 | pc = pc.transpose() |
| 28 | pc = pc[mask] |
| 29 | color = color[mask] |
| 30 | return pc[:, :3], color |
| 31 | |
| 32 | |
| 33 | def _box_add_thickness(box, thickness): |