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)