def f_Gamma(g, dt, omega_coriolis):
"""Compute Gamma preintegration with Coriolis forces"""
vec = torch.cat((omega_coriolis, g, g)) * dt
tmp = SE3_2.uexp(vec.cuda()).cpu()[:4, :4] # for taking left-Jacobian
Omega = SO3.uwedge(omega_coriolis)
Omega2 = Omega.mm(Omega)
ang = omega_coriolis.norm()
if ang == 0: # without Coriolis
mat = (dt**2) / 2 * torch.eye(3)
else:
a = (dt * ang * (dt * ang).cos() - (dt * ang).sin()) / (ang**3)
b = (-dt * ang * (dt * ang).sin() - (dt * ang).cos() + 1 +
((dt * ang)**2) / 2) / (ang**4)
mat = (dt**2) / 2 * torch.eye(3) + a * Omega + b * Omega2
Gamma = torch.eye(5)
Gamma[:3, :3] = tmp[:3, :3]
Gamma[:3, 3] = tmp[:3, 3]
Gamma[:3, 4] = tmp[:3, :3].mm(mat).mv(g)
return Gamma
return res
if __name__ == '__main__':
path = 'figures/second_vs_four_order.txt'
### Parameters ###
i_max = 5000 # number of random points
k_max = 301 # number of compounded poses
g = torch.Tensor([0, 0, 9.81]) # gravity vector
dt = 0.05 # step time (s)
sigmas = 0.03 * torch.Tensor(
[0.1, 0.3, 0.5, 1, 1.5, 2, 2.5, 3, 3.5, 4, 4.5, 5])
# Constant acceleration, noise on IMU
# Define a PDF over transformations (mean and covariance)
xibar = torch.Tensor([0, 0, 0, 1, 0, 0, 0, 0, 0]).cuda() * dt
Upsilon = SE3_2.uexp(xibar).cpu()
Upsilon[:3, 3] += -g * dt
Upsilon[:3, 4] = Upsilon[:3, 3] * dt / 2
T0 = torch.eye(5)
Sigma0 = torch.zeros(9, 9)
res = torch.zeros(sigmas.shape[0], 3)
for i in range(sigmas.shape[0]):
# Define right perturbation noise
cholQ = torch.Tensor([0, 0, sigmas[i], 0, 0, 0, 0, 0, 0]).diag()
Q = cholQ.mm(cholQ.t())
res[i] = main(i_max, k_max, T0, Sigma0, Upsilon, Q, cholQ, dt, g)
res[:, 0] = sigmas
# np.savetxt(path, res.numpy(), comments="", header="sigma second four")
plt.plot(res[:, 0], res[:, 2], color='cyan')
