def test_right_jacobians_of_boxminusr(self):
for T1 in self.transforms:
T2 = SE3.random()
tau, Jr1 = T1.boxminusr(T2, Jr1=np.eye(6))
dT = T2.inv() * T1
_, Jr1_true = SE3.Log(dT, Jr=np.eye(6))
_, Jr2 = T1.boxminusr(T2, Jr2=np.eye(6))
_, Jr2_true = SE3.Log(dT, Jl=np.eye(6))
np.testing.assert_allclose(Jr1_true, Jr1)
np.testing.assert_allclose(-Jr2_true, Jr2)
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 se3LogJacobian(T):
Jr, Jl = np.eye(6), np.eye(6)
tau = SE3.Log(T)
dx = 1e-4
for i in range(6):
delta = np.zeros(6)
delta[i] = dx
Tr = T.boxplusr(delta)
Tl = T.boxplusl(delta)
vecr = SE3.Log(Tr) - tau
vecl = SE3.Log(Tl) - tau
Jr[:, i] = vecr / dx
Jl[:, i] = vecl / dx
return Jr, Jl
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
# Jr = [T.R.T, -T.R.T @ skew(pt)]
# Jl = [I_3, -skew(pt2)]
# All other quat jacobians are a composition of the above jacobians