Applies logarithm map to q :param q: (4,) :return: (3,)
(q)
| 19 | |
| 20 | ## NUMPY |
| 21 | def qlog(q): |
| 22 | """ |
| 23 | Applies logarithm map to q |
| 24 | :param q: (4,) |
| 25 | :return: (3,) |
| 26 | """ |
| 27 | if all(q[1:] == 0): |
| 28 | q = np.zeros(3) |
| 29 | else: |
| 30 | q = np.arccos(q[0]) * q[1:] / np.linalg.norm(q[1:]) |
| 31 | return q |
| 32 | |
| 33 | def process_poses_rotmat(poses_in, rot_mat): |
| 34 | """ |
nothing calls this directly
no outgoing calls
no test coverage detected