| 178 | return pose, intrinsic |
| 179 | |
| 180 | def fundamental_matrix_gen(Tcw1, Tcw2, K1, K2): |
| 181 | |
| 182 | T21 = Tcw2.dot(np.linalg.inv(Tcw1)) |
| 183 | |
| 184 | R = T21[:3, :3] |
| 185 | t = T21[:3, 3] |
| 186 | |
| 187 | t_antisymmetric = np.zeros([3, 3]) |
| 188 | t_antisymmetric[0][1] = -t[2] |
| 189 | t_antisymmetric[0][2] = t[1] |
| 190 | t_antisymmetric[1][0] = t[2] |
| 191 | t_antisymmetric[1][2] = -t[0] |
| 192 | t_antisymmetric[2][0] = -t[1] |
| 193 | t_antisymmetric[2][1] = t[0] |
| 194 | |
| 195 | essential_matrix = t_antisymmetric.dot(R) |
| 196 | |
| 197 | fundamental_matrix = (np.linalg.inv(K2.transpose()).dot(essential_matrix)).dot(np.linalg.inv(K1)) |
| 198 | |
| 199 | return fundamental_matrix.astype(np.float32) |
| 200 | |
| 201 | def expand_dim(tensor, dim, desired_dim_len): |
| 202 | sz = list(tensor.size()) |