Compute the xyz in the world coordinate using z_map and camera pose. Important note: The function uses a image coordinate system: x to right, y to "down", z to far. If the world coordinate is a different one (say x to right, y to "up", z to us), H_c2w need to includ
(
z_map: torch.Tensor,
intrinsic: torch.Tensor,
H_c2w: torch.Tensor,
subsample: int = 1,
other_maps: T.List[torch.Tensor] = None,
)
| 1034 | |
| 1035 | |
| 1036 | def compute_3d_xyz( |
| 1037 | z_map: torch.Tensor, |
| 1038 | intrinsic: torch.Tensor, |
| 1039 | H_c2w: torch.Tensor, |
| 1040 | subsample: int = 1, |
| 1041 | other_maps: T.List[torch.Tensor] = None, |
| 1042 | ): |
| 1043 | """ |
| 1044 | Compute the xyz in the world coordinate using z_map and camera pose. |
| 1045 | |
| 1046 | Important note: |
| 1047 | The function uses a image coordinate system: x to right, y to "down", z to far. |
| 1048 | If the world coordinate is a different one (say x to right, y to "up", z to us), |
| 1049 | H_c2w need to include the image coordinate to world (ie. flip y and z), |
| 1050 | ex: H_actual * H_i2l |
| 1051 | |
| 1052 | Args: |
| 1053 | z_map: |
| 1054 | (*, h, w) the z coordinate of the point in the camera coordinate on the sensor, |
| 1055 | not along the corresponding camera ray. |
| 1056 | intrinsic: |
| 1057 | (*, 3, 3) camera intrinsic matrix |
| 1058 | H_c2w: |
| 1059 | (*, 4, 4) homegeneous matrix that convert camera coord to world coord. |
| 1060 | Note that the y axis should be inverted in the cam_poses. |
| 1061 | subsample: int |
| 1062 | index stride |
| 1063 | other_maps: |
| 1064 | a list of (*, h, w, d) to associated with each point |
| 1065 | |
| 1066 | Returns: |
| 1067 | points: |
| 1068 | (*, h//subsample, w//subsample, 3) xyz in world coordinates |
| 1069 | |
| 1070 | Notes: |
| 1071 | The function assumes the image coordinate origin is at the upper-left. So H_c2w should include |
| 1072 | the y-inverted transformation (e.g., H_c2w = H_c2w_y_flipped * H_flip_y). |
| 1073 | |
| 1074 | Notes2: |
| 1075 | If an element in z_map == torch.inf or torch.nan, |
| 1076 | the output xyz_w of the corresponding point will be torch.nan. |
| 1077 | Other points will be normal. |
| 1078 | |
| 1079 | """ |
| 1080 | if other_maps is None: |
| 1081 | other_maps = [] |
| 1082 | |
| 1083 | dtype = z_map.dtype |
| 1084 | device = z_map.device |
| 1085 | h, w = z_map.size(-2), z_map.size(-1) |
| 1086 | |
| 1087 | # generate u v w on the sensor coord |
| 1088 | u, v = torch.meshgrid( |
| 1089 | torch.arange(0, w, subsample, device=device), |
| 1090 | torch.arange(0, h, subsample, device=device), |
| 1091 | indexing='xy', |
| 1092 | |
| 1093 | ) # u: (h', w') for x, v: (h', w') for y in the sensor coord |