def dexp(v): x = np.linalg.norm(v) a = trig.sinox(x) b = trig.cosox2(x) c = trig.sinox3(x) I = np.identity(3) S = skew(v) W = v * v.transpose() return a*I + b*S + c*W
def exp(v): hv = 0.5*v theta = np.linalg.norm(hv) a = trig.sinox(theta) b = math.cos(theta) return Quaternion(b, a*hv)