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)