def test_jacobian_right(): rho_vec = np.array([[1, 2, 3]]).T theta_vec = 3 * np.pi / 4 * np.array([[1, -1, 1]]).T / np.sqrt(3) xi_vec = np.vstack((rho_vec, theta_vec)) J_r = SE3.jac_right(xi_vec) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = SE3.Exp(xi_vec + delta) - (SE3.Exp(xi_vec) + J_r @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)
def test_jacobian_left(): rho_vec = np.array([[2, 1, 2]]).T theta_vec = np.pi / 4 * np.array([[-1, -1, -1]]).T / np.sqrt(3) xi_vec = np.vstack((rho_vec, theta_vec)) J_l = SE3.jac_left(xi_vec) # Should have J_l(xi_vec) == J_r(-xi_vec). np.testing.assert_almost_equal(J_l, SE3.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((6, 1)) taylor_diff = SE3.Log(SE3.Exp(xi_vec + delta) @ (SE3.Exp(J_l @ delta) @ SE3.Exp(xi_vec)).inverse()) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)
def left_right_perturbations(): # Define the pose of a camera. T_w_c = SE3((SO3.from_roll_pitch_yaw(5 * np.pi / 4, 0, np.pi / 2), np.array([[2, 2, 2]]).T)) # Perturbation. xsi_vec = np.array([[2, 0, 0, 0, 0, 0]]).T # Perform the perturbation on the right (locally). T_r = T_w_c @ SE3.Exp(xsi_vec) # Perform the perturbation on the left (globally). T_l = SE3.Exp(xsi_vec) @ T_w_c # We can transform the tangent vector between local and global frames using the adjoint matrix. xsi_vec_w = T_w_c.adjoint( ) @ xsi_vec # When xsi_vec is used on the right side. xsi_vec_c = T_w_c.inverse().adjoint( ) @ xsi_vec # When xsi_vec is used on the left side. print('xsi_vec_w: ', xsi_vec_w.flatten()) print('xsi_vec_c: ', xsi_vec_c.flatten()) # Visualise the perturbations: # Use Qt 5 backend in visualisation, use latex. 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 frames. to_right = np.hstack((T_w_c.translation, T_r.translation)) ax.plot(to_right[0, :], to_right[1, :], to_right[2, :], 'k:') to_left = np.hstack((T_w_c.translation, T_l.translation)) ax.plot(to_left[0, :], to_left[1, :], to_left[2, :], 'k:') vg.plot_pose(ax, SE3().to_tuple(), text='$\mathcal{F}_w$') vg.plot_pose(ax, T_w_c.to_tuple(), text='$\mathcal{F}_c$') vg.plot_pose(ax, T_r.to_tuple(), text=r'$\mathbf{T}_{wc} \circ \mathrm{Exp}(\mathbf{\xi})$') vg.plot_pose(ax, T_l.to_tuple(), text=r'$\mathrm{Exp}(\mathbf{\xi}) \circ \mathbf{T}_{wc}$') # Show figure. vg.plot.axis_equal(ax) plt.show()
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_log_of_exp(): rho_vec = np.array([[1, 2, 3]]).T theta_vec = 2 * np.pi / 3 * np.array([[2, 1, -1]]).T / np.sqrt(6) xi_vec = np.vstack((rho_vec, theta_vec)) X = SE3.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, 3]]).T theta_vec = np.zeros((3, 1)) xi_vec = np.vstack((rho_vec, theta_vec)) X = SE3.Exp(xi_vec) np.testing.assert_equal(X.rotation.matrix, np.identity(3)) np.testing.assert_equal(X.translation, rho_vec)
def draw_exp(ax, xi_vec, draw_options): vg.plot_pose(ax, SE3().to_tuple(), scale=1, text='$\mathcal{F}_w$') T_l = SE3.Exp(xi_vec) vg.plot_pose(ax, T_l.to_tuple()) #, text=r'$\mathrm{Exp}(\mathbf{\xi})$') if draw_options['Draw box']: box_points = vg.utils.generate_box(pose=T_l.to_tuple(), scale=1) vg.utils.plot_as_box(ax, box_points) if draw_options['Draw manifold\ntrajectory']: draw_interpolated(ax, SE3(), xi_vec, SE3())
def draw_left_perturbation(ax, T_w_a, xi_vec, draw_options): vg.plot_pose(ax, SE3().to_tuple(), scale=1, text='$\mathcal{F}_w$') vg.plot_pose(ax, T_w_a.to_tuple(), scale=1, text='$\mathcal{F}_a$') T_l = SE3.Exp(xi_vec) @ T_w_a vg.plot_pose(ax, T_l.to_tuple( )) #, text=r'$\mathrm{Exp}(\mathbf{\xi}) \circ \mathbf{T}_{wa}$') if draw_options['Draw box']: box_points = vg.utils.generate_box(pose=T_l.to_tuple(), scale=1) vg.utils.plot_as_box(ax, box_points) if draw_options['Draw manifold\ntrajectory']: draw_interpolated(ax, SE3(), xi_vec, T_w_a)
def test_jacobian_right_inverse(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) xi_vec = X.Log() J_r_inv = SE3.jac_right_inverse(xi_vec) # Should have J_l * J_r_inv = Exp(xi_vec).adjoint(). J_l = SE3.jac_left(xi_vec) np.testing.assert_almost_equal(J_l @ J_r_inv, SE3.Exp(xi_vec).adjoint(), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).Log() - (X.Log() + J_r_inv @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)
def test_jacobian_X_oplus_tau_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)) rho_vec = np.array([[2, 1, 2]]).T theta_vec = np.pi / 4 * np.array([[-1, -1, -1]]).T / np.sqrt(3) xi_vec = np.vstack((rho_vec, theta_vec)) 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(SE3.Exp(xi_vec).adjoint()), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 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((6, 1)), 14)
def test_jacobian_left_inverse(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) xi_vec = X.Log() J_l_inv = SE3.jac_left_inverse(xi_vec) # Should have that left and right are block transposes of each other. J_r_inv = SE3.jac_right_inverse(xi_vec) np.testing.assert_almost_equal(J_l_inv[:3, :3], J_r_inv[:3, :3].T, 14) np.testing.assert_almost_equal(J_l_inv[:3, 3:], J_r_inv[:3, 3:].T, 14) np.testing.assert_almost_equal(J_l_inv[3:, 3:], J_r_inv[3:, 3:].T, 14) # Test the Jacobian numerically (using Exps and Logs, since left oplus and ominus have not been defined). delta = 1e-3 * np.ones((6, 1)) taylor_diff = (SE3.Exp(delta) @ X).Log() - (X.Log() + J_l_inv @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)
import numpy as np import visgeom as vg import matplotlib import matplotlib.pyplot as plt from pylie import SO3, SE3 # Define the pose of a camera. T_w_c = SE3((SO3.from_roll_pitch_yaw(5 * np.pi / 4, 0, np.pi / 2), np.array([[2, 2, 2]]).T)) # Perturbation. xsi_vec = np.array([[2, 0, 0, 0, 0, 0]]).T # Perform the perturbation on the right (locally). T_r = T_w_c @ SE3.Exp(xsi_vec) # Perform the perturbation on the left (globally). T_l = SE3.Exp(xsi_vec) @ T_w_c # We can transform the tangent vector between local and global frames using the adjoint matrix. xsi_vec_w = T_w_c.adjoint( ) @ xsi_vec # When xsi_vec is used on the right side. xsi_vec_c = T_w_c.inverse().adjoint( ) @ xsi_vec # When xsi_vec is used on the left side. print('xsi_vec_w: ', xsi_vec_w.flatten()) print('xsi_vec_c: ', xsi_vec_c.flatten()) # Visualise the perturbations: # Use Qt 5 backend in visualisation, use latex. matplotlib.use('qt5agg')
def draw_interpolated(ax, T_1, xi, T_2): for alpha in np.linspace(0, 1, 20): T = T_1 @ SE3.Exp(alpha * xi) @ T_2 vg.plot_pose(ax, T.to_tuple(), alpha=0.1)