def exp(cls, m):
m = np.asarray(m)
assert np.shape(m) == (3, ), 'shape was ' + str(np.shape(m))
t = np.linalg.norm(m)
if t < 1e-8:
return np.eye(3) # exp(0) = I
skewm = skew(m)
A = np.sin(t) / t
B = (1. - np.cos(t)) / (t * t)
I = np.eye(3)
return I + A * skewm + B * np.dot(skewm, skewm)
def exp(cls, m):
m = np.asarray(m)
assert np.shape(m) == (3,), 'shape was '+str(np.shape(m))
t = np.linalg.norm(m)
if t < 1e-8:
return np.eye(3) # exp(0) = I
skewm = skew(m)
A = np.sin(t)/t
B = (1. - np.cos(t)) / (t*t)
I = np.eye(3)
return I + A * skewm + B * np.dot(skewm, skewm)
def generator_field(cls, x):
return skew(x)
def J_expm_x(cls, x):
return skew(-x)
def generator_field(cls, x):
return skew(x)
def J_expm_x(cls, x):
return skew(-x)