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]])