Generate 3D point coordinates and related rgb feature Args: rgb_image: (h, w, 3) rgb depth_image: (h, w) depth, along z direction (not along individual camera ray) intrinsic: (3, 3) subsample: int resize stride world_coordinate: bool
(
rgb_image: np.ndarray,
depth_image: np.ndarray,
intrinsic: np.ndarray,
subsample: int = 1,
world_coordinate: bool = True,
pose: np.ndarray = None,
)
| 373 | |
| 374 | |
| 375 | def generate_point( |
| 376 | rgb_image: np.ndarray, |
| 377 | depth_image: np.ndarray, |
| 378 | intrinsic: np.ndarray, |
| 379 | subsample: int = 1, |
| 380 | world_coordinate: bool = True, |
| 381 | pose: np.ndarray = None, |
| 382 | ): |
| 383 | """ |
| 384 | Generate 3D point coordinates and related rgb feature |
| 385 | |
| 386 | Args: |
| 387 | rgb_image: (h, w, 3) rgb |
| 388 | depth_image: (h, w) depth, along z direction (not along individual camera ray) |
| 389 | intrinsic: (3, 3) |
| 390 | subsample: int |
| 391 | resize stride |
| 392 | world_coordinate: bool |
| 393 | pose: (4, 4) matrix |
| 394 | transfer from camera to world coordindate |
| 395 | |
| 396 | Returns: |
| 397 | points: (N, 3) point cloud coordinates |
| 398 | in world-coordinates if world_coordinate==True |
| 399 | else in camera coordinates |
| 400 | rgb_feat: (N, 3) rgb feature of each point |
| 401 | |
| 402 | Important note: |
| 403 | The function uses the image coordinate system: x to right, y to "down", z to far. |
| 404 | If the world coordinate is a different one (say x to right, y to "up", z to us), |
| 405 | H_c2w need to include the coordinate conversion. |
| 406 | """ |
| 407 | intrinsic_4x4 = np.identity(4) |
| 408 | intrinsic_4x4[:3, :3] = intrinsic |
| 409 | |
| 410 | u, v = np.meshgrid( |
| 411 | range(0, depth_image.shape[1], subsample), |
| 412 | range(0, depth_image.shape[0], subsample), |
| 413 | ) |
| 414 | # u: (depth_image.shape[0]//subsample, depth_image.shape[1]//subsample), x |
| 415 | # v: (depth_image.shape[0]//subsample, depth_image.shape[1]//subsample), y |
| 416 | d = depth_image[v, u] |
| 417 | d_filter = d != 0 |
| 418 | mat = np.vstack( |
| 419 | ( |
| 420 | u[d_filter] * d[d_filter], |
| 421 | v[d_filter] * d[d_filter], |
| 422 | d[d_filter], |
| 423 | np.ones_like(u[d_filter]), |
| 424 | ) |
| 425 | ) |
| 426 | new_points_3d = np.dot(np.linalg.inv(intrinsic_4x4), mat)[:3] |
| 427 | if world_coordinate: |
| 428 | new_points_3d_padding = np.vstack( |
| 429 | (new_points_3d, np.ones((1, new_points_3d.shape[1]))) |
| 430 | ) |
| 431 | world_coord_padding = np.dot(pose, new_points_3d_padding) |
| 432 | new_points_3d = world_coord_padding[:3] |