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 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_vee():
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))
xi_hat = SE3.hat(xi_vec)
np.testing.assert_equal(SE3.vee(xi_hat), xi_vec)
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_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_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_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 _pose_to_Qt(pose: SE3):
if isinstance(pose, SO3):
mat = np.eye(4)
mat[0:3, 0:3] = pose.as_matrix()
else:
mat = pose.as_matrix()
Q = mat[0:3, 0:3]
t = mat[0:3, 3:4]
return Q, t
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_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_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 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_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_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_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_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_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 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_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 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 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 read_trajectory(fname: str,
format_spec: str,
min_time: float = -1.0) -> Trajectory:
times = []
poses = []
scol = 0
if format_spec[0].isdigit():
scol = int(format_spec[0])
format_spec = format_spec[1:]
time_scale = 1.0
if format_spec[0] == 'n':
time_scale = 1.0e-9
format_spec = format_spec[1:]
with open(fname, 'r') as file:
reader = csv.reader(file)
# Skip header
next(reader)
for line in reader:
t = float(line[scol + 0]) * time_scale
if t < 0:
continue
if min_time > 0 and t < min_time:
continue
pose = SE3.from_list(line[scol + 1:], format_spec)
times.append(t)
poses.append(pose)
return Trajectory(poses, times)
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_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_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_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)
