def v(w):
l = w[6]
w = w[:3, :]
if len(l.free_symbols) == 0 and abs(l.evalf()) < 1e-9:
return se3.v(w[:3, :])
s = skew(w)
e = sy.exp(l)
theta = norm3(w)
l2t2 = l**2 + theta**2
A = (e - 1) / l
if len(theta.free_symbols) == 0 and abs(theta.evalf()) < 1e-9:
B = (1 + e * (l - 1)) / l**2
C = (-2 + e * (2 - 2 * l + l**2)) / (2 * l**3)
else:
B = (theta *
(1 - e * sy.cos(theta)) + e * l * sy.sin(theta)) / (theta * l2t2)
C = A / (theta**2) - e * sy.sin(theta) / (theta * l2t2) - l * (
e * sy.cos(theta) - 1) / (theta**2 * l2t2)
return A * sy.eye(3) + B * s + C * s * s
def dba_db(wa, wb):
A = exp(-wa)
B = exp(wb)
AB = A * B
theta = sy.acos((AB.trace() - 1) / 2)
s1 = (sy.sin(theta) -
theta * sy.cos(theta)) / (2 * sy.sin(theta)**2) * (AB - AB.T)
result = sy.zeros(3, 3)
for i in range(3):
dx = sy.zeros(3, 1)
dx[i] = 1.0
dAXB = A * skew(dx) * B
dtheta = -dAXB.trace() / (2 * sy.sin(theta))
foo = A * exp(1e-6 * dx) * B
theta2 = sy.acos((foo.trace() - 1) / 2)
s2 = theta / (2 * sy.sin(theta)) * (dAXB - dAXB.T)
dalpha = dtheta * s1[2, 1] + s2[2, 1]
dbeta = dtheta * s1[0, 2] + s2[0, 2]
dgamma = dtheta * s1[1, 0] + s2[1, 0]
result[0, i] = dalpha
result[1, i] = dbeta
result[2, i] = dgamma
return result
def dw_dw(w):
s = skew(w)
theta = norm3(w)
A = -1 / 2
B = 1 / theta**2 - (1 + sy.cos(theta)) / (2 * theta * sy.sin(theta))
return sy.eye(3) + A * s + B * s * s
def dv(w, dw_dx):
w = w[:3, :]
dw_dx = dw_dx[:3, :]
wW = (w.T * dw_dx)[0, 0]
s = skew(w)
S = skew(dw_dx)
theta = norm3(w)
F1 = sy.sin(theta) / theta**3 - 2 * (1 - sy.cos(theta)) / theta**4
F2 = (1 - sy.cos(theta)) / theta**4 - 3 * (theta -
sy.sin(theta)) / theta**5
F3 = (1 - sy.cos(theta)) / theta**2
F4 = (theta - sy.sin(theta)) / theta**3
return wW * (F1 * s + F2 * s * s) + F3 * S + F4 * (s * S + S * s)
def dv(w, dw_dx):
l = w[6]
w = w[:3, :]
if len(l.free_symbols) == 0 and abs(l.evalf()) < 1e-9:
return se3.dv(w, dw_dx)
dw_dx = dw_dx[:3, :]
wW = (w.T * dw_dx)[0, 0]
s = skew(w)
S = skew(dw_dx)
e = sy.exp(l)
theta = norm3(w)
l2t2 = l**2 + theta**2
e = sy.exp(l)
le1cos = l * (1 - e * sy.cos(theta))
F1 = -2*(l*e*sy.sin(theta)) / (theta*l2t2**2) - \
2*(1 - e*sy.cos(theta)) / (l2t2**2) + \
(l*e*sy.cos(theta) - e*sy.cos(theta) + 1) / (theta**2 * l2t2) + \
(e*sy.sin(theta)) / (theta * l2t2) - \
l*e*sy.sin(theta) / (theta**3 * l2t2) - \
(1 - e*sy.cos(theta)) / (theta**2*l2t2)
F2 = -2*le1cos / (theta ** 2 * l2t2 ** 2) - \
2*le1cos / (theta**4 * l2t2) + \
2*e*sy.sin(theta) / (theta * l2t2**2) - \
e*sy.cos(theta) / (theta**2 * l2t2) + \
l*e*sy.sin(theta) / (theta**3 * l2t2) + \
e*sy.sin(theta) / (theta**3 * l2t2) - \
2*(e-1) / (l*theta**4)
F3 = l*e*sy.sin(theta) / (theta * l2t2) + \
(1 - e*sy.cos(theta)) / l2t2
F4 = le1cos / (theta**2 * l2t2) - \
e * sy.sin(theta) / (theta * l2t2) - \
(-e + 1) / (l * theta**2)
return wW * (F1 * s + F2 * s * s) + F3 * S + F4 * (s * S + S * s)
def exp(w):
s = skew(w)
theta = norm3(w)
if len(theta.free_symbols) == 0 and theta.evalf() < 1e-9:
A = 1
B = 1 / 2
else:
A = sy.sin(theta) / theta
B = (1 - sy.cos(theta)) / theta**2
return sy.eye(3) + A * s + B * s * s
def v(w):
w = w[:3, :]
s = skew(w)
theta = norm3(w)
if len(theta.free_symbols) == 0 and theta.evalf() < 1e-9:
A = 1 / 2
B = 1 / 6
else:
A = (1 - sy.cos(theta)) / theta**2
B = (theta - sy.sin(theta)) / theta**3
return sy.eye(3) + A * s + B * s * s
def dw_dw(w):
result = sy.zeros(6, 6)
vw = v(w)
vi = so3.dw_dw(w[:3, :])
result[:3, :3] = result[3:, 3:] = vi
for i in range(3):
dx = sy.zeros(3)
dx[i] = 1
result[3:,
i] = -vi * (dv(w[:3, :], vi[:, i]) - skew(dx) * vw) * w[3:6, :]
return result
def dv_dlambda(w):
l = w[6]
w = w[:3, :]
s = skew(w)
e = sy.exp(l)
theta = norm3(w)
l2t2 = l**2 + theta**2
if len(l.free_symbols) == 0 and abs(l.evalf()) < 1e-9:
A = 1 / 2
if len(theta.free_symbols) == 0 and theta.evalf() < 1e-9:
B = 1 / 3
C = 1 / 8
else:
B = -(theta * sy.cos(theta) - sy.sin(theta)) / theta**3
C = -(-theta**2 + 2 * theta * sy.sin(theta) + 2 * sy.cos(theta) -
2) / (2 * theta**4)
else:
A = e / l - (e - 1) / l**2
if len(theta.free_symbols) == 0 and theta.evalf() < 1e-9:
B = (l**2 * e - 2 * l * e + 2 * e - 2) / l**3
C = (l**3 * e - 3 * l**2 * e + 6 * l * e - 6 * e + 6) / (2 * l**4)
else:
B = -2*l*(l*e*sy.sin(theta) + theta*(-e*sy.cos(theta) + 1))/(theta*(l2t2)**2) + \
(l*e*sy.sin(theta) - theta*e*sy.cos(theta) + e*sy.sin(theta))/(theta*(l2t2))
C = 2*l**2*(e*sy.cos(theta) - 1)/(theta**2*(l2t2)**2) + \
2*l*e*sy.sin(theta)/(theta*(l2t2)**2) - \
l*e*sy.cos(theta)/(theta**2*(l2t2)) - \
e*sy.sin(theta)/(theta*(l2t2)) - \
(e*sy.cos(theta) - 1)/(theta**2*(l2t2)) + \
e/(l*theta**2) - \
(e - 1)/(l**2*theta**2)
return A * sy.eye(3) + B * s + C * s * s
def dw_dw(w):
result = sy.zeros(7, 7)
vw = v(w)
vi = vw**-1
so3_dw_dw = so3.dw_dw(w[:3, :])
result[:3, :3] = so3_dw_dw
result[3:6, 3:6] = vi * sy.exp(-w[6])
result[6, 6] = 1
for i in range(3):
dx = sy.zeros(3)
dx[i] = 1
result[3:6,
i] = -vi * (dv(w, so3_dw_dw[:, i]) - skew(dx) * vw) * w[3:6, :]
result[3:6, 6] = -vi * dv_dlambda(w) * w[3:6, :]
return result
def test_skew():
skew_list = skew('CATGGGCATCGGCCATACGCC')
assert ' '.join(map(
str, skew_list)) == '0 -1 -1 -1 0 1 2 1 1 1 0 1 2 1 0 0 0 0 -1 0 -1 -2'