Return V^-1 = I - 0.5 * S + 1/theta^2 * (1 - theta * sin(theta) / (2 * (1 - cos(theta)))) * S^2
(S: np.ndarray, theta: float = None)
| 151 | |
| 152 | @staticmethod |
| 153 | def get_inv_V(S: np.ndarray, theta: float = None): |
| 154 | """Return V^-1 = I - 0.5 * S + 1/theta^2 * (1 - theta * sin(theta) / (2 * (1 - cos(theta)))) * S^2""" |
| 155 | |
| 156 | if theta is None: |
| 157 | s = np.array([S[2, 1], S[0, 2], S[1, 0]]) |
| 158 | theta = np.sqrt(np.sum(s ** 2.0)) |
| 159 | |
| 160 | if theta > 0: |
| 161 | theta_square = theta * theta |
| 162 | c = 1 - (theta * np.sin(theta)) / (2 * (1 - np.cos(theta))) |
| 163 | return np.eye(3) - 0.5 * S + (c / theta_square) * (S @ S) |
| 164 | else: |
| 165 | return np.eye(3) |
| 166 | |
| 167 | @staticmethod |
| 168 | def multiply(H0, H1): |
no outgoing calls
no test coverage detected