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
