def test_derivatives(self):
d = sophus.Matrix(
2, 2, lambda r, c: sympy.diff((self.a * self.b)[r], self.a[c]))
self.assertEqual(d, Complex.Da_a_mul_b(self.a, self.b))
d = sophus.Matrix(
2, 2, lambda r, c: sympy.diff((self.a * self.b)[r], self.b[c]))
self.assertEqual(d, Complex.Db_a_mul_b(self.a, self.b))
def Dxi_x_matrix(x, i):
if i == 0:
return sophus.Matrix([[1, 0],
[0, 1]])
if i == 1:
return sophus.Matrix([[0, -1],
[1, 0]])
def test_derivatives(self):
d = sophus.Matrix(
4, 4, lambda r, c: sympy.diff((self.a * self.b)[r], self.a[c]))
self.assertEqual(d, Quaternion.Da_a_mul_b(self.a, self.b))
d = sophus.Matrix(
4, 4, lambda r, c: sympy.diff((self.a * self.b)[r], self.b[c]))
self.assertEqual(d, Quaternion.Db_a_mul_b(self.a, self.b))
def Db_a_mul_b(a, b):
""" derivatice of quaternion muliplication wrt right multiplicand b """
u0 = a.vec[0]
u1 = a.vec[1]
u2 = a.vec[2]
x = a.real
return sophus.Matrix([[x, -u2, u1, u0], [u2, x, -u0, u1],
[-u1, u0, x, u2], [-u0, -u1, -u2, x]])
def log(self):
theta = self.so2.log()
halftheta = 0.5 * theta
a = -(halftheta * self.so2.z.imag) / (self.so2.z.real - 1)
V_inv = sophus.Matrix([[a, halftheta], [-halftheta, a]])
upsilon = V_inv * self.t
return sophus.Vector3(upsilon[0], upsilon[1], theta)
def Dxi_x_matrix(x, i):
if i == 0:
return sophus.Matrix([[0, 2 * x[1], 2 * x[2]],
[2 * x[1], -4 * x[0], -2 * x[3]],
[2 * x[2], 2 * x[3], -4 * x[0]]])
if i == 1:
return sophus.Matrix([[-4 * x[1], 2 * x[0], 2 * x[3]],
[2 * x[0], 0, 2 * x[2]],
[-2 * x[3], 2 * x[2], -4 * x[1]]])
if i == 2:
return sophus.Matrix([[-4 * x[2], -2 * x[3], 2 * x[0]],
[2 * x[3], -4 * x[2], 2 * x[1]],
[2 * x[0], 2 * x[1], 0]])
if i == 3:
return sophus.Matrix([[0, -2 * x[2], 2 * x[1]],
[2 * x[2], 0, -2 * x[0]],
[-2 * x[1], 2 * x[0], 0]])
def Da_a_mul_b(a, b):
""" derivatice of quaternion muliplication wrt left multiplier a """
v0 = b.vec[0]
v1 = b.vec[1]
v2 = b.vec[2]
y = b.real
return sophus.Matrix([[y, v2, -v1, v0], [-v2, y, v0, v1],
[v1, -v0, y, v2], [-v0, -v1, -v2, y]])
def matrix(self):
""" returns matrix representation """
return sophus.Matrix(
[[
1 - 2 * self.q.vec.y()**2 - 2 * self.q.vec.z()**2,
2 * self.q.vec.x() * self.q.vec.y() -
2 * self.q.vec.z() * self.q[3],
2 * self.q.vec.x() * self.q.vec.z() +
2 * self.q.vec.y() * self.q[3]
],
[
2 * self.q.vec.x() * self.q.vec.y() +
2 * self.q.vec.z() * self.q[3],
1 - 2 * self.q.vec.x()**2 - 2 * self.q.vec.z()**2,
2 * self.q.vec.y() * self.q.vec.z() -
2 * self.q.vec.x() * self.q[3]
],
[
2 * self.q.vec.x() * self.q.vec.z() -
2 * self.q.vec.y() * self.q[3],
2 * self.q.vec.y() * self.q.vec.z() +
2 * self.q.vec.x() * self.q[3],
1 - 2 * self.q.vec.x()**2 - 2 * self.q.vec.y()**2
]])
def matrix(self):
""" returns matrix representation """
return sophus.Matrix([
[self.z.real, -self.z.imag],
[self.z.imag, self.z.real]])
def Dx_exp_x_at_0():
return sophus.Matrix([[0.5, 0.0, 0.0, 0.0], [0.0, 0.5, 0.0, 0.0],
[0.0, 0.0, 0.5, 0.0]])
def calc_Dx_exp_x(x):
return sophus.Matrix(3, 4,
lambda r, c: sympy.diff(So3.exp(x)[c], x[r, 0]))
def hat(o):
return sophus.Matrix([[0, -o[2], o[1]], [o[2], 0, -o[0]],
[-o[1], o[0], 0]])
def calc_Dx_exp_x_matrix_at_0(x):
return sophus.Matrix(2, 2, lambda r, c:
sympy.diff(So2.exp(x).matrix()[r, c], x)
).limit(x, 0)
def calc_Dxi_exp_x_matrix(x, i):
return sophus.Matrix(
3, 3, lambda r, c: sympy.diff(So3.exp(x).matrix()[r, c], x[i]))
def hat(theta):
return sophus.Matrix([[0, -theta],
[theta, 0]])
def calc_Dx_exp_x(x):
return sophus.Matrix(1, 2, lambda r, c:
sympy.diff(So2.exp(x)[c], x))
def Db_a_mul_b(a, b):
""" derivatice of complex muliplication wrt right multiplicand b """
return sophus.Matrix([[a.real, -a.imag], [a.imag, a.real]])
def Da_a_mul_b(a, b):
""" derivatice of complex muliplication wrt left multiplier a """
return sophus.Matrix([[b.real, -b.imag], [b.imag, b.real]])
def matrix(self):
""" returns matrix representation """
R = self.so3.matrix()
return (R.row_join(self.t)).col_join(sophus.Matrix(1, 4, [0, 0, 0, 1]))
def calc_Dxi_exp_x_matrix_at_0(x, i):
return sophus.Matrix(
3, 3,
lambda r, c: sympy.diff(So3.exp(x).matrix()[r, c], x[i])).subs(
x[0], 0).subs(x[1], 0).limit(x[2], 0)
def calc_Dxi_x_matrix(x, i):
return sophus.Matrix(3, 3,
lambda r, c: sympy.diff(x.matrix()[r, c], x[i]))
def Dx_exp_x_at_0():
return sophus.Matrix([[0, 1]])
