compute so(3) from the SO(3) Args: R (tensor): SO(3),rotation matrix. Returns: w (tensor,R^{3}): so(3).
(cls, R, eps=1e-7)
| 36 | |
| 37 | @classmethod |
| 38 | def log(cls, R, eps=1e-7): # [...,3,3] |
| 39 | """ |
| 40 | compute so(3) from the SO(3) |
| 41 | Args: |
| 42 | R (tensor): SO(3),rotation matrix. |
| 43 | Returns: |
| 44 | w (tensor,R^{3}): so(3). |
| 45 | """ |
| 46 | trace = R[..., 0, 0] + R[..., 1, 1] + R[..., 2, 2] |
| 47 | # ln(R) will explode if theta==pi |
| 48 | theta = ((trace - 1) / 2).clamp(-1 + eps, 1 - |
| 49 | eps).acos_()[..., None, None] % np.pi |
| 50 | lnR = 1 / (2 * cls.taylor_A(theta) + 1e-8) * \ |
| 51 | (R - R.transpose(-2, -1)) # FIXME: wei-chiu finds it weird |
| 52 | w0, w1, w2 = lnR[..., 2, 1], lnR[..., 0, 2], lnR[..., 1, 0] |
| 53 | w = torch.stack([w0, w1, w2], dim=-1) |
| 54 | return w |
| 55 | |
| 56 | @classmethod |
| 57 | def from_matrix(cls, Rt, eps=1e-8): # [...,3,4] |
no test coverage detected