def test_jacobian_right():
rho_vec = np.array([[1, 2]]).T
theta = 3 * np.pi / 4
xi_vec = np.vstack((rho_vec, theta))
J_r = SE2.jac_right(xi_vec)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = SE2.Exp(xi_vec + delta) - (SE2.Exp(xi_vec) + J_r @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_jacobian_left():
rho_vec = np.array([[2, 1]]).T
theta = np.pi / 4
xi_vec = np.vstack((rho_vec, theta))
J_l = SE2.jac_left(xi_vec)
# Should have J_l(xi_vec) == J_r(-xi_vec).
np.testing.assert_almost_equal(J_l, SE2.jac_right(-xi_vec), 14)
# Test the Jacobian numerically (using Exps and Logs, since left oplus and ominus have not been defined).
delta = 1e-3 * np.ones((3, 1))
taylor_diff = SE2.Log(
SE2.Exp(xi_vec + delta)
@ (SE2.Exp(J_l @ delta) @ SE2.Exp(xi_vec)).inverse())
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_log_of_exp():
rho_vec = np.array([[2, 3]]).T
theta = 2 * np.pi / 3
xi_vec = np.vstack((rho_vec, theta))
X = SE2.Exp(xi_vec)
np.testing.assert_almost_equal(X.Log(), xi_vec, 14)
def test_exp_with_no_rotation():
rho_vec = np.array([[1, 2]]).T
theta = 0.0
xi_vec = np.vstack((rho_vec, theta))
X = SE2.Exp(xi_vec)
np.testing.assert_equal(X.rotation.to_matrix(), np.identity(2))
np.testing.assert_equal(X.translation, rho_vec)
def test_jacobian_left_inverse():
X = SE2((SO2(np.pi / 8), np.array([[2, 1]]).T))
xi_vec = X.Log()
J_l_inv = SE2.jac_left_inverse(xi_vec)
# Test the Jacobian numerically (using Exps and Logs, since left oplus and ominus have not been defined).
delta = 1e-3 * np.ones((3, 1))
taylor_diff = (SE2.Exp(delta) @ X).Log() - (X.Log() + J_l_inv @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_jacobian_right_inverse():
X = SE2((SO2(np.pi / 8), np.array([[1, 1]]).T))
xi_vec = X.Log()
J_r_inv = SE2.jac_right_inverse(xi_vec)
# Should have J_l * J_r_inv = Exp(xi_vec).adjoint().
J_l = SE2.jac_left(xi_vec)
np.testing.assert_almost_equal(J_l @ J_r_inv,
SE2.Exp(xi_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_jacobian_X_oplus_tau_wrt_X():
X = SE2((SO2(np.pi / 10), np.array([[3, 2]]).T))
rho_vec = np.array([[1, 2]]).T
theta = np.pi / 4
xi_vec = np.vstack((rho_vec, theta))
J_oplus_X = X.jac_X_oplus_tau_wrt_X(xi_vec)
# Should be Exp(tau).adjoint().inverse()
np.testing.assert_almost_equal(J_oplus_X,
np.linalg.inv(SE2.Exp(xi_vec).adjoint()),
14)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.oplus(delta).oplus(xi_vec) - X.oplus(xi_vec).oplus(
J_oplus_X @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_exp_of_log():
X = SE2((SO2(np.pi / 10), np.array([[2, 1]]).T))
xi_vec = X.Log()
np.testing.assert_almost_equal(
SE2.Exp(xi_vec).to_matrix(), X.to_matrix(), 14)