def testBoxMinusR(self):
R1 = SO3.random()
R2 = SO3.random()
w = R1.boxminusr(R2)
R_res = R2.boxplusr(w)
np.testing.assert_allclose(R1.R, R_res.R)
def test_right_jacobian_of_composition(self):
R1 = SO3.random()
R2 = SO3.random()
R3, Jr = R1.compose(R2, Jr=np.eye(3))
Jr_true = np.linalg.inv(R2.Adj)
np.testing.assert_allclose(Jr_true, Jr)
def test_jacobians_of_boxminusl_second_element(self):
for i in range(100):
R1, R2 = SO3.random(), SO3.random()
theta, Jr2 = R1.boxminusl(R2, Jr2=np.eye(3))
_, Jl2 = R1.boxminusl(R2, Jl2=np.eye(3))
Jl_true = np.eye(3) @ Jr2 @ R2.Adj.T
np.testing.assert_allclose(Jl_true, Jl2)
def test_jacobians_of_boxminusl(self):
for i in range(100):
R1, R2 = SO3.random(), SO3.random()
theta, Jr = R1.boxminusl(R2, Jr1=np.eye(3))
_, Jl = R1.boxminusl(R2, Jl1=np.eye(3))
Jl_true = np.eye(3) @ Jr @ R1.Adj.T
np.testing.assert_allclose(Jl_true, Jl, atol=1e-8)
def test_boxminusl(self):
for i in range(100):
R1 = SO3.random()
R2 = SO3.random()
v = R1.boxminusl(R2)
R = R2.boxplusl(v)
np.testing.assert_allclose(R1.R, R.R)
def test_jacobians_of_composition_second_element(self):
for i in range(100):
R1 = SO3.random()
R2 = SO3.random()
R3, Jr2 = R1.compose(R2, Jr2=np.eye(3))
_, Jl2 = R1.compose(R2, Jl2=np.eye(3))
Jl2_true = R3.Adj @ Jr2 @ np.linalg.inv(R2.Adj)
np.testing.assert_allclose(Jl2_true, Jl2)
def test_left_jacobian_of_composition(self):
for i in range(100):
R1 = SO3.random()
R2 = SO3.random()
R3, Jr = R1.compose(R2, Jr=np.eye(3))
_, Jl = R1.compose(R2, Jl=np.eye(3))
Jl_true = R3.Adj @ Jr @ np.linalg.inv(R1.Adj)
np.testing.assert_allclose(Jl_true, Jl, atol=1e-10)
def test_right_jacobians_of_boxminusr(self):
for i in range(100):
R1, R2 = SO3.random(), SO3.random()
theta, Jr1 = R1.boxminusr(R2, Jr1=np.eye(3))
dR = R2.inv() * R1
_, Jr1_true = SO3.Log(dR, Jr=np.eye(3))
_, Jr2 = R1.boxminusr(R2, Jr2=np.eye(3))
_, Jr2_true = SO3.Log(dR, Jl=np.eye(3))
np.testing.assert_allclose(Jr1_true, Jr1)
np.testing.assert_allclose(-Jr2_true, Jr2)
def test_left_jacobians_of_boxminusr(self):
for i in range(100):
R1, R2 = SO3.random(), SO3.random()
theta, Jl1 = R1.boxminusr(R2, Jl1=np.eye(3))
_, Jr1 = R1.boxminusr(R2, Jr1=np.eye(3))
Jl1_true = np.eye(3) @ Jr1 @ R1.Adj.T
_, Jl2 = R1.boxminusr(R2, Jl2=np.eye(3))
_, Jr2 = R1.boxminusr(R2, Jr2=np.eye(3))
Jl2_true = np.eye(3) @ Jr2 @ R2.Adj.T
np.testing.assert_allclose(Jl1_true, Jl1, rtol=1e-5)
np.testing.assert_allclose(Jl2_true, Jl2, rtol=1e-5)
def test_left_jacobian_of_logarithm(self):
for i in range(100):
R = SO3.random()
logR, Jl_inv = SO3.Log(R, Jl=np.eye(3))
_, Jl = SO3.Exp(logR, Jl=np.eye(3))
np.testing.assert_allclose(np.linalg.inv(Jl), Jl_inv)
def testNormalize(self):
for i in range(10):
R = SO3.random()
for i in range(10):
R = R * R
R.normalize()
np.testing.assert_allclose(1, R.det())
def test_from_rot_and_trans(self):
rots = [SO3.random().R for i in range(100)]
trans = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
for (R, t) in zip(rots, trans):
T = SE3.fromRAndt(R, t)
q_true = Quaternion.fromRotationMatrix(R)
np.testing.assert_allclose(q_true.q, T.q_arr)
np.testing.assert_allclose(t, T.t)
def test_right_jacobian_or_rotp(self):
for i in range(100):
R = SO3.random()
v = np.random.uniform(-10, 10, size=3)
vp, Jr = R.rotp(v, Jr=np.eye(3))
Jr_true = np.array([[0, -vp[2], vp[1]], [vp[2], 0, -vp[0]],
[-vp[1], vp[0], 0]])
np.testing.assert_allclose(Jr_true, Jr, atol=1e-10)
def test_jacobians_of_boxplusl(self):
for i in range(100):
R = SO3.random()
theta = np.random.uniform(-np.pi, np.pi, size=3)
R2, Jr = R.boxplusl(theta, Jr=np.eye(3))
_, Jl = R.boxplusl(theta, Jl=np.eye(3))
Jl_true = R2.Adj @ Jr @ np.eye(3)
np.testing.assert_allclose(Jl_true, Jl)
def test_left_jacobian_of_inversion(self):
for i in range(100):
R = SO3.random()
R_inv, Jl = R.inv(Jl=np.eye(3))
_, Jr = R.inv(Jr=np.eye(3))
Jl_true = R_inv.Adj @ Jr @ np.linalg.inv(R.Adj)
np.testing.assert_allclose(-R_inv.Adj, Jl)
np.testing.assert_allclose(Jl_true, Jl)
def test_left_jacobian_of_rotp(self):
for i in range(100):
R = SO3.random()
v = np.random.uniform(-10, 10, size=3)
vp, Jl = R.rotp(v, Jl=np.eye(3))
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]],
[-v[1], v[0], 0]])
Jl_true = R.R.T @ vx
np.testing.assert_allclose(Jl_true, Jl)
def test_boxplusl(self):
for i in range(100):
R = SO3.random()
theta = np.random.uniform(0, np.pi)
vec = np.random.uniform(-1, 1, size=3)
vec = vec / np.linalg.norm(vec) * theta
R2 = R.boxplusl(vec)
R2_true = SO3.fromAxisAngle(vec) * R
np.testing.assert_allclose(R2_true.R, R2.R)
def test_right_jacobian_of_rotation(self):
for i in range(100):
R = SO3.random()
v = np.random.uniform(-10, 10, size=3)
vp, Jr = R.rota(v, Jr=np.eye(3))
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]],
[-v[1], v[0], 0]])
Jr_true = -R.R @ vx
np.testing.assert_allclose(Jr_true, Jr)
def test_left_jacobian_of_rotation(self):
for i in range(100):
R = SO3.random()
v = np.random.uniform(-10, 10, size=3)
vp, Jl = R.rota(v, Jl=np.eye(3))
_, Jr = R.rota(v, Jr=np.eye(3))
# Jl = f(R)_Adj * Jr * R_Adj
Jl_true = np.eye(3) @ Jr @ R.Adj.T
np.testing.assert_allclose(Jl_true, Jl, atol=1e-10)
def test_euler(self):
for i in range(100):
R1 = SO3.random()
rpy = R1.euler
R2 = SO3.fromRPY(rpy)
np.testing.assert_allclose(R1.R, R2.R, rtol=1e-5)
def test_right_jacobian_of_inversion(self):
R = SO3.random()
R_inv, Jr = R.inv(Jr=np.eye(3))
Adj_R = R.Adj
np.testing.assert_allclose(-Adj_R, Jr)
def testRandom(self):
for i in range(100):
R = SO3.random()
detR = np.linalg.det(R.R)
np.testing.assert_allclose(1.0, detR)