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)
