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_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_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_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_to_tuple():
so3 = SO3.from_roll_pitch_yaw(np.pi / 3, -np.pi / 2, -3 * np.pi / 2)
t = np.array([[-1, 1, 3]]).T
se3 = SE3((so3, t))
pose_tuple = se3.to_tuple()
np.testing.assert_equal(pose_tuple[0], so3.matrix)
np.testing.assert_equal(pose_tuple[1], t)
def test_composition_with_identity():
X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T))
comp_with_identity = X.compose(SE3())
comp_from_identity = SE3().compose(X)
np.testing.assert_almost_equal(comp_with_identity.to_matrix(), X.to_matrix(), 14)
np.testing.assert_almost_equal(comp_from_identity.to_matrix(), X.to_matrix(), 14)
def test_to_matrix():
so3 = SO3.from_roll_pitch_yaw(np.pi / 4, np.pi / 2, 3 * np.pi / 2)
t = np.array([[3, 2, 1]]).T
se3 = SE3((so3, t))
T = se3.to_matrix()
np.testing.assert_equal(T[0:3, 0:3], so3.matrix)
np.testing.assert_equal(T[0:3, 3:], t)
np.testing.assert_equal(T[3:, :], np.array([[0, 0, 0, 1]]))
def test_inverse():
X = SE3((SO3.from_roll_pitch_yaw(np.pi / 4, np.pi / 2, 3 * np.pi / 2), np.array([[1, -2, 3]]).T))
X_inv = X.inverse()
np.testing.assert_equal(X_inv.rotation.matrix, X.rotation.inverse().matrix)
np.testing.assert_equal(X_inv.translation, -(X.rotation.inverse() * X.translation))
np.testing.assert_almost_equal((X @ X_inv).to_matrix(), np.identity(4))
def test_construct_with_matrix():
so3 = SO3.from_roll_pitch_yaw(np.pi / 10, np.pi / 4, -np.pi / 2)
t = np.array([[1, 2, 3]]).T
T = np.block([[so3.matrix, t], [0, 0, 0, 1]])
se3 = SE3.from_matrix(T)
np.testing.assert_almost_equal(se3.rotation.matrix, so3.matrix, 14)
np.testing.assert_equal(se3.translation, t)
def test_construct_with_roll_pitch_yaw():
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(np.pi / 2, 0, 0).matrix,
SO3.rot_x(np.pi / 2).matrix, 14)
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(0, np.pi / 2, 0).matrix,
SO3.rot_y(np.pi / 2).matrix, 14)
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(0, 0, np.pi / 2).matrix,
SO3.rot_z(np.pi / 2).matrix, 14)
roll = np.pi
pitch = np.pi / 2
yaw = np.pi
so3 = SO3.from_roll_pitch_yaw(roll, pitch, yaw)
expected = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
np.testing.assert_almost_equal(so3.matrix, expected, 14)
def test_ominus_with_oplus_diff():
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([[4, 3, 2]]).T
theta_vec = 2 * np.pi / 3 * np.array([[2, 1, 1]]).T / np.sqrt(6)
xi_vec = np.vstack((rho_vec, theta_vec))
Y = X.oplus(xi_vec)
xi_vec_diff = Y.ominus(X)
np.testing.assert_almost_equal(xi_vec_diff, xi_vec, 14)
def test_ominus_with_oplus_diff_with_operators():
X = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T))
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))
Y = X + xi_vec
xi_vec_diff = Y - X
np.testing.assert_almost_equal(xi_vec_diff, xi_vec, 14)
def test_jacobian_inverse():
X = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T))
J_inv = X.jac_inverse_X_wrt_X()
# Jacobian should be -X.adjoint().
np.testing.assert_equal(J_inv, -X.adjoint())
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((6, 1))
taylor_diff = X.oplus(delta).inverse() - X.inverse().oplus(J_inv @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 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()
# Jacobian should be R.
np.testing.assert_array_equal(J_action_x, X.rotation.matrix)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.action(x + delta) - (X.action(x) + J_action_x @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 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 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_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_tau():
X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 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_tau = X.jac_X_oplus_tau_wrt_tau(xi_vec)
# Should be J_r.
np.testing.assert_equal(J_oplus_tau, X.jac_right(xi_vec))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((6, 1))
taylor_diff = X.oplus(xi_vec + delta) - X.oplus(xi_vec).oplus(J_oplus_tau @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 6)
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)
def vis_perturbations():
# Define the fixed frame "a" relative to the world frame "w".
T_w_a = SE3((SO3.from_roll_pitch_yaw(5 * np.pi / 4, 0,
np.pi / 2), np.array([[2, 2, 2]]).T))
# The vector xi represents a perturbation on the tangent vector space.
# We change the elements in this vector by using the sliders.
xi_vec = np.zeros([6, 1])
# We can choose to draw an oriented box around the perturbed pose,
# and we can draw the trajectory along the manifold (by interpolation).
draw_options = {'Draw box': False, 'Draw manifold\ntrajectory': True}
# Use Qt 5 backend in visualisation.
matplotlib.use('qt5agg')
# Create figure and axis.
fig, ax = plt.subplots(subplot_kw={'projection': '3d'})
plt.subplots_adjust(left=0.25)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
ax.margins(x=0)
# Add the widgets.
widget_color = 'lightgoldenrodyellow'
xi_sliders = [
Slider(plt.axes([0.05, 0.9 - 0.05 * i, 0.20, 0.03],
facecolor=widget_color),
r'$\xi_' + str(i + 1) + '$',
-4.0,
4.0,
valinit=xi_vec[i].item(),
valstep=0.01) for i in range(6)
]
button = Button(plt.axes([0.1, 0.55, 0.1, 0.04]),
'Reset',
color=widget_color,
hovercolor='0.975')
check = CheckButtons(
plt.axes([0.025, 0.10, 0.24, 0.15], facecolor=widget_color),
draw_options.keys(), draw_options.values())
radio = RadioButtons(
plt.axes([0.025, 0.3, 0.24, 0.2], facecolor=widget_color),
(r'1. $\mathrm{Exp}(\mathbf{\xi})$',
r'2. $\mathbf{T}_{wc} \circ \mathrm{Exp}(\mathbf{\xi})$',
r'3. $\mathrm{Exp}(\mathbf{\xi}) \circ \mathbf{T}_{wc}$'),
active=0)
# Setup the update callback, which is called by the sliders and the radio buttons.
def update(val):
ax.clear()
for i, slider in enumerate(xi_sliders):
xi_vec[i] = slider.val
if radio.value_selected[0] == '1':
draw_exp(ax, xi_vec, draw_options)
elif radio.value_selected[0] == '2':
draw_right_perturbation(ax, T_w_a, xi_vec, draw_options)
else:
draw_left_perturbation(ax, T_w_a, xi_vec, draw_options)
ax.set_xlim(-4, 4)
ax.set_ylim(-4, 4)
ax.set_zlim(-4, 4)
vg.plot.axis_equal(ax)
fig.canvas.draw_idle()
for slider in xi_sliders:
slider.on_changed(update)
radio.on_clicked(update)
# Setup the check buttons to update the "draw options".
def update_draw_options(label):
draw_options[label] = not draw_options[label]
update([])
check.on_clicked(update_draw_options)
# Setup the reset callback, used by the reset button.
def reset(event):
for slider in xi_sliders:
slider.reset()
button.on_clicked(reset)
# Start with first update.
update([])
plt.show()
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 poses_and_cameras():
# 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 pose of the world North-East-Down (NED) frame (relative to the world frame).
T_w_w = SE3()
vg.plot_pose(ax, T_w_w.to_tuple(), scale=3, text='$\mathcal{F}_w$')
# Plot the body frame (a body-fixed Forward-Right-Down (FRD) frame).
roll = np.radians(-10)
pitch = np.radians(0)
yaw = np.radians(135)
t_w_b = np.array([[-10, -10, -2]]).T
T_w_b = SE3((SO3.from_roll_pitch_yaw(roll, pitch, yaw), t_w_b))
vg.plot_pose(ax, T_w_b.to_tuple(), scale=3, text='$\mathcal{F}_b$')
# Plot the camera frame.
# The camera is placed 2 m directly above the body origin.
# Its optical axis points to the left (in opposite direction of the y-axis in F_b).
# Its y-axis points downwards along the z-axis of F_b.
R_b_c = np.array([[1, 0, 0],
[0, 0, -1],
[0, 1, 0]])
t_b_c = np.array([[0, 0, -2]]).T
T_b_c = SE3((SO3(R_b_c), t_b_c))
T_w_c = T_w_b @ T_b_c
vg.plot_pose(ax, T_w_c.to_tuple(), scale=3, text='$\mathcal{F}_c$')
# Plot obstacle frame.
# The cube is placed at (North: 10 m, East: 10 m, Down: -1 m).
# Its top points upwards, and its front points south.
R_w_o = np.array([[-1, 0, 0],
[0, 1, 0],
[0, 0, -1]])
t_w_o = np.array([[10, 10, -1]]).T
T_w_o = SE3((SO3(R_w_o), t_w_o))
vg.plot_pose(ax, T_w_o.to_tuple(), scale=3, text='$\mathcal{F}_o$')
# Plot the cube with sides 3 meters.
points_o = vg.utils.generate_box(scale=3)
points_w = T_w_o * points_o
vg.utils.plot_as_box(ax, points_w)
# Plot the image plane.
img_plane_scale = 3
K = np.array([[50, 0, 40],
[0, 50, 30],
[0, 0, 1]])
vg.plot_camera_image_plane(ax, K, T_w_c.to_tuple(), scale=img_plane_scale)
# Project the box onto the normalised image plane (at z=img_plane_scale).
points_c = T_w_c.inverse() @ T_w_o * points_o
xn = points_c / points_c[2, :]
xn_w = T_w_c * (img_plane_scale * xn)
vg.utils.plot_as_box(ax, xn_w)
# Show figure.
vg.plot.axis_equal(ax)
ax.invert_zaxis()
ax.invert_yaxis()
plt.show()
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
# Plot the pose of the world North-East-Down (NED) frame (relative to the world frame).
T_w_w = SE3()
vg.plot_pose(ax, T_w_w.to_tuple(), scale=3, text='$\mathcal{F}_w$')
# Plot the body frame (a body-fixed Forward-Right-Down (FRD) frame).
roll = np.radians(-10)
pitch = np.radians(0)
yaw = np.radians(135)
t_w_b = np.array([[-10, -10, -2]]).T
T_w_b = SE3((SO3.from_roll_pitch_yaw(roll, pitch, yaw), t_w_b))
vg.plot_pose(ax, T_w_b.to_tuple(), scale=3, text='$\mathcal{F}_b$')
# Plot the camera frame.
# The camera is placed 2 m directly above the body origin.
# Its optical axis points to the left (in opposite direction of the y-axis in F_b).
# Its y-axis points downwards along the z-axis of F_b.
R_b_c = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
t_b_c = np.array([[0, 0, -2]]).T
T_b_c = SE3((SO3(R_b_c), t_b_c))
T_w_c = T_w_b @ T_b_c
vg.plot_pose(ax, T_w_c.to_tuple(), scale=3, text='$\mathcal{F}_c$')
# Plot obstacle frame.
# The cube is placed at (North: 10 m, East: 10 m, Down: -1 m).
# Its top points upwards, and its front points south.
