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 (3x3 matrix)
:return: 3D tangent space column vector xi_vec = [rho_vec, theta]^T.
"""
return np.vstack((xi_hat[:2, 2:3], SO2.vee(xi_hat[:2, :2])))
def test_vee_extracts_correct_angle_from_skew_symmetric_matrix():
theta = 3.0
theta_hat = SO2.hat(theta)
theta_hat_vee = SO2.vee(theta_hat)
np.testing.assert_array_equal(theta_hat_vee, theta)