def test_difference_for_simple_rotation_works(): X = SO3() theta_vec = np.pi / 3 * np.array([[0, 1, 0]]).T Y = SO3.Exp(theta_vec) theta_vec_diff = Y.ominus(X) np.testing.assert_array_equal(theta_vec_diff, theta_vec)
def test_difference_for_simple_rotation_with_operator_works(): X = SO3() theta_vec = 3 * np.pi / 4 * np.array([[1, 0, -1]]).T / np.sqrt(2) Y = SO3.Exp(theta_vec) theta_vec_diff = Y - X np.testing.assert_almost_equal(theta_vec_diff, theta_vec, 14)
def test_to_rotation_matrix_results_in_close_rotation(): angle = 0.5 * np.pi axis = np.array([[1, 0, 0]]).T R = SO3.Exp(angle * axis).matrix # Invalidate a valid rotation matrix by scaling it. R_scaled = 3 * R # Fit to SO(3). R_closest = SO3.to_so3_matrix(R_scaled) # Result should be the same rotation matrix. np.testing.assert_almost_equal(R_closest, R, 14) # Perturb the rotation matrix with random noise. R_noisy = R + 0.01 * np.random.rand(3, 3) # Fit to SO(3) so3_closest = SO3(R_noisy) # Extract angle-axis representation. angle_closest, axis_closest = so3_closest.Log(True) # Result should be close to the same rotation. np.testing.assert_almost_equal(angle_closest, angle, 2) np.testing.assert_almost_equal(axis_closest, axis, 2)
def main(): # Define the first pose. T_1 = SE3((SO3.from_roll_pitch_yaw(np.pi / 4, 0, np.pi / 2), np.array([[1, 1, 1]]).T)) # Define the second pose. T_2 = SE3((SO3.from_roll_pitch_yaw(-np.pi / 6, np.pi / 4, np.pi / 2), np.array([[1, 4, 2]]).T)) # Plot the interpolation. # Use Qt 5 backend in visualisation. matplotlib.use('qt5agg') # Create figure and axis. fig = plt.figure() ax = plt.axes(projection='3d') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') # Plot the poses. vg.plot_pose(ax, T_1.to_tuple()) vg.plot_pose(ax, T_2.to_tuple()) # Plot the interpolated poses. for alpha in np.linspace(0, 1, 20): T = interpolate_lie_element(alpha, T_1, T_2) vg.plot_pose(ax, T.to_tuple(), alpha=0.1) # Show figure. vg.plot.axis_equal(ax) plt.show()
def test_oplus_with_ominus_diff(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) xi_vec_diff = Y.ominus(X) Y_from_X = X.oplus(xi_vec_diff) np.testing.assert_almost_equal(Y_from_X.to_matrix(), Y.to_matrix(), 14)
def test_construct_with_angle_axis(): angle = 3 * np.pi / 4 axis = np.array([[1, 1, -1]]) / np.sqrt(3) theta_vec = angle * axis so3 = SO3.from_angle_axis(angle, axis) np.testing.assert_array_equal(so3.matrix, SO3.Exp(theta_vec).matrix)
def test_composition_with_identity_works(): so3 = SO3.from_angle_axis(np.pi / 4, np.array([[0, 1, 0]]).T) comp_with_identity = so3.compose(SO3()) comp_from_identity = SO3().compose(so3) np.testing.assert_almost_equal(comp_with_identity.matrix, so3.matrix, 14) np.testing.assert_almost_equal(comp_from_identity.matrix, so3.matrix, 14)
def _compute_horizon_slope(attitude: SO3): theta = ArtificialHorizonArtist._find_heading_angle(attitude) H = attitude.inv() * np.array([[np.cos(theta)], [np.sin(theta)], [0.0] ]) DH = attitude.inv() * np.array([[-np.sin(theta)], [np.cos(theta)], [0.0]]) DH2 = H[0, 0] * DH - DH[0, 0] * H slope = np.arctan2(DH2[2, 0], DH2[1, 0]) return slope
def test_adjoint(): np.testing.assert_equal(SE3().adjoint(), np.identity(6)) X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T)) Adj = X.adjoint() np.testing.assert_almost_equal(Adj[:3, :3], X.rotation.matrix, 14) np.testing.assert_almost_equal(Adj[3:, 3:], X.rotation.matrix, 14) np.testing.assert_almost_equal(Adj[:3, 3:], SO3.hat(X.translation) @ X.rotation.matrix, 14)
def test_composition_with_operator_works(): so3_90_z = SO3.from_angle_axis(np.pi / 2, np.array([[0, 0, 1]]).T) so3_n90_x = SO3.from_angle_axis(np.pi / 2, np.array([[-1, 0, 0]]).T) so3_comp = so3_n90_x @ so3_90_z expected = np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]) np.testing.assert_almost_equal(so3_comp.matrix, expected, 14)
def test_composition_returns_correct_rotation(): so3_90_x = SO3.from_angle_axis(np.pi / 2, np.array([[1, 0, 0]]).T) so3_90_y = SO3.from_angle_axis(np.pi / 2, np.array([[0, 1, 0]]).T) so3_comp = so3_90_y.compose(so3_90_x) expected = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]) np.testing.assert_almost_equal(so3_comp.matrix, expected, 14)
def test_jacobian_right(): theta_vec = 3 * np.pi / 4 * np.array([[1, -1, 1]]).T / np.sqrt(3) J_r = SO3.jac_right(theta_vec) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = SO3.Exp(theta_vec + delta) - (SO3.Exp(theta_vec) + J_r @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_composition(): X = SE3((SO3.rot_x(np.pi / 2), np.array([[1, 2, 3]]).T)) Y = SE3((SO3.rot_z(np.pi / 2), np.array([[0, 1, 0]]).T)) Z = X.compose(Y) rot_expected = SO3.from_angle_axis(2 * np.pi / 3, np.array([[1, -1, 1]]).T / np.sqrt(3)) t_expected = np.array([[1, 2, 4]]).T np.testing.assert_almost_equal(Z.rotation.matrix, rot_expected.matrix, 14) np.testing.assert_almost_equal(Z.translation, t_expected, 14)
def test_composition_with_operator(): X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T)) Y = SE3((SO3.rot_y(np.pi / 2), np.array([[-1, 0, 1]]).T)) Z = X @ Y rot_expected = SO3.from_angle_axis(2 * np.pi / 3, np.array([[-1, 1, -1]]).T / np.sqrt(3)) t_expected = np.array([[0, 3, 3]]).T np.testing.assert_almost_equal(Z.rotation.matrix, rot_expected.matrix, 14) np.testing.assert_almost_equal(Z.translation, t_expected, 14)
def test_jacobian_composition_XY_wrt_Y(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) J_comp_Y = X.jac_composition_XY_wrt_Y() # Jacobian should be identity np.testing.assert_array_equal(J_comp_Y, np.identity(6)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.compose(Y.oplus(delta)) - X.compose(Y).oplus(J_comp_Y @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 14)
def test_jacobian_Y_ominus_X_wrt_Y(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X) # Should be J_r_inv. np.testing.assert_equal(J_ominus_Y, SE3.jac_right_inverse(Y - X)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = Y.oplus(delta).ominus(X) - (Y.ominus(X) + (J_ominus_Y @ delta)) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 6)
def test_jacobian_composition_XY_wrt_X(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) J_comp_X = X.jac_composition_XY_wrt_X(Y) # Jacobian should be Y.inverse().adjoint() np.testing.assert_almost_equal(J_comp_X, Y.inverse().adjoint(), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).compose(Y) - X.compose(Y).oplus(J_comp_X @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 14)
def test_construct_with_matrix(): R = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]) so3 = SO3(R) np.testing.assert_array_equal(so3.matrix, R) # Matrices not elements of SO(3) should be fitted to SO(3) R_noisy = R + 0.1 + np.random.rand(3) so3_fitted = SO3(R_noisy) R_fitted = so3_fitted.matrix assert np.any(np.not_equal(R_fitted, R_noisy)) np.testing.assert_almost_equal(np.linalg.det(R_fitted), 1, 14) np.testing.assert_almost_equal((R_fitted.T @ R_fitted), np.identity(3), 14)
def test_jacobian_composition_XY_wrt_Y(): X = SO3.from_angle_axis(np.pi / 4, np.array([[1, -1, 1]]).T / np.sqrt(3)) Y = SO3.from_angle_axis(np.pi / 2, np.array([[1, 0, 1]]).T / np.sqrt(2)) J_comp_Y = X.jac_composition_XY_wrt_Y() # Jacobian should be identity np.testing.assert_array_equal(J_comp_Y, np.identity(3)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = X.compose(Y.oplus(delta)) - X.compose(Y).oplus( J_comp_Y @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_jacobian_action_Xx_wrt_X(): X = SO3.from_angle_axis(3 * np.pi / 4, np.array([[1, -1, 1]]).T / np.sqrt(3)) x = np.array([[1, 2, 3]]).T J_action_X = X.jac_action_Xx_wrt_X(x) # Jacobian should be -X.matrix * SO3.hat(x). np.testing.assert_array_equal(J_action_X, -X.matrix @ SO3.hat(x)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = X.oplus(delta).action(x) - (X.action(x) + J_action_X @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_jacobian_composition_XY_wrt_X(): X = SO3.from_angle_axis(np.pi / 4, np.array([[1, -1, 1]]).T / np.sqrt(3)) Y = SO3.from_angle_axis(np.pi / 2, np.array([[1, 0, 1]]).T / np.sqrt(2)) J_comp_X = X.jac_composition_XY_wrt_X(Y) # Jacobian should be Y.inverse().adjoint() np.testing.assert_array_equal(J_comp_X, Y.inverse().adjoint()) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = X.oplus(delta).compose(Y) - X.compose(Y).oplus( J_comp_X @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_jacobian_Y_ominus_X_wrt_Y(): X = SO3.from_angle_axis(np.pi / 4, np.array([[1, 1, 1]]).T / np.sqrt(3)) Y = SO3.from_angle_axis(np.pi / 3, np.array([[1, 0, -1]]).T / np.sqrt(2)) J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X) # Should be J_r_inv. np.testing.assert_equal(J_ominus_Y, SO3.jac_right_inverse(Y - X)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = Y.oplus(delta).ominus(X) - (Y.ominus(X) + (J_ominus_Y @ delta)) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 6)
def test_jacobian_right_inverse(): theta_vec = 3 * np.pi / 4 * np.array([[1, -1, 0]]).T / np.sqrt(2) X = SO3.Exp(theta_vec) J_r_inv = SO3.jac_right_inverse(theta_vec) # Should have J_l * J_r_inv = Exp(theta_vec).adjoint(). J_l = SO3.jac_left(theta_vec) np.testing.assert_almost_equal(J_l @ J_r_inv, SO3.Exp(theta_vec).adjoint(), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = X.oplus(delta).Log() - (X.Log() + J_r_inv @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_action_on_vectors_with_operator(): X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T)) x = np.array([[-1, 0, 1]]).T vec_expected = np.array([[0, 3, 3]]).T np.testing.assert_almost_equal(X * x, vec_expected, 14)
def test_construct_with_tuple(): so3 = SO3() + np.array([[0.1, -0.3, 0.2]]).T t = np.array([[1, 2, 3]]).T se3 = SE3((so3, t)) np.testing.assert_equal(se3.rotation.matrix, so3.matrix) np.testing.assert_equal(se3.translation, t)
def test_exp_of_log(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) xi_vec = X.Log() np.testing.assert_almost_equal( SE3.Exp(xi_vec).to_matrix(), X.to_matrix(), 14)
def test_exp_for_vector_close_to_zero_norm_returns_identity(): theta = 1e-14 u_vec = np.array([[0, 1, 0]]).T so3 = SO3.Exp(theta * u_vec) np.testing.assert_array_equal(so3.matrix, np.identity(3))
def test_action_on_vector_works(): unit_x = np.array([[1, 0, 0]]).T unit_y = np.array([[0, 1, 0]]).T unit_z = np.array([[0, 0, 1]]).T so3 = SO3.from_angle_axis(np.pi / 2, unit_y) np.testing.assert_almost_equal(so3.action(unit_x), -unit_z, 14)
def test_jacobian_action_Xx_wrt_X(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) x = np.array([[1, 2, 3]]).T J_action_X = X.jac_action_Xx_wrt_X(x) # Jacobian should be [R, - R*SO3.hat(x)]. np.testing.assert_array_equal( J_action_X, np.block([[X.rotation.matrix, -(X.rotation.matrix @ SO3.hat(x))]])) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).action(x) - (X.action(x) + J_action_X @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def __init__(self, pose_tuple=(SO3(), np.zeros((3, 1)))): """Constructs an SE(3) element. The default is the identity element. :param pose_tuple: A tuple (rotation (SO3), translation (3D column vector) (optional). """ self.rotation, self.translation = pose_tuple