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 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 exp(theta):
""" exponential map """
return So2(
sophus.Complex(
sympy.cos(theta),
sympy.sin(theta)))
