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