Average unit quaternions properly.
(quats, weights=None)
| 1538 | |
| 1539 | |
| 1540 | def _average_quats(quats, weights=None): |
| 1541 | """Average unit quaternions properly.""" |
| 1542 | assert quats.ndim == 2 and quats.shape[1] in (3, 4) |
| 1543 | if weights is None: |
| 1544 | weights = np.ones(quats.shape[0]) |
| 1545 | assert (weights >= 0).all() |
| 1546 | norm = weights.sum() |
| 1547 | if weights.sum() == 0: |
| 1548 | return np.zeros(3) |
| 1549 | weights = weights / norm |
| 1550 | # The naive step here would be: |
| 1551 | # |
| 1552 | # avg_quat = np.dot(weights, quats[:, :3]) |
| 1553 | # |
| 1554 | # But this is not robust to quaternions having sign ambiguity, |
| 1555 | # i.e., q == -q. Thus we instead use the rank 1 update method: |
| 1556 | # |
| 1557 | # https://arc.aiaa.org/doi/abs/10.2514/1.28949?journalCode=jgcd |
| 1558 | # https://github.com/tolgabirdal/averaging_quaternions/blob/master/wavg_quaternion_markley.m # noqa: E501 |
| 1559 | # |
| 1560 | # We use unit quats and don't store the last element, so reconstruct it |
| 1561 | # to get our 4-element quaternions: |
| 1562 | quats = np.concatenate((_quat_real(quats)[..., np.newaxis], quats), -1) |
| 1563 | quats *= weights[:, np.newaxis] |
| 1564 | A = np.einsum("ij,ik->jk", quats, quats) # sum of outer product of each q |
| 1565 | avg_quat = linalg.eigh(A)[1][:, -1] # largest eigenvector is the avg |
| 1566 | # Same as the largest eigenvector from the concatenation of all as |
| 1567 | # svd(quats, full_matrices=False)[-1][0], but faster. |
| 1568 | # |
| 1569 | # By local convention we take the real term (which we remove from our |
| 1570 | # representation) as positive. Since it can be zero, let's just ensure |
| 1571 | # that the first non-zero element is positive. This shouldn't matter once |
| 1572 | # we go to a rotation matrix, but it's nice for testing to have |
| 1573 | # consistency. |
| 1574 | avg_quat *= np.sign(avg_quat[avg_quat != 0][0]) |
| 1575 | avg_quat = avg_quat[1:] |
| 1576 | return avg_quat |
| 1577 | |
| 1578 | |
| 1579 | @fill_doc |