processes the 1x12 raw pose from dataset by aligning and then normalizing :param poses_in: N x 12 :param mean_t: 3 :param std_t: 3 :return: processed poses (translation + quaternion) N x 7
(poses_in, mean_t, std_t)
| 21 | import json |
| 22 | |
| 23 | def RT2QT(poses_in, mean_t, std_t): |
| 24 | """ |
| 25 | processes the 1x12 raw pose from dataset by aligning and then normalizing |
| 26 | :param poses_in: N x 12 |
| 27 | :param mean_t: 3 |
| 28 | :param std_t: 3 |
| 29 | :return: processed poses (translation + quaternion) N x 7 |
| 30 | """ |
| 31 | poses_out = np.zeros((len(poses_in), 7)) |
| 32 | poses_out[:, 0:3] = poses_in[:, [3, 7, 11]] |
| 33 | |
| 34 | # align |
| 35 | for i in range(len(poses_out)): |
| 36 | R = poses_in[i].reshape((3, 4))[:3, :3] |
| 37 | q = txq.mat2quat(R) |
| 38 | q = q/(np.linalg.norm(q) + 1e-12) # normalize |
| 39 | q *= np.sign(q[0]) # constrain to hemisphere |
| 40 | poses_out[i, 3:] = q |
| 41 | |
| 42 | # normalize translation |
| 43 | poses_out[:, :3] -= mean_t |
| 44 | poses_out[:, :3] /= std_t |
| 45 | return poses_out |
| 46 | |
| 47 | def qlog(q): |
| 48 | """ |