Exemple #1
0
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
Exemple #2
0
    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')