def vee(xi_hat):
"""Performs the vee operator on the Lie Algebra matrix xi_hat,
which returns the corresponding tangent space vector.
:param xi_hat: The Lie Algebra (4x4 matrix)
:return: 6d tangent space column vector xi_vec = [rho_vec, theta_vec]^T.
"""
return np.vstack((xi_hat[:3, 3:4], SO3.vee(xi_hat[:3, :3])))
def test_vee_extracts_correct_vector_from_skew_symmetric_matrix():
theta_vec = np.array([[3, 2, 1]]).T
theta_hat = SO3.hat(theta_vec)
theta_hat_vee = SO3.vee(theta_hat)
np.testing.assert_array_equal(theta_hat_vee, theta_vec)
