(tau)
| 518 | |
| 519 | |
| 520 | def SE3_exp(tau): |
| 521 | dtype = tau.dtype |
| 522 | device = tau.device |
| 523 | |
| 524 | rho = tau[:3] |
| 525 | theta = tau[3:] |
| 526 | R = SO3_exp(theta) |
| 527 | t = V(theta) @ rho |
| 528 | |
| 529 | T = torch.eye(4, device=device, dtype=dtype) |
| 530 | T[:3, :3] = R |
| 531 | T[:3, 3] = t |
| 532 | return T |