Enforce quaternion continuity across the time dimension by selecting the representation (q or -q) with minimal distance (or, equivalently, maximal dot product) between two consecutive frames. Expects a tensor of shape (L, J, 4), where L is the sequence length and J is the number of
(q)
| 147 | |
| 148 | |
| 149 | def qfix(q): |
| 150 | """ |
| 151 | Enforce quaternion continuity across the time dimension by selecting |
| 152 | the representation (q or -q) with minimal distance (or, equivalently, maximal dot product) |
| 153 | between two consecutive frames. |
| 154 | |
| 155 | Expects a tensor of shape (L, J, 4), where L is the sequence length and J is the number of joints. |
| 156 | Returns a tensor of the same shape. |
| 157 | """ |
| 158 | assert len(q.shape) == 3 |
| 159 | assert q.shape[-1] == 4 |
| 160 | |
| 161 | result = q.copy() |
| 162 | dot_products = np.sum(q[1:] * q[:-1], axis=2) |
| 163 | mask = dot_products < 0 |
| 164 | mask = (np.cumsum(mask, axis=0) % 2).astype(bool) |
| 165 | result[1:][mask] *= -1 |
| 166 | return result |
| 167 | |
| 168 | |
| 169 | def euler2quat(e, order, deg=True): |