41 lines
919 B
Python
41 lines
919 B
Python
|
import numpy as np
|
||
|
|
|
||
|
|
def hat(w):
|
||
|
|
wx, wy, wz = w
|
||
|
|
return np.array([[0, -wz, wy],
|
||
|
|
[wz, 0, -wx],
|
||
|
|
[-wy, wx, 0]], dtype=float)
|
||
|
|
|
||
|
|
|
||
|
|
def rotvec_to_R(w):
|
||
|
|
"""Axis-angle (rotation vector) to rotation matrix."""
|
||
|
|
th = np.linalg.norm(w)
|
||
|
|
if th < 1e-12:
|
||
|
|
return np.eye(3)
|
||
|
|
k = w / th
|
||
|
|
K = hat(k)
|
||
|
|
return np.eye(3) + np.sin(th) * K + (1-np.cos(th)) * (K @ K)
|
||
|
|
|
||
|
|
|
||
|
|
def R_to_rotvec(R):
|
||
|
|
"""Rotation matrix to rotation vector (log map)."""
|
||
|
|
tr = np.trace(R)
|
||
|
|
cos_th = (tr - 1) / 2
|
||
|
|
cos_th = np.clip(cos_th, -1.0, 1.0)
|
||
|
|
th = np.arccos(cos_th)
|
||
|
|
if th < 1e-12:
|
||
|
|
return np.zeros(3)
|
||
|
|
w_hat = (R - R.T) / (2*np.sin(th))
|
||
|
|
return th * np.array([w_hat[2,1], w_hat[0,2], w_hat[1,0]])
|
||
|
|
|
||
|
|
|
||
|
|
def normalize(v, eps=1e-12):
|
||
|
|
n = np.linalg.norm(v)
|
||
|
|
if n < eps:
|
||
|
|
return v*0.0
|
||
|
|
return v / n
|
||
|
|
|
||
|
|
|
||
|
|
def clip(x, a, b):
|
||
|
|
return max(a, min(b, x))
|