| 124 | |
| 125 | |
| 126 | class Camera(object): |
| 127 | |
| 128 | def __init__(self, intrinsics): |
| 129 | self._intrinsics = intrinsics |
| 130 | self._camera_matrix = self.build_camera_matrix(self.intrinsics) |
| 131 | self._K_inv = inv(self.camera_matrix) |
| 132 | |
| 133 | @staticmethod |
| 134 | def build_camera_matrix(intrinsics): |
| 135 | """Build the 3x3 camera matrix K using the given intrinsics. |
| 136 | |
| 137 | Equation 6.10 from HZ. |
| 138 | """ |
| 139 | f = intrinsics['focal_length'] |
| 140 | pp_x = intrinsics['pp_x'] |
| 141 | pp_y = intrinsics['pp_y'] |
| 142 | |
| 143 | K = np.array([[f, 0, pp_x], [0, f, pp_y], [0, 0, 1]], dtype=np.float32) |
| 144 | # K[:, 0] *= -1. # Step 1 of Kyle |
| 145 | assert matrix_rank(K) == 3 |
| 146 | return K |
| 147 | |
| 148 | @staticmethod |
| 149 | def extrinsics2RT(extrinsics): |
| 150 | """Convert extrinsics matrix to separate rotation matrix R and translation vector T. |
| 151 | """ |
| 152 | assert extrinsics.shape == (4, 4) |
| 153 | R = extrinsics[:3, :3] |
| 154 | T = extrinsics[3, :3] |
| 155 | R = np.copy(R) |
| 156 | T = np.copy(T) |
| 157 | T = T.reshape(3, 1) |
| 158 | R[0, :] *= -1. # Step 1 of Kyle |
| 159 | T *= 100. # Convert from m to cm |
| 160 | return R, T |
| 161 | |
| 162 | def project(self, points_3d, extrinsics=None): |
| 163 | """Project a 3D point in camera coordinates into the camera/image plane. |
| 164 | |
| 165 | Args: |
| 166 | point_3d: |
| 167 | """ |
| 168 | if extrinsics is not None: # Map points to camera coordinates |
| 169 | points_3d = self.world2camera(extrinsics, points_3d) |
| 170 | |
| 171 | # Make sure to handle homogeneous AND non-homogeneous coordinate points |
| 172 | # Consider handling a set of points |
| 173 | raise NotImplementedError |
| 174 | |
| 175 | def backproject(self, |
| 176 | depth_map, |
| 177 | labels=None, |
| 178 | max_depth=None, |
| 179 | max_height=None, |
| 180 | min_height=None, |
| 181 | rgb_img=None, |
| 182 | extrinsics=None, |
| 183 | prune=True): |
nothing calls this directly
no outgoing calls
no test coverage detected