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)