(theta)
| 500 | |
| 501 | |
| 502 | def V(theta): |
| 503 | dtype = theta.dtype |
| 504 | device = theta.device |
| 505 | I = torch.eye(3, device=device, dtype=dtype) |
| 506 | W = skew_sym_mat(theta) |
| 507 | W2 = W @ W |
| 508 | angle = torch.norm(theta) |
| 509 | if angle < 1e-5: |
| 510 | V = I + 0.5 * W + (1.0 / 6.0) * W2 |
| 511 | else: |
| 512 | V = ( |
| 513 | I |
| 514 | + W * ((1.0 - torch.cos(angle)) / (angle**2)) |
| 515 | + W2 * ((angle - torch.sin(angle)) / (angle**3)) |
| 516 | ) |
| 517 | return V |
| 518 | |
| 519 | |
| 520 | def SE3_exp(tau): |
no test coverage detected