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)