def jac_action_Xx_wrt_X(X, x):
"""Computes the Jacobian of the action X.action(x) with respect to the element X.
:param x: The 3D column vector x.
:return: The Jacobian (6x3 matrix)
"""
return np.block([[X.rotation.matrix, -(np.matmul(X.rotation.matrix, SO3.hat(x)))]])
def adjoint(self):
"""The adjoint at the element.
:return: The adjoint, a 6x6 matrix.
"""
R = self.rotation.matrix
return np.block([[R, SO3.hat(self.translation) @ R],
[np.zeros((3, 3)), R]])
def test_hat():
rho_vec = np.array([[1, 2, 3]]).T
theta_vec = np.pi / 3 * np.array([[1, -1, 2]]).T / np.sqrt(6)
xi_vec = np.vstack((rho_vec, theta_vec))
xi_hat_expected = np.block([[SO3.hat(theta_vec), rho_vec],
[np.zeros((1, 4))]])
np.testing.assert_equal(SE3.hat(xi_vec), xi_hat_expected)
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 jac_composition_XY_wrt_X(Y):
"""Computes the Jacobian of the composition X.compose(Y) with respect to the element X.
:param Y: SE3 element Y
:return: The Jacobian (6x6 matrix)
"""
R_Y_inv = Y.rotation.inverse().matrix
return np.block([[R_Y_inv, -(R_Y_inv @ SO3.hat(Y.translation))],
[np.zeros((3, 3)), R_Y_inv]])
def hat(xi_vec):
"""Performs the hat operator on the tangent space vector xi_vec,
which returns the corresponding Lie Algebra matrix xi_hat.
:param xi_vec: 6d tangent space column vector xi_vec = [rho_vec, theta_vec]^T.
:return: The Lie Algebra (4x4 matrix).
"""
return np.block([[SO3.hat(xi_vec[3:]), xi_vec[:3]], [np.zeros(
(1, 4))]])
def _Q_left(xi_vec):
rho_vec = xi_vec[:3]
theta_vec = xi_vec[3:]
theta = np.linalg.norm(theta_vec)
if theta < 1e-10:
return np.zeros((3, 3))
rho_hat = SO3.hat(rho_vec)
theta_hat = SO3.hat(theta_vec)
return 0.5 * rho_hat + ((theta - np.sin(theta)) / theta ** 3) * \
(theta_hat @ rho_hat + rho_hat @ theta_hat + theta_hat @ rho_hat @ theta_hat) - \
((1 - 0.5 * theta ** 2 - np.cos(theta)) / theta ** 4) * \
(theta_hat @ theta_hat @ rho_hat + rho_hat @ theta_hat @ theta_hat -
3 * theta_hat @ rho_hat @ theta_hat) - \
0.5 * ((1 - 0.5 * theta ** 2 - np.cos(theta)) / theta ** 4 - 3 *
((theta - np.sin(theta) - (theta ** 3 / 6)) / theta ** 5)) * \
(theta_hat @ rho_hat @ theta_hat @ theta_hat + theta_hat @ theta_hat @ rho_hat @ theta_hat)
def test_hat_returns_skew_symmetric_matrix():
theta_vec = np.array([[1, 2, 3]]).T
theta_hat = SO3.hat(theta_vec)
assert theta_hat[0, 0] == 0
assert theta_hat[0, 1] == -theta_vec[2]
assert theta_hat[0, 2] == theta_vec[1]
assert theta_hat[1, 0] == theta_vec[2]
assert theta_hat[1, 1] == 0
assert theta_hat[1, 2] == -theta_vec[0]
assert theta_hat[2, 0] == -theta_vec[1]
assert theta_hat[2, 1] == theta_vec[0]
assert theta_hat[2, 2] == 0
def test_jacobian_action_Xx_wrt_X():
X = SO3.from_angle_axis(3 * np.pi / 4,
np.array([[1, -1, 1]]).T / np.sqrt(3))
x = np.array([[1, 2, 3]]).T
J_action_X = X.jac_action_Xx_wrt_X(x)
# Jacobian should be -X.matrix * SO3.hat(x).
np.testing.assert_array_equal(J_action_X, -X.matrix @ SO3.hat(x))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.oplus(delta).action(x) - (X.action(x) + J_action_X @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
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(x)
# Jacobian should be [R, - R*SO3.hat(x)].
np.testing.assert_array_equal(
J_action_X,
np.block([[X.rotation.matrix, -(X.rotation.matrix @ SO3.hat(x))]]))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((6, 1))
taylor_diff = X.oplus(delta).action(x) - (X.action(x) + J_action_X @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def Log(self):
"""Computes the tangent space vector xi_vec at the current element X.
:return: The tangent space vector xi_vec = [rho_vec, theta_vec]^T.
"""
theta, u_vec = self.rotation.Log(split_angle_axis=True)
if theta == 0:
return np.vstack((self.translation, np.zeros((3, 1))))
theta_vec = theta * u_vec
a = np.sin(theta) / theta
b = (1 - np.cos(theta)) / (theta**2)
theta_hat = SO3.hat(theta_vec)
V_inv = np.identity(3) - 0.5 * theta_hat + np.linalg.matrix_power(
theta_hat, 2) * (1 - a / (2 * b)) / (theta**2)
rho_vec = V_inv @ self.translation
return np.vstack((rho_vec, theta_vec))
def test_vee_extracts_correct_vector_from_skew_symmetric_matrix():
theta_vec = np.array([[3, 2, 1]]).T
theta_hat = SO3.hat(theta_vec)
theta_hat_vee = SO3.vee(theta_hat)
np.testing.assert_array_equal(theta_hat_vee, theta_vec)