def test_right_jacobian_of_composition_second_element(self):
R1 = SO2.random()
R2 = SO2.random()
R3, Jr2 = R1.compose(R2, Jr2=True)
Jr2_true = 1.0
np.testing.assert_allclose(Jr2_true, Jr2)
def test_right_jacobian_of_composition(self):
R1 = SO2.random()
R2 = SO2.random()
R3, Jr1 = R1.compose(R2, Jr=True)
Jr1_true = 1.0/R2.Adj
np.testing.assert_allclose(Jr1_true, Jr1)
def testBoxMinusR(self):
R1 = SO2.random()
R2 = SO2.random()
w = R1.boxminusr(R2)
Rres = R2.boxplusr(w)
np.testing.assert_allclose(R1.R, Rres.R)
def test_boxminusl(self):
for i in range(100):
R1 = SO2.random()
R2 = SO2.random()
w = R1.boxminusl(R2)
R = R2.boxplusl(w)
np.testing.assert_allclose(R.R, R1.R)
def test_jacobians_of_boxminus_second_element(self):
for i in range(100):
R1, R2 = SO2.random(), SO2.random()
theta, Jr2 = R1.boxminusl(R2, Jr2=1)
_, Jl2 = R1.boxminusl(R2, Jl2=1)
Jl_true = 1 * Jr2 * R2.Adj
np.testing.assert_allclose(Jl_true, Jl2)
def test_left_jacobian_of_composition_second_element(self):
R1 = SO2.random()
R2 = SO2.random()
R3, Jl2 = R1.compose(R2, Jl2=True)
_, Jr2 = R1.compose(R2, Jr2=True)
Jl2_true = R3.Adj * Jr2 * 1.0 / R2.Adj
np.testing.assert_allclose(Jl2_true, Jl2)
def test_jacobians_of_boxminusl(self):
for i in range(100):
R1, R2 = SO2.random(), SO2.random()
theta, Jr = R1.boxminusl(R2, Jr1=1)
_, Jl = R1.boxminusl(R2, Jl1=1)
Jl_true = 1 * Jr * R1.Adj
np.testing.assert_allclose(Jl_true, Jl)
def test_left_jacobian_of_composition(self):
R1 = SO2.random()
R2 = SO2.random()
R3, Jr = R1.compose(R2, Jr=True)
_, Jl = R1.compose(R2, Jl=True)
Adj_R1 = R1.Adj
Adj_R3 = R3.Adj
Jl_true = Adj_R3 * Jr * 1.0/Adj_R1
np.testing.assert_allclose(Jl_true, Jl)
def test_right_jacobians_or_boxminusr(self):
for i in range(100):
R1 = SO2.random()
R2 = SO2.random()
theta, Jr1 = R1.boxminusr(R2, Jr1=1)
_, Jr2 = R1.boxminusr(R2, Jr2=1)
dR = R2.inv() * R1
_, Jr1_true = SO2.Log(dR, Jr=1)
_, Jr2_true = SO2.Log(dR, Jl=1)
np.testing.assert_allclose(Jr1_true, Jr1)
np.testing.assert_allclose(Jr2_true, Jr2)
def test_left_jacobians_of_boxminusr(self):
R1, R2 = SO2.random(), SO2.random()
theta, Jl1 = R1.boxminusr(R2, Jl1=1)
_, Jr1 = R1.boxminusr(R2, Jr1=1)
Jl1_truth = 1 * Jr1 * R1.Adj
theta, Jl2 = R1.boxminusr(R2, Jl2=1)
_, Jr2 = R1.boxminusr(R2, Jr2=1)
Jl2_truth = 1 * Jr2 * R2.Adj
np.testing.assert_allclose(Jl1_truth, Jl1)
np.testing.assert_allclose(Jl2_truth, Jl2)
def test_left_jacobian_of_rota(self):
for i in range(100):
R = SO2.random()
v = np.random.uniform(-10, 10, size=2)
vp, Jl = R.rota(v, Jl=True)
_, Jr = R.rota(v, Jr=True)
np.testing.assert_allclose(Jr, Jl)
def test_right_jacobian_of_boxplusr(self):
for i in range(100):
R = SO2.random()
theta = np.random.uniform(-np.pi, np.pi)
R2, Jr = R.boxplusr(theta, Jr=1)
_, Jr_true = SO2.Exp(theta, Jr=1)
np.testing.assert_allclose(Jr_true, Jr)
def test_left_jacobian_of_rotp(self):
for i in range(100):
R = SO2.random()
v = np.random.uniform(-10, 10, size=2)
vp, Jl = R.rotp(v, Jl=1)
one_x = np.array([[0, -1], [1, 0]])
Jl_true = - R.R.T @ one_x @ v
np.testing.assert_allclose(Jl_true, Jl)
def test_boxplusl(self):
for i in range(100):
R = SO2.random()
w = np.random.uniform(-np.pi, np.pi)
R2 = SO2.fromAngle(w)
R3 = R.boxplusl(w)
R3_true = R2 * R
np.testing.assert_allclose(R3_true.R, R3.R)
def test_right_jacobian_of_rotp(self):
for i in range(100):
R = SO2.random()
v = np.random.uniform(-10, 10, size=2)
vp, Jr = R.rotp(v, Jr=1)
one_x = np.array([[0, -1], [1, 0]])
Jr_true = -one_x @ vp
np.testing.assert_allclose(Jr_true, Jr)
def test_right_jacobian_of_rota(self):
for i in range(100):
R = SO2.random()
v = np.random.uniform(-10, 10, size=2)
vp, Jr = R.rota(v, Jr=True)
vx = np.array([-v[1], v[0]])
Jr_true = R.R @ vx
np.testing.assert_allclose(Jr_true, Jr)
def test_jacobians_of_boxplusl(self):
for i in range(100):
R = SO2.random()
theta = np.random.uniform(-np.pi, np.pi)
R2, Jr = R.boxplusl(theta, Jr=1)
_, Jl = R.boxplusl(theta, Jl=1)
Jl_true = R2.Adj * Jr * 1
np.testing.assert_allclose(Jl_true, Jl)
def test_left_jacobian_of_boxplusr(self):
for i in range(100):
R = SO2.random()
theta = np.random.uniform(-np.pi, np.pi)
R2, Jl = R.boxplusr(theta, Jl=1)
_, Jr = R.boxplusr(theta, Jr=1)
Jl_true = R2.Adj * Jr * 1.0 # Using adjoing formula
np.testing.assert_allclose(Jl_true, Jl)
def test_left_jacobian_of_inversion(self):
R = SO2.random()
R_inv, Jl = R.inv(Jl=True)
_, Jr = R.inv(Jr=True)
Adj_R = R.Adj
Adj_Rinv = R_inv.Adj
Jl_true = Adj_Rinv * Jr * (1.0 / Adj_R)
self.assertEqual(-1, Jl)
self.assertEqual(Jl_true, Jl)
def test_right_jacobian_of_inversion(self):
R = SO2.random()
R_inv, Jr = R.inv(Jr=True)
self.assertEqual(-1, Jr)
def test_right_jacobian_of_logarithm(self):
R = SO2.random()
logR, Jr_inv = SO2.Log(R, Jr=True)
_, Jr = SO2.Exp(logR, Jr=True)
self.assertEqual(1/Jr, Jr_inv)
def test_left_jacobian_of_logarithm(self):
R = SO2.random()
logR, Jl_inv = SO2.Log(R, Jl=True)
_, Jl = SO2.Exp(logR, Jl=True)
self.assertEqual(1/Jl, Jl_inv)