Unproject a single depth map to world coordinates.
(depth, K, pose, downsample=4, max_pts=500000)
| 1499 | |
| 1500 | |
| 1501 | def _unproject_frame(depth, K, pose, downsample=4, max_pts=500000): |
| 1502 | """Unproject a single depth map to world coordinates.""" |
| 1503 | H, W = depth.shape |
| 1504 | v_idx = np.arange(0, H, downsample) |
| 1505 | u_idx = np.arange(0, W, downsample) |
| 1506 | uu, vv = np.meshgrid(u_idx, v_idx) |
| 1507 | dd = depth[vv, uu] |
| 1508 | valid = (dd > 0.01) |
| 1509 | |
| 1510 | u_v = uu[valid].astype(np.float32) |
| 1511 | v_v = vv[valid].astype(np.float32) |
| 1512 | z = dd[valid] |
| 1513 | |
| 1514 | x = (u_v - K[0, 2]) / K[0, 0] * z |
| 1515 | y = (v_v - K[1, 2]) / K[1, 1] * z |
| 1516 | pts_cam = np.stack([x, y, z], axis=-1) |
| 1517 | |
| 1518 | R, t = pose[:3, :3], pose[:3, 3] |
| 1519 | pts_world = (pts_cam @ R.T + t).astype(np.float32) |
| 1520 | |
| 1521 | if len(pts_world) > max_pts: |
| 1522 | idx = np.random.choice(len(pts_world), max_pts, replace=False) |
| 1523 | pts_world = pts_world[idx] |
| 1524 | valid_indices = np.where(valid) |
| 1525 | sampled_v = valid_indices[0][idx] |
| 1526 | sampled_u = valid_indices[1][idx] |
| 1527 | return pts_world, sampled_v * downsample, sampled_u * downsample |
| 1528 | else: |
| 1529 | valid_indices = np.where(valid) |
| 1530 | return pts_world, valid_indices[0] * downsample, valid_indices[1] * downsample |
| 1531 | |
| 1532 | |
| 1533 | # --------------------------------------------------------------------------- |
no outgoing calls
no test coverage detected