def computeJacobian(omegas_m, accs_m, biases, dt):
"""
Compute first-order Jacobian for bias update.
Note: same for each distribution assuming Delta t is small.
"""
J = torch.zeros(9, 6)
G = torch.zeros(9, 6)
F = torch.eye(9)
F[6:9, 3:6] = torch.eye(3) * dt
G[:3, :3] = -dt * torch.eye(3)
G[3:6, 3:6] = -dt * torch.eye(3)
G[6:9, 3:6] = -0.5 * (dt**2) * torch.eye(3)
Upsilon = torch.eye(5)
Omegas = SO3.exp((omegas_m * dt).cuda()).cpu()
invJac = SO3.inv_left_jacobian((omegas_m * dt).cuda()).cpu()
for k in range(accs_m.shape[0]):
acc = accs_m[k]
Upsilon[:3, :3] = Omegas[k]
Upsilon[:3, 3] = acc * dt
Upsilon[:3, 4] = 1 / 2 * acc * (dt**2)
G[:3, :3] = -invJac[k] * dt
G[3:6, 3:6] = -dt * Upsilon[:3, :3].t()
G[6:9, 3:6] = -0.5 * (dt**2) * Upsilon[:3, :3].t()
J = SE3_2.uAd(Upsilon.inverse()).mm(F).mm(J) + G
return J
def propagate(T0, P, Upsilon, Q, method, dt, g, cholQ=0):
"""Propagate state for one time step"""
Gamma = f_Gamma(g, dt)
Phi = f_flux(T0, dt)
# propagate the mean
T = Gamma.mm(Phi).mm(Upsilon)
# Jacobian for propagating prior along time
F = torch.eye(9)
F[6:9, 3:6] = torch.eye(3) * dt
# compute Adjoint of right transformation mean
AdUps = SE3_2.uAd(SE3_2.uinv(Upsilon))
Pprime = axat(AdUps.mm(F), P)
# compound the covariances based on the second-order method
Pprop = Pprime + Q
if method == 1:
# add fourth-order method
Pprop += four_order(Pprime, Q)
elif method == 2:
# Monte Carlo method
n_tot_samples = 1000000
nsamples = 50000
N = int(n_tot_samples / nsamples) + 1
tmp = torch.cholesky(P + 1e-20 * torch.eye(9))
cholP = tmp.cuda().expand(nsamples, 9, 9)
cholQ = cholQ.cuda().expand(nsamples, 9, 9)
Pprop = torch.zeros(9, 9)
Gamma = Gamma.cuda().expand(nsamples, 5, 5)
Upsilon = Upsilon.cuda().expand(nsamples, 5, 5)
T0 = T0.cuda().expand(nsamples, 5, 5)
T_inv = T.inverse().cuda().expand(nsamples, 5, 5)
for i in range(N):
xi0 = bmv(cholP, torch.randn(nsamples, 9).cuda())
w = bmv(cholQ, torch.randn(nsamples, 9).cuda())
T0_i = T0.bmm(SE3_2.exp(xi0))
Phi = f_flux(T0_i, dt)
Upsilon_i = Upsilon.bmm(SE3_2.exp(w))
T_i = Gamma.bmm(Phi).bmm(Upsilon_i)
xi = SE3_2.log(T_inv.bmm(T_i))
xi_mean = xi.mean(dim=0)
Pprop += bouter(xi - xi_mean, xi - xi_mean).sum(dim=0).cpu()
Pprop = Pprop / (N * nsamples + 1)
Pprop = (Pprop + Pprop.t()) / 2 # symmetric
return T, Pprop
def propagate(T0, Sigma, Upsilon, Q, method, dt, g):
"""Propagate state for one time step"""
Gamma = f_Gamma(g, dt)
Phi = f_flux(T0, dt)
# propagate the mean
T = Gamma.mm(Phi).mm(Upsilon)
# Jacobian for propagating prior along time
F = torch.eye(9)
F[6:9, 3:6] = torch.eye(3) * dt
# compute Adjoint of right transformation mean
AdUps = SE3_2.uAd(SE3_2.uinv(Upsilon))
Sigma_tmp = axat(AdUps.mm(F), Sigma)
# compound the covariances based on the second-order method
Sigma_prop = Sigma_tmp + Q
if method == 1:
# add fourth-order method
Sigma_prop += four_order(Sigma_tmp, Q)
elif method == 2:
# SO(3) x R^6
wedge_acc = SO3.uwedge(Upsilon[:3, 3]) # already multiplied by dt
F = torch.eye(9)
F[3:6, :3] = T0[:3, :3].t()
F[3:6, :3] = -T0[:3, :3].mm(wedge_acc)
F[6:9, :3] = F[3:6, :3] * dt / 2
F[6:9, 3:6] = dt * torch.eye(3)
G = torch.zeros(9, 6)
G[:3, :3] = dt * T0[:3, :3].t()
G[3:6, 3:6] = T0[:3, :3] * dt
G[6:9, 3:6] = 1 / 2 * T0[:3, :3] * (dt**2)
Sigma_prop = axat(F, Sigma) + axat(G, Q[:6, :6] / (dt**2))
Sigma_prop = (Sigma_prop + Sigma_prop.t()) / 2 # symmetric
return T, Sigma_prop
def propagate(T0, Sigma, Upsilon, Q, method, dt, g, cholQ=0):
"""Propagate state for one time step"""
Gamma = f_Gamma(g, dt)
Phi = f_flux(T0, dt)
# propagate the mean
T = Gamma.mm(Phi).mm(Upsilon)
# Jacobian for propagating prior along time
F = torch.eye(9)
F[6:9, 3:6] = torch.eye(3) * dt
# compute Adjoint of right transformation mean
AdUps = SE3_2.uAd(SE3_2.uinv(Upsilon))
Sigma_tmp = axat(AdUps.mm(F), Sigma)
# compound the covariances based on the second-order method
Sigma_prop = Sigma_tmp + Q
if method == 1:
# add fourth-order method
Sigma_prop += four_order(Sigma_tmp, Q)
Sigma_prop = (Sigma_prop + Sigma_prop.t()) / 2 # symmetric
return T, Sigma_prop
def compound(T0, Sigma, Upsilon, Q, method, dt, g, cholQ=0):
Gamma = f_Gamma(g, dt)
Phi = f_flux(T0, dt)
# compound the mean
T = Gamma.mm(Phi).mm(Upsilon)
# Jacobian for propagating prior along time
F = torch.eye(9)
F[6:9, 3:6] = torch.eye(3) * dt
# compute Adjoint of right transformation mean
AdUps = SE3_2.uAd(SE3_2.uinv(Upsilon))
Sigma_tmp = axat(AdUps.mm(F), Sigma)
# compound the covariances based on the second-order method
Sigma_prop = Sigma_tmp + Q
if method == 3:
# baseline SO(3) x R^6
wedge_acc = SO3.uwedge(Upsilon[:3, 3]) # already multiplied by dt
F = torch.eye(9)
F[3:6, :3] = T0[:3, :3].t()
F[3:6, :3] = -T0[:3, :3].mm(wedge_acc)
F[6:9, :3] = F[3:6, :3] * dt / 2
F[6:9, 3:6] = dt * torch.eye(3)
G = torch.zeros(9, 6)
G[:3, :3] = T0[:3, :3].t()
G[3:6, 3:6] = T0[:3, :3]
G[6:9, 3:6] = 1 / 2 * T0[:3, :3] * dt
Sigma_prop = axat(F, Sigma) + axat(G, Q[:6, :6])
elif method == 4:
# Monte Carlo method
n_tot_samples = 100000
nsamples = 50000
N = int(n_tot_samples / nsamples) + 1
tmp = torch.cholesky(Sigma_prop + 1e-16 * torch.eye(9))
cholP = tmp.cuda().expand(nsamples, 9, 9)
cholQ = cholQ.cuda().expand(nsamples, 9, 9)
Sigma_prop = torch.zeros(9, 9)
Gamma = Gamma.cuda().expand(nsamples, 5, 5)
Upsilon = Upsilon.cuda().expand(nsamples, 5, 5)
T0 = T0.cuda().expand(nsamples, 5, 5)
T_inv = T.inverse().cuda().expand(nsamples, 5, 5)
for i in range(N):
xi0 = bmv(cholP, torch.randn(nsamples, 9).cuda())
w = bmv(cholQ, torch.randn(nsamples, 9).cuda())
T0_i = T0.bmm(SE3_2.exp(xi0))
Phi = f_flux(T0_i, dt)
Upsilon_i = Upsilon.bmm(SE3_2.exp(w))
T_i = Gamma.bmm(Phi).bmm(Upsilon_i)
xi = SE3_2.log(T_inv.bmm(T_i))
xi_mean = xi.mean(dim=0)
Sigma_prop += bouter(xi - xi_mean, xi - xi_mean).sum(dim=0).cpu()
Sigma_prop = Sigma_prop / (N * nsamples + 1)
Sigma_prop = Sigma_prop / (N * nsamples + 1)
Sigma_prop = (Sigma_prop + Sigma_prop.t()) / 2
return T, Sigma_prop
r = torch.randn(i_max, 9)
r = r[r.norm(dim=1) < 4.1682][:, 6:9]
P_est_chol = torch.cholesky(P_est[6:9, 6:9])
p_est = bmv(P_est_chol.expand(r.shape[0], 3, 3), r) + T_est[:3, 4]
plt.scatter(p_est[:, i1], p_est[:, i2], color=color, s=2, alpha=0.5)
if i1 == 0 and i2 == 1:
np.savetxt(path2, p_est.numpy(), header="x y z", comments='')
if __name__ == '__main__':
i_max = 1000
# Propagate the uncertainty methods
T_est = torch.eye(5)
T_est[:3, 4] = torch.Tensor([5, 0, 0])
P_est = torch.diag(torch.Tensor([0.1, 0.1, 0.5, 1, 1, 1, 0.1, 0.1, 0.1]))
P_est = axat(SE3_2.uAd(T_est.inverse()), P_est)
SigmaSO3 = torch.eye(9)
SigmaSO3[7, 7] = 5
SigmaSO3[6, 6] = 0.5
# saving paths
path1 = "figures/retraction_b.txt"
path2 = "figures/retraction_est.txt"
labels = ['x (m)', 'y (m)', 'z (m)']
for i1, i2 in [(0, 1)]:
plt.figure()
# Plot the covariance of the samples
T_est[:3, 4] = torch.Tensor([-4, 0, 0])
plot_so3_helper(T_est, SigmaSO3, "red", i1, i2, i_max, path1)
plt.scatter(T_est[i1, 4], T_est[i2, 4], color='blue', s=20)
