def test_vee():
rho_vec = np.array([[4, 3, 2]]).T
theta_vec = 2 * np.pi / 3 * np.array([[2, 1, 1]]).T / np.sqrt(6)
xi_vec = np.vstack((rho_vec, theta_vec))
xi_hat = SE3.hat(xi_vec)
np.testing.assert_equal(SE3.vee(xi_hat), xi_vec)
def test_hat():
rho_vec = np.array([[1, 2, 3]]).T
theta_vec = np.pi / 3 * np.array([[1, -1, 2]]).T / np.sqrt(6)
xi_vec = np.vstack((rho_vec, theta_vec))
xi_hat_expected = np.block([[SO3.hat(theta_vec), rho_vec],
[np.zeros((1, 4))]])
np.testing.assert_equal(SE3.hat(xi_vec), xi_hat_expected)