|
import copy |
|
|
|
import numpy as np |
|
|
|
from liegroups.numpy import SO3 |
|
|
|
|
|
def test_identity(): |
|
T = SO3.identity() |
|
assert isinstance(T, SO3) |
|
|
|
|
|
def test_from_rpy_to_rpy(): |
|
r, p, y = np.pi / 4., np.pi / 3., np.pi |
|
test_r, test_p, test_y = SO3.from_rpy(r, p, y).to_rpy() |
|
assert np.isclose(test_r, r) |
|
assert np.isclose(test_p, p) |
|
assert np.isclose(test_y, y) |
|
|
|
|
|
def test_dot(): |
|
C = np.array([[0, 0, -1], |
|
[0, 1, 0], |
|
[1, 0, 0]]) |
|
C2 = C.dot(C) |
|
assert np.allclose((SO3(C).dot(SO3(C))).mat, C2) |
|
|
|
|
|
def test_rotx(): |
|
C_got = SO3.rotx(np.pi / 2) |
|
C_expected = np.array([[1, 0, 0], |
|
[0, 0, -1], |
|
[0, 1, 0]]) |
|
assert np.allclose(C_got.mat, C_expected) |
|
|
|
|
|
def test_roty(): |
|
C_got = SO3.roty(np.pi / 2) |
|
C_expected = np.array([[0, 0, 1], |
|
[0, 1, 0], |
|
[-1, 0, 0]]) |
|
assert np.allclose(C_got.mat, C_expected) |
|
|
|
|
|
def test_rotz(): |
|
C_got = SO3.rotz(np.pi / 2) |
|
C_expected = np.array([[0, -1, 0], |
|
[1, 0, 0], |
|
[0, 0, 1]]) |
|
assert np.allclose(C_got.mat, C_expected) |
|
|
|
|
|
def test_rpy(): |
|
r = np.pi / 12 |
|
p = np.pi / 6 |
|
y = np.pi / 3 |
|
C_got = SO3.from_rpy(r, p, y) |
|
C_expected = SO3.rotz(y).dot(SO3.roty(p).dot(SO3.rotx(r))) |
|
assert np.allclose(C_got.mat, C_expected.mat) |
|
|
|
|
|
def test_quaternion(): |
|
q1 = np.array([1, 0, 0, 0]) |
|
q2 = np.array([0, 1, 0, 0]) |
|
q3 = np.array([0, 0, 1, 0]) |
|
q4 = np.array([0, 0, 0, 1]) |
|
q5 = 0.5 * np.ones(4) |
|
q6 = -q5 |
|
|
|
assert np.allclose(SO3.from_quaternion(q1).to_quaternion(), q1) |
|
assert np.allclose(SO3.from_quaternion(q2).to_quaternion(), q2) |
|
assert np.allclose(SO3.from_quaternion(q3).to_quaternion(), q3) |
|
assert np.allclose(SO3.from_quaternion(q4).to_quaternion(), q4) |
|
assert np.allclose(SO3.from_quaternion(q5).to_quaternion(), q5) |
|
assert np.allclose(SO3.from_quaternion(q5).mat, |
|
SO3.from_quaternion(q6).mat) |
|
|
|
|
|
def test_wedge(): |
|
phi = [1, 2, 3] |
|
Phi = SO3.wedge(phi) |
|
phis = np.array([[1, 2, 3], [4, 5, 6]]) |
|
Phis = SO3.wedge(phis) |
|
assert np.array_equal(Phi, -Phi.T) |
|
assert np.array_equal(Phis[0, :, :], SO3.wedge(phis[0])) |
|
assert np.array_equal(Phis[1, :, :], SO3.wedge(phis[1])) |
|
|
|
|
|
def test_wedge_vee(): |
|
phi = [1, 2, 3] |
|
Phi = SO3.wedge(phi) |
|
phis = np.array([[1, 2, 3], [4, 5, 6]]) |
|
Phis = SO3.wedge(phis) |
|
assert np.array_equal(phi, SO3.vee(Phi)) |
|
assert np.array_equal(phis, SO3.vee(Phis)) |
|
|
|
|
|
def test_left_jacobians(): |
|
phi_small = [0., 0., 0.] |
|
phi_big = [np.pi / 2, np.pi / 3, np.pi / 4] |
|
|
|
left_jacobian_small = SO3.left_jacobian(phi_small) |
|
inv_left_jacobian_small = SO3.inv_left_jacobian(phi_small) |
|
assert np.allclose(left_jacobian_small.dot(inv_left_jacobian_small), |
|
np.identity(3)) |
|
|
|
left_jacobian_big = SO3.left_jacobian(phi_big) |
|
inv_left_jacobian_big = SO3.inv_left_jacobian(phi_big) |
|
assert np.allclose(left_jacobian_big.dot(inv_left_jacobian_big), |
|
np.identity(3)) |
|
|
|
|
|
def test_exp_log(): |
|
C = SO3.exp(np.pi * np.ones(3) / 4) |
|
assert np.allclose(SO3.exp(SO3.log(C)).mat, C.mat) |
|
|
|
|
|
def test_exp_log_zeros(): |
|
C = SO3.exp(np.zeros(3)) |
|
assert np.allclose(SO3.exp(SO3.log(C)).mat, C.mat) |
|
|
|
|
|
def test_perturb(): |
|
C = SO3.exp(np.pi * np.ones(3) / 4) |
|
C_copy = copy.deepcopy(C) |
|
phi = np.array([0.3, 0.2, 0.1]) |
|
C.perturb(phi) |
|
assert np.allclose(C.as_matrix(), (SO3.exp(phi).dot(C_copy)).as_matrix()) |
|
|
|
|
|
def test_normalize(): |
|
C = SO3.exp(np.pi * np.ones(3) / 4) |
|
C.mat += 0.1 |
|
C.normalize() |
|
assert SO3.is_valid_matrix(C.mat) |
|
|
|
|
|
def test_inv(): |
|
C = SO3.exp(np.pi * np.ones(3) / 4) |
|
assert np.allclose((C.dot(C.inv())).mat, np.identity(3)) |
|
|
|
|
|
def test_adjoint(): |
|
C = SO3.exp(np.pi * np.ones(3) / 4) |
|
assert np.allclose(C.adjoint(), C.mat) |
|
|
|
|
|
def test_transform_vectorized(): |
|
C = SO3.exp(np.pi * np.ones(3) / 4) |
|
pt1 = np.array([1, 2, 3]) |
|
pt2 = np.array([4, 5, 3]) |
|
pts = np.array([pt1, pt2]) |
|
Cpt1 = C.dot(pt1) |
|
Cpt2 = C.dot(pt2) |
|
Cpts = C.dot(pts) |
|
assert( |
|
np.allclose(Cpt1, Cpts[0]) |
|
and np.allclose(Cpt2, Cpts[1]) |
|
) |
|
|
