Applies logarithm map to q :param q: (4,) :return: (3,)
(q)
| 45 | return poses_out |
| 46 | |
| 47 | def qlog(q): |
| 48 | """ |
| 49 | Applies logarithm map to q |
| 50 | :param q: (4,) |
| 51 | :return: (3,) |
| 52 | """ |
| 53 | if all(q[1:] == 0): |
| 54 | q = np.zeros(3) |
| 55 | else: |
| 56 | q = np.arccos(q[0]) * q[1:] / np.linalg.norm(q[1:]) |
| 57 | return q |
| 58 | |
| 59 | import transforms3d.quaternions as txq # Warning: outdated package |
| 60 |
no outgoing calls
no test coverage detected