Geodestic interpolation between RigidMotion H0 and RigidMotion H1. t=0 -> H0, t=1 -> H1.
(t: float, H0: 'RigidMotion', H1: 'RigidMotion')
| 172 | |
| 173 | @staticmethod |
| 174 | def interp(t: float, H0: 'RigidMotion', H1: 'RigidMotion') -> 'RigidMotion': |
| 175 | """ |
| 176 | Geodestic interpolation between RigidMotion H0 and RigidMotion H1. t=0 -> H0, t=1 -> H1. |
| 177 | """ |
| 178 | |
| 179 | if not np.allclose(H0.R @ H0.R.T, np.eye(3)): |
| 180 | warnings.warn('support only rigit transformation') |
| 181 | if not np.allclose(H1.R @ H1.R.T, np.eye(3)): |
| 182 | warnings.warn('support only rigit transformation') |
| 183 | |
| 184 | H0_inv = RigidMotion.inverse(H0) |
| 185 | H = RigidMotion.multiply(H1, H0_inv) |
| 186 | S = RigidMotion.log_rotation(H.R) |
| 187 | s = np.array([S[2, 1], S[0, 2], S[1, 0]]) |
| 188 | theta = np.sqrt(np.sum(s ** 2.0)) |
| 189 | inv_V1 = RigidMotion.get_inv_V(S, theta) |
| 190 | U = inv_V1 @ H.t |
| 191 | |
| 192 | interp_R = RigidMotion.exp_skew_symmetric(S, t, theta) |
| 193 | interp_t_times_V = RigidMotion.get_t_times_V(t, S, theta) |
| 194 | out_R = interp_R @ H0.R |
| 195 | out_t = interp_R @ H0.t + interp_t_times_V @ U |
| 196 | return RigidMotion(R=out_R, t=out_t) |
| 197 | |
| 198 | |
| 199 | def interp_homegeneous_matrices( |
no test coverage detected