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