def test_jacobian_right(): theta_vec = 3 * np.pi / 4 * np.array([[1, -1, 1]]).T / np.sqrt(3) J_r = SO3.jac_right(theta_vec) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = SO3.Exp(theta_vec + delta) - (SO3.Exp(theta_vec) + J_r @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_jacobian_right_inverse(): theta_vec = 3 * np.pi / 4 * np.array([[1, -1, 0]]).T / np.sqrt(2) X = SO3.Exp(theta_vec) J_r_inv = SO3.jac_right_inverse(theta_vec) # Should have J_l * J_r_inv = Exp(theta_vec).adjoint(). J_l = SO3.jac_left(theta_vec) np.testing.assert_almost_equal(J_l @ J_r_inv, SO3.Exp(theta_vec).adjoint(), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = X.oplus(delta).Log() - (X.Log() + J_r_inv @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_exp_for_vector_close_to_zero_norm_returns_identity(): theta = 1e-14 u_vec = np.array([[0, 1, 0]]).T so3 = SO3.Exp(theta * u_vec) np.testing.assert_array_equal(so3.matrix, np.identity(3))
def test_to_rotation_matrix_results_in_close_rotation(): angle = 0.5 * np.pi axis = np.array([[1, 0, 0]]).T R = SO3.Exp(angle * axis).matrix # Invalidate a valid rotation matrix by scaling it. R_scaled = 3 * R # Fit to SO(3). R_closest = SO3.to_so3_matrix(R_scaled) # Result should be the same rotation matrix. np.testing.assert_almost_equal(R_closest, R, 14) # Perturb the rotation matrix with random noise. R_noisy = R + 0.01 * np.random.rand(3, 3) # Fit to SO(3) so3_closest = SO3(R_noisy) # Extract angle-axis representation. angle_closest, axis_closest = so3_closest.Log(True) # Result should be close to the same rotation. np.testing.assert_almost_equal(angle_closest, angle, 2) np.testing.assert_almost_equal(axis_closest, axis, 2)
def test_difference_for_simple_rotation_with_operator_works(): X = SO3() theta_vec = 3 * np.pi / 4 * np.array([[1, 0, -1]]).T / np.sqrt(2) Y = SO3.Exp(theta_vec) theta_vec_diff = Y - X np.testing.assert_almost_equal(theta_vec_diff, theta_vec, 14)
def test_difference_for_simple_rotation_works(): X = SO3() theta_vec = np.pi / 3 * np.array([[0, 1, 0]]).T Y = SO3.Exp(theta_vec) theta_vec_diff = Y.ominus(X) np.testing.assert_array_equal(theta_vec_diff, theta_vec)
def test_construct_with_angle_axis(): angle = 3 * np.pi / 4 axis = np.array([[1, 1, -1]]) / np.sqrt(3) theta_vec = angle * axis so3 = SO3.from_angle_axis(angle, axis) np.testing.assert_array_equal(so3.matrix, SO3.Exp(theta_vec).matrix)
def test_log_returns_correct_angle_axis(): theta = np.pi / 4 u_vec = np.array([[1, 0, -1]]).T / np.sqrt(2) so3 = SO3.Exp(theta * u_vec) theta_log, u_vec_log = so3.Log(True) np.testing.assert_almost_equal(theta_log, theta, 14) np.testing.assert_almost_equal(u_vec_log, u_vec, 14)
def test_log_returns_correct_theta_vec(): theta = np.pi / 6 u_vec = np.array([[-1, 1, 1]]).T / np.sqrt(3) theta_vec = theta * u_vec so3 = SO3.Exp(theta_vec) theta_vec_log = so3.Log() np.testing.assert_almost_equal(theta_vec_log, theta_vec, 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_exp_returns_correct_rotation(): theta = 0.5 * np.pi u_vec = np.array([[0, 1, 0]]).T so3 = SO3.Exp(theta * u_vec) np.testing.assert_almost_equal(so3.matrix[:, 0:1], np.array([[0, 0, -1]]).T, 14) np.testing.assert_almost_equal(so3.matrix[:, 1:2], np.array([[0, 1, 0]]).T, 14) np.testing.assert_almost_equal(so3.matrix[:, 2:3], np.array([[1, 0, 0]]).T, 14)
def test_jacobian_X_oplus_tau_wrt_X(): X = SO3.from_angle_axis(3 * np.pi / 4, np.array([[1, -1, 1]]).T / np.sqrt(3)) theta_vec = np.pi / 4 * np.array([[-1, -1, 1]]).T / np.sqrt(3) J_oplus_X = X.jac_X_oplus_tau_wrt_X(theta_vec) # Should be Exp(tau).adjoint().inverse() np.testing.assert_almost_equal(J_oplus_X, np.linalg.inv(SO3.Exp(theta_vec).adjoint()), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = X.oplus(delta).oplus(theta_vec) - X.oplus(theta_vec).oplus( J_oplus_X @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)