def test_adjoint(self):
v_list = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
w_list = [np.random.uniform(-np.pi, np.pi, size=3) for i in range(100)]
for (v, w, T) in zip(v_list, w_list, self.transforms):
vec = np.array([*v, *w])
T1 = T * SE3.Exp(vec)
T2 = SE3.Exp(T.Adj @ vec) * T
np.testing.assert_allclose(T1.T, T2.T)
def test_jacobians_of_exponential(self):
for i in range(100):
rho = np.random.uniform(-10, 10, size=3)
theta = np.random.uniform(-np.pi, np.pi, size=3)
tau = np.array([*rho, *theta])
T, Jr = SE3.Exp(tau, Jr=np.eye(6))
_, Jl = SE3.Exp(-tau, Jl=np.eye(6))
np.testing.assert_allclose(Jr, Jl)
def test_right_jacobian_of_logarithm(self):
for i in range(100):
T = SE3.random()
logT, Jr_inv = SE3.Log(T, Jr=np.eye(6))
_, Jr = SE3.Exp(logT, Jr=np.eye(6))
np.testing.assert_allclose(np.linalg.inv(Jr), Jr_inv)
def se3ExpJacobian(tau):
Jr, Jl = np.eye(6), np.eye(6)
T = SE3.Exp(tau)
dx = 1e-4
for i in range(6):
delta = np.zeros(6)
delta[i] = dx
tau2 = tau + delta
T2 = SE3.Exp(tau2)
vecr = T2.boxminusr(T)
vecl = T2.boxminusl(T)
Jr[:, i] = vecr / dx
Jl[:, i] = vecl / dx
return Jr, Jl
def test_right_jacobian_of_boxplusr(self):
for T in self.transforms:
v = np.random.uniform(-10, 10, size=3)
theta = np.random.uniform(-np.pi, np.pi, size=3)
tau = np.array([*v, *theta])
T2, Jr = T.boxplusr(tau, Jr=np.eye(6))
_, Jr_true = SE3.Exp(tau, Jr=np.eye(6))
np.testing.assert_allclose(Jr_true, Jr)
def test_boxplusl(self):
v_list = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
w_list = [np.random.uniform(-np.pi, np.pi, size=3) for i in range(100)]
for (T, v, w) in zip(self.transforms, v_list, w_list):
vec = np.array([*v, *w])
T2 = T.boxplusl(vec)
T2_true = SE3.Exp(vec) * T
np.testing.assert_allclose(T2_true.T, T2.T)
def test_exponential_map(self):
v_list = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
w_list = [np.random.uniform(-np.pi, np.pi, size=3) for i in range(100)]
for (v, w) in zip(v_list, w_list):
vec = np.array([*v, *w])
T = SE3.Exp(vec)
logT = np.array([[0, -w[2], w[1], v[0]], [w[2], 0, -w[0], v[1]],
[-w[1], w[0], 0, v[2]], [0, 0, 0, 0]])
T_true = sp.linalg.expm(logT)
R_true = T_true[:3, :3]
t_true = T_true[:3, 3]
np.testing.assert_allclose(R_true, T.R.T)
np.testing.assert_allclose(t_true, T.t)
def test_exponential_map_taylor_series(self):
axis = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
v_list = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
angles = [np.random.uniform(0, 1e-8) for i in range(100)]
for (a, v, psi) in zip(axis, v_list, angles):
w = a / np.linalg.norm(a) * psi
vec = np.array([*v, *w])
T = SE3.Exp(vec)
logT = np.array([[0, -w[2], w[1], v[0]], [w[2], 0, -w[0], v[1]],
[-w[1], w[0], 0, v[2]], [0, 0, 0, 0]])
T_true = sp.linalg.expm(logT)
R_true = T_true[:3, :3]
t_true = T_true[:3, 3]
np.testing.assert_allclose(R_true, T.R.T, rtol=1e-4, atol=1e-4)
np.testing.assert_allclose(t_true, T.t, rtol=1e-4, atol=1e-4)
T_inv, Jl = T.inv(Jl=np.eye(6))
Jrn, Jln = se3InverseJacobian(T)
# Jacobian of composition works
T2 = SE3.random()
T3, Jr1 = T.compose(T2, Jr=np.eye(6))
T3, Jl1 = T.compose(T2, Jl=np.eye(6))
T3, Jr2 = T.compose(T2, Jr2=np.eye(6))
T3, Jl2 = T.compose(T2, Jl2=np.eye(6))
Jr1n, Jl1n, Jr2n, Jl2n = se3ComposeJacobians(T, T2)
# Exponential Jacobian works
rho = np.random.uniform(-10, 10, size=3)
theta = np.random.uniform(-np.pi, np.pi, size=3)
tau = np.array([*rho, *theta])
T, Jr = SE3.Exp(tau, Jr=np.eye(6))
T, Jl = SE3.Exp(tau, Jl=np.eye(6))
Jrn, Jln = se3ExpJacobian(tau)
# Logarithm jacobian works
tau, Jr = SE3.Log(T, Jr=np.eye(6))
tau, Jl = SE3.Log(T, Jl=np.eye(6))
Jrn, Jln = se3LogJacobian(T)
debug = 1
# Jacobians of transformation works
pt = np.random.uniform(-10, 10, size=3)
pt2, Jr = T.transa(pt, Jr=np.eye(6))
pt2, Jl = T.transa(pt, Jl=np.eye(6))
Jrn, Jln = transJacobian(T, pt)
debug = 1