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 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 compute_results(T, i_max, T_est, Sigma_est, SigmaSO3, Sigma_est_mc): results = torch.zeros(3) # Methods on SE_2(3) chi_diff = SE3_2.uinv(T_est[-1]).expand(i_max, 5, 5).bmm(T[:, -1]) xi = SE3_2.log(chi_diff.cuda()).cpu() s_nees = compute_nees(Sigma_est, xi) mc_nees = compute_nees(Sigma_est_mc, xi) results[0] = s_nees results[1] = mc_nees # Method on SO(3) xi = SE3_2.boxminus(T_est[-1].expand(i_max, 5, 5).cuda(), T[:, -1].cuda()).cpu() s_nees = compute_nees(SigmaSO3, xi) results[2] = s_nees return results
def main(i_max, k_max, T0, Upsilons, Q, cholQ, dt, g): # Generate some random samples # NOTE: initial covariance is zero T = torch.zeros(i_max, k_max, 5, 5).cuda() T[:, 0] = T0.cuda().repeat(i_max, 1, 1) Gamma = f_Gamma(g, dt).cuda().expand(i_max, 5, 5) tmp = cholQ.cuda().expand(i_max, 9, 9) for k in range(1, k_max): T_k = SE3_2.exp(bmv(tmp, torch.randn(i_max, 9).cuda())) Phi = f_flux(T[:, k-1], dt) tmp2 = Upsilons[k].cuda().expand(i_max, 5, 5) T[:, k] = Gamma.bmm(Phi).bmm(T_k).bmm(tmp2) T = T.cpu() # Propagate the uncertainty using second- and fourth-order methods T_est = torch.zeros(k_max, 5, 5) Sigma_est = torch.zeros(k_max, 9, 9) # covariance SigmaSO3 = torch.zeros(k_max, 9, 9) # SO(3) x R^6 covariance Sigma_est_mc = torch.zeros(k_max, 9, 9) # Monte-Carlo covariance on SE_2(3) T_est[0] = T0 for k in range(1, k_max): # Second-order method T_est[k], Sigma_est[k] = compound(T_est[k-1], Sigma_est[k-1], Upsilons[k], Q, 1, dt, g) # baseline method _, SigmaSO3[k] = compound(T_est[k-1], SigmaSO3[k-1], Upsilons[k], Q, 3, dt, g) # Monte-Carlo method _, Sigma_est_mc[k] = compound(T_est[k-1], Sigma_est_mc[k-1], Upsilons[k], Q, 4, dt, g, cholQ) results = compute_results(T, i_max, T_est, Sigma_est, SigmaSO3, Sigma_est_mc) return results
def correctPIM(DeltaRs, DeltaVs, DeltaPs, DUpsilonDb, biases, methode): """ Correct preintegration measurement of $SE_2(3)$ and $SO(3)xR^6$ distributions. """ N = DeltaRs.shape[0] delta_pim = bmv(DUpsilonDb.expand(N, 9, 6), biases) if methode == 1: # Correct preintegration measurement of SE_2(3) distribution. Upsilon = torch.eye(5) Upsilon[:3, :3] = DeltaRs[0] Upsilon[:3, 3] = DeltaVs[0] Upsilon[:3, 4] = DeltaPs[0] Upsilon_cor = Upsilon.expand(N, 5, 5).bmm(SE3_2.exp(delta_pim.cuda()).cpu()) DeltaRs_cor = Upsilon_cor[:, :3, :3] DeltaVs_cor = Upsilon_cor[:, :3, 3] DeltaPs_cor = Upsilon_cor[:, :3, 4] else: # Correct preintegration measurement of SO(3)xR^6 distribution. DeltaRs_cor = DeltaRs[0].expand(N, 3, 3).bmm( SO3.exp(delta_pim[:, :3].cuda()).cpu()) DeltaVs_cor = DeltaVs[0].expand(N, 3) + bmv(DeltaRs[0].expand(N, 3, 3), delta_pim[:, 3:6]) DeltaPs_cor = DeltaPs[0].expand(N, 3) + bmv(DeltaRs[0].expand(N, 3, 3), delta_pim[:, 6:9]) return DeltaRs_cor, DeltaVs_cor, DeltaPs_cor
def plot_se23_helper(T_est, P_est, color, i1, i2, i_max, path): P_est_chol = torch.cholesky(P_est) r = bmv(P_est_chol.expand(i_max, 9, 9), torch.randn(i_max, 9)) r = r[r.norm(dim=1) < 8.1682] Ttemp = T_est.expand(r.shape[0], 5, 5).bmm(SE3_2.exp(r.cuda()).cpu()) p_est = Ttemp[:, :3, 4] plt.scatter(p_est[:, i1], p_est[:, i2], color=color, s=3) if i1 == 0 and i2 == 1: np.savetxt(path, p_est.numpy(), header="x y z", comments='')
def f(eta): omega_i = omega + eta[:3] acc_i = acc + eta[3:6] u = torch.cat((omega_i, acc_i, 0.5 * acc_i * Deltat)) * Deltat Ups_i = torch.eye(5) Ups_i[:3, :3] = SO3.uexp(u[:3]) Ups_i[:3, 3] = u[3:6] Ups_i[:3, 4] = u[6:9] return SE3_2.ulog(Ups_i.inverse().mm(Ups).cuda()).cpu()
def main(i_max, k_max, T0, P0, Upsilon, Q, cholQ, dt, g): # Generate some random samples T = torch.zeros(i_max, k_max, 5, 5).cuda() T[:, 0] = T0.cuda().repeat(i_max, 1, 1) tmp = P0.sqrt().cuda().expand(i_max, 9, 9) # Pxi assumed diagonal! T[:, 0] = T[:, 0].bmm(SE3_2.exp(bmv(tmp, torch.randn(i_max, 9).cuda()))) Gamma = f_Gamma(g, dt).cuda().expand(i_max, 5, 5) tmp = cholQ.cuda().expand(i_max, 9, 9) tmp2 = Upsilon.cuda().expand(i_max, 5, 5) for k in range(1, k_max): T_k = SE3_2.exp(bmv(tmp, torch.randn(i_max, 9).cuda())) Phi = f_flux(T[:, k - 1], dt) T[:, k] = Gamma.bmm(Phi).bmm(tmp2).bmm(T_k) T = T.cpu() # Propagate the uncertainty using second- and fourth-order methods T_est = torch.zeros(k_max, 5, 5) P_est1 = torch.zeros(k_max, 9, 9) # second order covariance P_est2 = torch.zeros(k_max, 9, 9) # fourth order covariance P_est_mc = torch.zeros(k_max, 9, 9) # SO(3) x R^6 covariance T_est[0] = T0 P_est1[0] = P0.clone() P_est2[0] = P0.clone() P_est_mc[0] = P0.clone() for k in range(1, k_max): # Second-order method T_est[k], P_est1[k] = propagate(T_est[k - 1], P_est1[k - 1], Upsilon, Q, 0, dt, g) # Fourth-order method _, P_est2[k] = propagate(T_est[k - 1], P_est2[k - 1], Upsilon, Q, 1, dt, g) # baseline method _, P_est_mc[k] = propagate(T_est[k - 1], P_est_mc[k - 1], Upsilon, Q, 2, dt, g, cholQ) res = torch.zeros(3) res[1] = fro_norm(P_est_mc[-1], P_est1[-1]) res[2] = fro_norm(P_est_mc[-1], P_est2[-1]) print(fro_norm(P_est1[-1], P_est2[-1])) return res
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 main(i_max, k_max, T0, Sigma0, Upsilon, Q, cholQ, dt, g): # Generate some random samples T = torch.zeros(i_max, k_max, 5, 5).cuda() T[:, 0] = T0.cuda().repeat(i_max, 1, 1) tmp = Sigma0.sqrt().cuda().expand(i_max, 9, 9) # Sigma0 assumed diagonal! T[:, 0] = T[:, 0].bmm(SE3_2.exp(bmv(tmp, torch.randn(i_max, 9).cuda()))) Gamma = f_Gamma(g, dt).cuda().expand(i_max, 5, 5) tmp = cholQ.cuda().expand(i_max, 9, 9) tmp2 = Upsilon.cuda().expand(i_max, 5, 5) for k in range(1, k_max): T_k = SE3_2.exp(bmv(tmp, torch.randn(i_max, 9).cuda())) Phi = f_flux(T[:, k - 1], dt) T[:, k] = Gamma.bmm(Phi).bmm(tmp2).bmm(T_k) T = T.cpu() # Propagate the uncertainty using second- and fourth-order methods T_est = torch.zeros(k_max, 5, 5) Sigma2th = torch.zeros(k_max, 9, 9) # second order covariance Sigma4th = torch.zeros(k_max, 9, 9) # fourth order covariance T_est[0] = T0 Sigma2th[0] = Sigma0.clone() Sigma4th[0] = Sigma0.clone() for k in range(1, k_max): # Second-order method T_est[k], Sigma2th[k] = propagate(T_est[k - 1], Sigma2th[k - 1], Upsilon, Q, 0, dt, g) # Fourth-order method _, Sigma4th[k] = propagate(T_est[k - 1], Sigma4th[k - 1], Upsilon, Q, 1, dt, g) xi = SE3_2.log((T_est[-1].inverse().expand(i_max, 5, 5).bmm(T[:, -1])).cuda()) P_est_mc = bouter(xi, xi).sum(dim=0).cpu() / (i_max - 1) res = torch.zeros(3) res[1] = fro_norm(P_est_mc[-1], Sigma2th[-1]) res[2] = fro_norm(P_est_mc[-1], Sigma4th[-1]) return res
def main(i_max, k_max, T0, Sigma0, Upsilon, Q, cholQ, dt, g, sigma, m_max, paths): # Generate some random samples T = torch.zeros(i_max, k_max, 5, 5).cuda() T[:, 0] = T0.cuda().repeat(i_max, 1, 1) # NOTE: no initial uncertainty Gamma = f_Gamma(g, dt).cuda().expand(i_max, 5, 5) tmp = cholQ.cuda().expand(i_max, 9, 9) tmp2 = Upsilon.cuda().expand(i_max, 5, 5) for k in range(1, k_max): T_k = SE3_2.exp(bmv(tmp, torch.randn(i_max, 9).cuda())) Phi = f_flux(T[:, k - 1], dt) T[:, k] = Gamma.bmm(Phi).bmm(T_k).bmm(tmp2) T = T.cpu() # Propagate the uncertainty methods T_est = torch.zeros(k_max, 5, 5) P_est = torch.zeros(k_max, 9, 9) SigmaSO3 = torch.zeros(k_max, 9, 9) # SO(3) x R^6 covariance T_est[0] = T0 P_est[0] = Sigma0.clone() SigmaSO3[0] = Sigma0.clone() for k in range(1, k_max): T_est[k], P_est[k] = propagate(T_est[k - 1], P_est[k - 1], Upsilon, Q, 1, dt, g) # baseline method _, SigmaSO3[k] = propagate(T_est[k - 1], SigmaSO3[k - 1], Upsilon, Q, 2, dt, g) # Now plot the transformations labels = ['x (m)', 'y (m)', 'z (m)'] for i1, i2 in ((0, 1), (0, 2), (1, 2)): plt.figure() # Plot the covariance of the samples plot_so3_helper(T_est, SigmaSO3[-1], "red", i1, i2, i_max, paths[1]) # Plot the propagated covariance projected onto i1, i2 plot_se23_helper(T_est, P_est[-1], 'green', i1, i2, i_max, paths[2]) # Plot the random samples' xy-locations plt.scatter(T[:, -1, i1, 4], T[:, -1, i2, 4], s=1, color='black', alpha=0.5) plt.scatter(T_est[-1, i1, 4], T_est[-1, i2, 4], color='yellow', s=30) plt.xlabel(labels[i1]) plt.ylabel(labels[i2]) plt.legend([r"$SO(3) \times \mathbb{R}^6$", r"$SE_2(3)$"]) # np.savetxt(paths[0], T[:, -1, :3, 4].numpy(), header="x y z", comments='') plt.show()
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 plot_se23_helper(T_est, P_est, v, color, i1, i2): """ Draw ellipse based on the 3 more important directions of the covariance """ D, V = torch.eig(P_est, eigenvectors=True) Y, I = torch.sort(D[:, 0], descending=True) a = sigma * D[I[0], 0].sqrt() * V[:, I[0]] b = sigma * D[I[1], 0].sqrt() * V[:, I[1]] c = sigma * D[I[2], 0].sqrt() * V[:, I[2]] for n in range(3): if n == 0: xi = a * v.sin() + b * v.cos() elif n == 1: xi = b * v.sin() + c * v.cos() elif n == 2: xi = a * v.sin() + c * v.cos() Ttemp = T_est[-1].expand(m_max, 5, 5).bmm(SE3_2.exp(xi.cuda()).cpu()) clines = Ttemp[:, :3, 4] plt.plot(clines[:, i1], clines[:, i2], color=color)
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')
def plot_se23_helper(T_est, P_est, color, i1, i2, i_max, path): P_est_chol = torch.cholesky(P_est + torch.eye(9) * 1e-16) r = bmv(P_est_chol.expand(i_max, 9, 9), torch.randn(i_max, 9)) Ttemp = T_est[-1].expand(i_max, 5, 5).bmm(SE3_2.exp(r.cuda()).cpu()) p_est = Ttemp[:, :3, 4] plt.scatter(p_est[:, i1], p_est[:, i2], color=color, s=3)
def main(i_max, k_max, T0, Sigma0, Upsilon, Q, cholQ, dt, g, sigma, m_max): # Generate some random samples T = torch.zeros(i_max, k_max, 5, 5).cuda() T[:, 0] = T0.cuda().repeat(i_max, 1, 1) tmp = Sigma0.sqrt().cuda().expand(i_max, 9, 9) # Pxi assumed diagonal! T[:, 0] = T[:, 0].bmm(SE3_2.exp(bmv(tmp, torch.randn(i_max, 9).cuda()))) Gamma = f_Gamma(g, dt).cuda().expand(i_max, 5, 5) tmp = cholQ.cuda().expand(i_max, 9, 9) tmp2 = Upsilon.cuda().expand(i_max, 5, 5) for k in range(1, k_max): T_k = SE3_2.exp(bmv(tmp, torch.randn(i_max, 9).cuda())) Phi = f_flux(T[:, k - 1], dt) T[:, k] = Gamma.bmm(Phi).bmm(tmp2).bmm(T_k) T = T.cpu() # Propagate the uncertainty using second- and fourth-order methods T_est = torch.zeros(k_max, 5, 5) Sigma2th = torch.zeros(k_max, 9, 9) # second order covariance Sigma4th = torch.zeros(k_max, 9, 9) # fourth order covariance SigmaSO3 = torch.zeros(k_max, 9, 9) # SO(3) x R^6 covariance T_est[0] = T0 Sigma2th[0] = Sigma0.clone() Sigma4th[0] = Sigma0.clone() SigmaSO3[0] = Sigma0.clone() for k in range(1, k_max): # Second-order method T_est[k], Sigma2th[k] = propagate(T_est[k - 1], Sigma2th[k - 1], Upsilon, Q, 0, dt, g) # Fourth-order method _, Sigma4th[k] = propagate(T_est[k - 1], Sigma4th[k - 1], Upsilon, Q, 1, dt, g) # baseline method _, SigmaSO3[k] = propagate(T_est[k - 1], SigmaSO3[k - 1], Upsilon, Q, 2, dt, g) ## Numerical check of paper formulas # Sigma_K = Sigma2th[-1] # sigma = Q[2, 2].sqrt() # K = k_max-1 # a = 1 # Deltat = 0.05 # Sigma_phiphi = K * sigma * sigma # print(Sigma_phiphi, Sigma_K[2, 2]) # Sigma_phiv = -(K-1)/2 * a * Deltat * Sigma_phiphi # print(Sigma_phiv, Sigma_K[2, 4]) # Sigma_phip = (K-1)*(2*K-1)/12 * a * (Deltat**2) * Sigma_phiphi # print(Sigma_phip, Sigma_K[2, 7]) # Sigma_vv = (K-1)*(2*K-1)/6 * ((a * Deltat)**2) * Sigma_phiphi # print(Sigma_vv, Sigma_K[4, 4]) # Sigma_vp = (K-1)**2 * (K)**2 * 1/(8*K) * ((a**2) * (Deltat**3)) * Sigma_phiphi # print(Sigma_vp, Sigma_K[4, 7]) # Sigma_pp = (K-1)*(2*K-1)*(3*(K-1)**2 + 3*K -4)/120 * ((a**2) * (Deltat**4)) * Sigma_phiphi # print(Sigma_pp, Sigma_K[7, 7]) # Plot the random samples' trajectory lines for i in range(i_max): plt.plot(T[i, :, 0, 4], T[i, :, 1, 4], color='gray', alpha=0.1) v = (2 * np.pi * torch.arange(m_max) / (m_max - 1) - np.pi).unsqueeze(1) x = T[:, -1, :3, 4] xmean = torch.mean(x, dim=0) vSigma = bouter(x - xmean, x - xmean).sum(dim=0) / (i_max - 1) # Plot blue dots for random samples plt.scatter(T[:, -1, 0, 4], T[:, -1, 1, 4], s=2, color='black') # Plot the mean of the samples plt.scatter(xmean[0], xmean[1], label='mean', color='orange') # Plot the covariance of the samples T_est2 = T_est.clone() T_est2[-1, :3, 4] = xmean plot_so3_helper(T_est2, vSigma, v, "orange", 0, 1) plt.scatter(T_est[-1, 0, 4], T_est[-1, 1, 4], label='estimation', color='green') plot_se23_helper(T_est, Sigma2th[-1], v, 'green', 0, 1) plot_so3_helper(T_est, SigmaSO3[-1, 6:9, 6:9], v, 'red', 0, 1) plot_se23_helper(T_est, Sigma4th[-1], v, 'cyan', 0, 1) plt.xlabel('x') plt.xlim(left=0) plt.ylabel('y') plt.show()
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)