Convert quaternion to rotation in batch. Since all operation in pytorch, support gradient passing. Args: quad (tensor, batch_size*4): quaternion. Returns: rot_mat (tensor, batch_size*3*3): rotation.
(q)
| 8 | |
| 9 | |
| 10 | def quad2rotation(q): |
| 11 | """ |
| 12 | Convert quaternion to rotation in batch. Since all operation in pytorch, support gradient passing. |
| 13 | |
| 14 | Args: |
| 15 | quad (tensor, batch_size*4): quaternion. |
| 16 | |
| 17 | Returns: |
| 18 | rot_mat (tensor, batch_size*3*3): rotation. |
| 19 | """ |
| 20 | # bs = quad.shape[0] |
| 21 | # qr, qi, qj, qk = quad[:, 0], quad[:, 1], quad[:, 2], quad[:, 3] |
| 22 | # two_s = 2.0 / (quad * quad).sum(-1) |
| 23 | # rot_mat = torch.zeros(bs, 3, 3).to(quad.get_device()) |
| 24 | # rot_mat[:, 0, 0] = 1 - two_s * (qj**2 + qk**2) |
| 25 | # rot_mat[:, 0, 1] = two_s * (qi * qj - qk * qr) |
| 26 | # rot_mat[:, 0, 2] = two_s * (qi * qk + qj * qr) |
| 27 | # rot_mat[:, 1, 0] = two_s * (qi * qj + qk * qr) |
| 28 | # rot_mat[:, 1, 1] = 1 - two_s * (qi**2 + qk**2) |
| 29 | # rot_mat[:, 1, 2] = two_s * (qj * qk - qi * qr) |
| 30 | # rot_mat[:, 2, 0] = two_s * (qi * qk - qj * qr) |
| 31 | # rot_mat[:, 2, 1] = two_s * (qj * qk + qi * qr) |
| 32 | # rot_mat[:, 2, 2] = 1 - two_s * (qi**2 + qj**2) |
| 33 | # return rot_mat |
| 34 | if not isinstance(q, torch.Tensor): |
| 35 | q = torch.tensor(q).cuda() |
| 36 | |
| 37 | norm = torch.sqrt( |
| 38 | q[:, 0] * q[:, 0] + q[:, 1] * q[:, 1] + q[:, 2] * q[:, 2] + q[:, 3] * q[:, 3] |
| 39 | ) |
| 40 | q = q / norm[:, None] |
| 41 | rot = torch.zeros((q.size(0), 3, 3)).to(q) |
| 42 | r = q[:, 0] |
| 43 | x = q[:, 1] |
| 44 | y = q[:, 2] |
| 45 | z = q[:, 3] |
| 46 | rot[:, 0, 0] = 1 - 2 * (y * y + z * z) |
| 47 | rot[:, 0, 1] = 2 * (x * y - r * z) |
| 48 | rot[:, 0, 2] = 2 * (x * z + r * y) |
| 49 | rot[:, 1, 0] = 2 * (x * y + r * z) |
| 50 | rot[:, 1, 1] = 1 - 2 * (x * x + z * z) |
| 51 | rot[:, 1, 2] = 2 * (y * z - r * x) |
| 52 | rot[:, 2, 0] = 2 * (x * z - r * y) |
| 53 | rot[:, 2, 1] = 2 * (y * z + r * x) |
| 54 | rot[:, 2, 2] = 1 - 2 * (x * x + y * y) |
| 55 | return rot |
| 56 | |
| 57 | def get_camera_from_tensor(inputs): |
| 58 | """ |
no test coverage detected