def setUp(self): upsilon0, upsilon1, theta = sympy.symbols( 'upsilon[0], upsilon[1], theta', real=True) x, y = sympy.symbols('c[0] c[1]', real=True) p0, p1 = sympy.symbols('p0 p1', real=True) t0, t1 = sympy.symbols('t[0] t[1]', real=True) self.upsilon_theta = sophus.Vector3(upsilon0, upsilon1, theta) self.t = sophus.Vector2(t0, t1) self.a = Se2(sophus.So2(sophus.Complex(x, y)), self.t) self.p = sophus.Vector2(p0, p1)
def setUp(self): self.theta = sympy.symbols( 'theta', real=True) x, y = sympy.symbols('x y', real=True) p0, p1 = sympy.symbols('p0 p1', real=True) self.a = So2(sophus.Complex(x, y)) self.p = sophus.Vector2(p0, p1)
def exp(v): """ exponential map """ theta = v[2] so2 = sophus.So2.exp(theta) a = so2.z.imag / theta b = (1 - so2.z.real) / theta t = sophus.Vector2(a * v[0] - b * v[1], b * v[0] + a * v[1]) return Se2(so2, t)
def hat(v): upsilon = sophus.Vector2(v[0], v[1]) theta = v[2] return sophus.So2.hat(theta).\ row_join(upsilon).\ col_join(sympy.Matrix.zeros(1, 3))