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 initialize_map(frame_0, frame_1):
# Compute relative pose from two-view geometry.
kp_0, id_0, kp_1, id_1 = frame_0.extract_correspondences_with_frame(
frame_1)
pose_0_1 = estimate_two_view_relative_pose(frame_0, kp_0, frame_1, kp_1)
# Triangulate points.
P_0 = frame_0.camera_model().projection_matrix(SE3())
P_1 = frame_1.camera_model().projection_matrix(pose_0_1.inverse())
points_0 = triangulate_points_from_two_views(P_0, kp_0, P_1, kp_1)
# Add first keyframe as reference frame.
sfm_map = SfmMap()
kf_0 = Keyframe(frame_0, SE3())
sfm_map.add_keyframe(kf_0)
# Add second keyframe from relative pose.
kf_1 = Keyframe(frame_1, pose_0_1)
sfm_map.add_keyframe(kf_1)
# Add triangulated points as map points relative to reference frame.
num_matches = len(id_0)
for i in range(num_matches):
map_point = MapPoint(i, points_0[:, [i]])
map_point.add_observation(kf_0, id_0[i])
map_point.add_observation(kf_1, id_1[i])
sfm_map.add_map_point(map_point)
return sfm_map
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_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_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():
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 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 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_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 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_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_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 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 compute_mean_pose(poses, conv_thresh=1e-14, max_iters=20):
"""Compute the mean pose from an array of poses.
:param poses: An array of SE3 objects
:param conv_thresh: The convergence threshold
:param max_iters: The maximum number of iterations
:return: The estimate of the mean pose
"""
num_poses = len(poses)
# Initialise mean pose.
mean_pose = poses[0]
for it in range(max_iters):
# TODO 2: Compute the mean tangent vector in the tangent space at the current estimate.
mean_xsi = np.zeros((6, 1)) # Mock implementation, do something more!
# TODO 3: Update the estimate.
mean_pose = SE3() # Mock implementation, do something else!
# Stop if the update is small.
if np.linalg.norm(mean_xsi) < conv_thresh:
break
return mean_pose
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_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 main():
# Define the pose distribution.
mean_pose = SE3()
cov_pose = np.diag(np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.4])**2)
# Draw random poses from this distribution.
poses = draw_random_poses(mean_pose, cov_pose)
# Estimate the mean pose from the random poses.
estimated_mean_pose = compute_mean_pose(poses)
print(estimated_mean_pose.to_matrix())
# Plot result.
# 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 each of the randomly drawn poses.
for pose in poses:
vg.plot_pose(ax, pose.to_tuple(), alpha=0.05)
# Plot the estimated mean pose.
vg.plot_pose(ax, estimated_mean_pose.to_tuple())
# Show figure.
vg.plot.axis_equal(ax)
plt.show()
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_action_on_vectors():
unit_x = np.array([[1, 0, 0]]).T
unit_z = np.array([[0, 0, 1]]).T
t = np.array([[3, 2, 1]]).T
X = SE3((SO3.from_angle_axis(np.pi / 2, np.array([[0, 1, 0]]).T), t))
np.testing.assert_almost_equal(X.action(unit_x), -unit_z + t, 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 main():
# World box.
points_w = vg.utils.generate_box()
# True observer pose.
true_pose_wo = SE3((SO3.rot_z(np.pi), np.array([[3, 0, 0]]).T))
# Observed box.
points_o = vg.utils.generate_box(pose=true_pose_wo.inverse().to_tuple())
# Perturb observer pose and use as initial state.
init_pose_wo = true_pose_wo + np.random.randn(6, 1)
# Estimate pose in the world frame from point correspondences.
model = NoiseFreePointAlignmentBasedPoseEstimatorObjective(
points_w, points_o)
x, cost, A, b = gauss_newton(init_pose_wo, model)
# Visualize (press a key to jump to the next iteration).
# 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 box and true state
vg.plot_pose(ax, true_pose_wo.to_tuple(), scale=1, alpha=0.4)
vg.utils.plot_as_box(ax, points_w, alpha=0.4)
# Plot initial state (to run axis equal first time).
ax.set_title('Cost: ' + str(cost[0]))
artists = vg.plot_pose(ax, x[0].to_tuple(), scale=1)
artists.extend(vg.utils.plot_as_box(ax, x[0] * points_o))
vg.plot.axis_equal(ax)
plt.draw()
while True:
if plt.waitforbuttonpress():
break
# Plot iterations
for i in range(1, len(x)):
for artist in artists:
artist.remove()
ax.set_title('Cost: ' + str(cost[i]))
artists = vg.plot_pose(ax, x[i].to_tuple(), scale=1)
artists.extend(vg.utils.plot_as_box(ax, x[i] * points_o))
plt.draw()
while True:
if plt.waitforbuttonpress():
break
def looks_at_pose(camera_pos_w: np.ndarray, target_pos_w: np.ndarray, up_vector_w: np.ndarray):
cam_to_target_w = target_pos_w - camera_pos_w
cam_z_w = cam_to_target_w.flatten() / np.linalg.norm(cam_to_target_w)
cam_to_right_w = np.cross(-up_vector_w.flatten(), cam_z_w)
cam_x_w = cam_to_right_w / np.linalg.norm(cam_to_target_w)
cam_y_w = np.cross(cam_z_w, cam_x_w)
return SE3((SO3(np.vstack((cam_x_w, cam_y_w, cam_z_w)).T), camera_pos_w))
def test_log_with_no_rotation():
X = SE3((SO3(), np.array([[-1, 2, -3]]).T))
xi_vec = X.Log()
# Translation part:
np.testing.assert_equal(xi_vec[:3], X.translation)
# Rotation part:
np.testing.assert_equal(xi_vec[3:], np.zeros((3, 1)))
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 estimate_pose_from_map_correspondences(frame: MatchedFrame, kp: np.ndarray,
points_w: np.ndarray):
# Estimate initial pose with a (new) PnP-method.
K = frame.camera_model().calibration_matrix()
_, theta_vec, t = cv2.solvePnP(points_w.T,
kp.T,
K,
None,
flags=cv2.SOLVEPNP_SQPNP)
pose_c_w = SE3((SO3.Exp(theta_vec), t.reshape(3, 1)))
return pose_c_w.inverse()
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)