def exp(v):
""" exponential map """
theta_sq = sophus.squared_norm(v)
theta = sympy.sqrt(theta_sq)
return So3(
sophus.Quaternion(sympy.cos(0.5 * theta),
sympy.sin(0.5 * theta) / theta * v))
def exp(v):
""" exponential map """
theta_sq = sophus.squared_norm(v)
theta = sympy.sqrt(theta_sq)
return So3(
sophus.Quaternion(
sympy.cos(0.5 * theta),
sympy.sin(0.5 * theta) / theta * v))
def exp(v):
""" exponential map """
upsilon = v[0:3, :]
omega = sophus.Vector3(v[3], v[4], v[5])
so3 = sophus.So3.exp(omega)
Omega = sophus.So3.hat(omega)
Omega_sq = Omega * Omega
theta = sympy.sqrt(sophus.squared_norm(omega))
V = (sympy.Matrix.eye(3) +
(1 - sympy.cos(theta)) / (theta**2) * Omega +
(theta - sympy.sin(theta)) / (theta**3) * Omega_sq)
return Se3(so3, V * upsilon)
def log(self):
omega = self.so3.log()
theta = sympy.sqrt(sophus.squared_norm(omega))
Omega = sophus.So3.hat(omega)
half_theta = 0.5 * theta
V_inv = sympy.Matrix.eye(3) - 0.5 * Omega + (1 - theta * sympy.cos(
half_theta) / (2 * sympy.sin(half_theta))) / (theta * theta) *\
(Omega * Omega)
upsilon = V_inv * self.t
return upsilon.col_join(omega)
def log(self):
""" logarithmic map"""
n = sympy.sqrt(sophus.squared_norm(self.q.vec))
return 2 * sympy.atan(n / self.q.real) / n * self.q.vec
def squared_norm(self):
""" squared norm when considering the quaternion as 4-tuple """
return sophus.squared_norm(self.vec) + self.real**2
def log(self):
""" logarithmic map"""
n = sympy.sqrt(sophus.squared_norm(self.q.vec))
return 2 * sympy.atan(n / self.q.real) / n * self.q.vec
def squared_norm(self):
""" squared norm when considering the quaternion as 4-tuple """
return sophus.squared_norm(self.vec) + self.real**2
