Beispiel #1
0
def error_and_plotting(m_res,
                       Res,
                       m_a,
                       l_a,
                       w_a,
                       al_a,
                       m_b,
                       l_b,
                       w_b,
                       al_b,
                       m_gt,
                       l_gt,
                       w_gt,
                       al_gt,
                       plotting,
                       name,
                       filename,
                       est_color='red'):
    # turn result back and compare
    l_res, w_res, al_res = get_ellipse_params(Res)

    if plotting:
        plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
                      w_gt, al_gt, m_res, l_res, w_res, al_res, name, filename,
                      est_color)

    return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, m_res, l_res, w_res,
                             al_res)
Beispiel #2
0
def best_or(m_a, cov_a, l_a, w_a, al_a, m_b, cov_b, l_b, w_b, al_b, m_gt, l_gt,
            w_gt, al_gt, plotting):
    [res_orig_alt, cov, k] = best_fusion(m_a, cov_a, l_a, w_a, al_a, m_b,
                                         cov_b, l_b, w_b, al_b)

    if plotting:
        plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
                      w_gt, al_gt, res_orig_alt[:2], res_orig_alt[3],
                      res_orig_alt[4], res_orig_alt[2],
                      'Linearized Approximated Fusion With Best Orientation',
                      'exampleRegAltFus.svg')

    return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, res_orig_alt[:2],
                             res_orig_alt[3], res_orig_alt[4], res_orig_alt[2])
Beispiel #3
0
def shape_mean_k(m_a, A, l_a, w_a, al_a, m_b, B, l_b, w_b, al_b, m_gt, l_gt,
                 w_gt, al_gt, plotting):
    m_res_alt2 = 0.5 * (m_a + m_b)
    Res_alt2 = 0.5 * (A + B) + 0.25 * np.outer(m_a - m_b, m_a - m_b)
    ellipse_axisalt2, valt2 = np.linalg.eig(Res_alt2)
    ellipse_axisalt2 = np.sqrt(ellipse_axisalt2)
    l_res_alt2 = ellipse_axisalt2[0]
    w_res_alt2 = ellipse_axisalt2[1]
    al_res_alt2 = np.arctan2(valt2[1, 0], valt2[0, 0])

    if plotting:
        plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
                      w_gt, al_gt, m_res_alt2, l_res_alt2, w_res_alt2,
                      al_res_alt2, 'Shape Mean', 'exampleMean.svg')

    return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, m_res_alt2, l_res_alt2,
                             w_res_alt2, al_res_alt2)
Beispiel #4
0
def ord_fusion(m_a, cov_a, l_a, w_a, al_a, m_b, cov_b, l_b, w_b, al_b, m_gt,
               l_gt, w_gt, al_gt, plotting):
    S_orig = cov_a + cov_b
    if np.linalg.det(S_orig) == 0:
        print('Singular S_orig')
        # print(S_orig)

    K_orig = np.dot(cov_a, np.linalg.inv(S_orig))
    res_orig = np.array([m_a[0], m_a[1], al_a, l_a, w_a]) + np.dot(
        K_orig,
        np.array([m_b[0], m_b[1], al_b, l_b, w_b]) -
        np.array([m_a[0], m_a[1], al_a, l_a, w_a]))

    if plotting:
        plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
                      w_gt, al_gt, res_orig[:2], res_orig[3], res_orig[4],
                      res_orig[2], 'Fusion of Original State',
                      'exampleRegFus.svg')

    return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, res_orig[:2],
                             res_orig[3], res_orig[4], res_orig[2])
Beispiel #5
0
def lin_approx(m_a, cov_a, A, l_a, w_a, al_a, m_b, cov_b, B, l_b, w_b, al_b,
               m_gt, l_gt, w_gt, al_gt, plotting):
    jac_a = np.array([
        [1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0],
        [
            0, 0, (w_a**2 - l_a**2) * np.sin(2 * al_a),
            2 * l_a * np.cos(al_a)**2, 2 * w_a * np.sin(al_a)**2
        ],
        [
            0, 0, (l_a**2 - w_a**2) * np.cos(2 * al_a),
            2 * l_a * np.cos(al_a) * np.sin(al_a),
            -2 * w_a * np.sin(al_a) * np.cos(al_a)
        ],
        [
            0, 0, (l_a**2 - w_a**2) * np.sin(2 * al_a),
            2 * l_a * np.sin(al_a)**2, 2 * w_a * np.cos(al_a)**2
        ],
    ])
    jac_b = np.array([
        [1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0],
        [
            0, 0, (w_b**2 - l_b**2) * np.sin(2 * al_b),
            2 * l_b * np.cos(al_b)**2, 2 * w_b * np.sin(al_b)**2
        ],
        [
            0, 0, (l_b**2 - w_b**2) * np.cos(2 * al_b),
            2 * l_b * np.cos(al_b) * np.sin(al_b),
            -2 * w_b * np.sin(al_b) * np.cos(al_b)
        ],
        [
            0, 0, (l_b**2 - w_b**2) * np.sin(2 * al_b),
            2 * l_b * np.sin(al_b)**2, 2 * w_b * np.cos(al_b)**2
        ],
    ])
    s_a = 2 * np.sqrt(A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1])
    t_a = np.sqrt(A[0, 0] + A[1, 1] + s_a)
    s_a_al = (jac_a[2, 2] * A[1, 1] + jac_a[4, 2] * A[0, 0] - 2 * jac_a[3, 2] *
              A[1, 0]) / np.sqrt(A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1])
    t_a_al = (jac_a[2, 2] + jac_a[4, 2] + s_a_al) / (2 * t_a)
    s_a_l = (jac_a[2, 3] * A[1, 1] + jac_a[4, 3] * A[0, 0] - 2 * jac_a[3, 3] *
             A[1, 0]) / np.sqrt(A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1])
    t_a_l = (jac_a[2, 3] + jac_a[4, 3] + s_a_al) / (2 * t_a)
    s_a_w = (jac_a[2, 4] * A[1, 1] + jac_a[4, 4] * A[0, 0] - 2 * jac_a[3, 4] *
             A[1, 0]) / np.sqrt(A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1])
    t_a_w = (jac_a[2, 4] + jac_a[4, 4] + s_a_al) / (2 * t_a)
    jac_a2 = np.array([
        [1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0],
        [
            0, 0,
            -t_a_al * (A[0, 0] + s_a) / t_a**2 + (jac_a[2, 2] + s_a_al) / t_a,
            -t_a_l * (A[0, 0] + s_a) / t_a**2 + (jac_a[2, 3] + s_a_l) / t_a,
            -t_a_w * (A[0, 0] + s_a) / t_a**2 + (jac_a[2, 4] + s_a_w) / t_a
        ],
        [
            0, 0, -t_a_al * A[1, 0] / t_a**2 + jac_a[3, 2] / t_a,
            -t_a_l * A[1, 0] / t_a**2 + jac_a[3, 3] / t_a,
            -t_a_w * A[1, 0] / t_a**2 + jac_a[3, 4] / t_a
        ],
        [
            0, 0,
            -t_a_al * (A[1, 1] + s_a) / t_a**2 + (jac_a[4, 2] + s_a_al) / t_a,
            -t_a_l * (A[1, 1] + s_a) / t_a**2 + (jac_a[4, 3] + s_a_l) / t_a,
            -t_a_w * (A[1, 1] + s_a) / t_a**2 + (jac_a[4, 4] + s_a_w) / t_a
        ],
    ])
    s_b = 2 * np.sqrt(B[0, 0] * B[1, 1] - B[1, 0] * B[0, 1])
    t_b = np.sqrt(B[0, 0] + B[1, 1] + s_b)
    s_b_al = (jac_b[2, 2] * B[1, 1] + jac_b[4, 2] * B[0, 0] - 2 * jac_b[3, 2] *
              B[1, 0]) / np.sqrt(B[0, 0] * B[1, 1] - B[1, 0] * B[0, 1])
    t_b_al = (jac_b[2, 2] + jac_b[4, 2] + s_b_al) / (2 * t_b)
    s_b_l = (jac_b[2, 3] * B[1, 1] + jac_b[4, 3] * B[0, 0] - 2 * jac_b[3, 3] *
             B[1, 0]) / np.sqrt(B[0, 0] * B[1, 1] - B[1, 0] * B[0, 1])
    t_b_l = (jac_b[2, 3] + jac_b[4, 3] + s_b_al) / (2 * t_b)
    s_b_w = (jac_b[2, 4] * B[1, 1] + jac_b[4, 4] * B[0, 0] - 2 * jac_b[3, 4] *
             B[1, 0]) / np.sqrt(B[0, 0] * B[1, 1] - B[1, 0] * B[0, 1])
    t_b_w = (jac_b[2, 4] + jac_b[4, 4] + s_b_al) / (2 * t_b)
    jac_b2 = np.array([
        [1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0],
        [
            0, 0,
            -t_b_al * (B[0, 0] + s_b) / t_b**2 + (jac_b[2, 2] + s_b_al) / t_b,
            -t_b_l * (B[0, 0] + s_b) / t_b**2 + (jac_b[2, 3] + s_b_l) / t_b,
            -t_b_w * (B[0, 0] + s_b) / t_b**2 + (jac_b[2, 4] + s_b_w) / t_b
        ],
        [
            0, 0, -t_b_al * B[1, 0] / t_b**2 + jac_b[3, 2] / t_b,
            -t_b_l * B[1, 0] / t_b**2 + jac_b[3, 3] / t_b,
            -t_b_w * B[1, 0] / t_b**2 + jac_b[3, 4] / t_b
        ],
        [
            0, 0,
            -t_b_al * (B[1, 1] + s_b) / t_b**2 + (jac_b[4, 2] + s_b_al) / t_b,
            -t_b_l * (B[1, 1] + s_b) / t_b**2 + (jac_b[4, 3] + s_b_l) / t_b,
            -t_b_w * (B[1, 1] + s_b) / t_b**2 + (jac_b[4, 4] + s_b_w) / t_b
        ],
    ])
    sqrt_A = sqrtm(A)
    sqrt_B = sqrtm(B)
    vec_A_direct = np.array(
        [m_a[0], m_a[1], sqrt_A[0, 0], sqrt_A[0, 1], sqrt_A[1, 1]])
    vec_B_direct = np.array(
        [m_b[0], m_b[1], sqrt_B[0, 0], sqrt_B[0, 1], sqrt_B[1, 1]])
    S_lin2 = np.dot(np.dot(jac_a2, cov_a), jac_a2.T) + np.dot(
        np.dot(jac_b2, cov_b), jac_b2.T)

    K2 = np.dot(np.dot(np.dot(jac_a2, cov_a), jac_a2.T), np.linalg.inv(S_lin2))
    res_lin_vec2 = vec_A_direct + np.dot(K2, vec_B_direct - vec_A_direct)
    res_lin_S2 = np.array([
        [res_lin_vec2[2], res_lin_vec2[3]],
        [res_lin_vec2[3], res_lin_vec2[4]],
    ])
    res_lin_S2 = np.dot(res_lin_S2, res_lin_S2)
    ellipse_axislin2, vlin2 = np.linalg.eig(res_lin_S2)
    ellipse_axislin2 = np.sqrt(ellipse_axislin2)
    l_res_lin2 = ellipse_axislin2[0]
    w_res_lin2 = ellipse_axislin2[1]
    al_res_lin2 = np.arctan2(vlin2[1, 0], vlin2[0, 0])

    if plotting:
        plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
                      w_gt, al_gt, res_lin_vec2[:2], l_res_lin2, w_res_lin2,
                      al_res_lin2, 'Linearized Approximated Fusion',
                      'exampleGWFus.svg')

    return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, res_lin_vec2[:2],
                             l_res_lin2, w_res_lin2, al_res_lin2)
Beispiel #6
0
def test_convergence_pos(steps, runs, m_prior, l_prior, w_prior, al_prior,
                         cov_a, cov_b, n_particles):
    save_path = '/home/kthormann/Desktop/research-fusion/research-fusion/Python/EllipseFusion/convergence/'
    error = np.zeros(steps)
    error_reg = np.zeros(steps)
    error_shape = np.zeros(steps)
    error_params = np.zeros(steps)
    error_lin = np.zeros(steps)

    for r in range(runs):
        print('Run %i of %i' % (r, runs))
        # create gt with sensor A as prior
        m_gt = mvn(m_prior, cov_a[:2, :2])

        l_gt = np.maximum(normal(l_prior, cov_a[3, 3]), 0.1)
        w_gt = np.maximum(normal(w_prior, cov_a[4, 4]),
                          0.1)  # np.maximum(normal(w_gt, cov_a[4, 4]), 0.1)
        al_gt = normal(al_prior, cov_a[2, 2])  # - 0.1 * np.pi
        al_gt %= (2 * np.pi)

        # get prior in square root space
        m_prior, el_sr_prior, cov_sr_prior = single_particle_approx_gaussian(
            m_prior, l_prior, w_prior, al_prior, cov_a, n_particles, True)

        # get prior for regular state
        prior_reg = np.array(
            [m_prior[0], m_prior[1], al_prior, l_prior, w_prior])
        cov_prior_reg = cov_a.copy()

        # get prior for best params
        prior_params = np.copy(prior_reg)
        cov_prior_params = np.copy(cov_prior_reg)

        # get prior for shape mean
        prior_shape_m = m_prior
        prior_shape_l = l_prior
        prior_shape_w = w_prior
        prior_shape_al = al_prior
        rot_prior = np.array([
            [np.cos(al_prior), -np.sin(al_prior)],
            [np.sin(al_prior), np.cos(al_prior)],
        ])
        prior_shape = np.dot(np.dot(rot_prior,
                                    np.diag([l_prior, w_prior])**2),
                             rot_prior.T)

        # get prior for linearization
        prior_shape_sqrt = sqrtm(prior_shape)
        prior_lin = np.array([
            m_prior[0], m_prior[1], prior_shape_sqrt[0, 0],
            prior_shape_sqrt[1, 0], prior_shape_sqrt[1, 1]
        ])
        prior_lin_l = l_prior
        prior_lin_w = w_prior
        prior_lin_al = al_prior
        jac = get_jacobian(l_prior, w_prior, al_prior)
        prior_lin_cov = np.dot(np.dot(jac, cov_prior_reg), jac.T)

        # test different methods =======================================================================================
        for i in range(steps):
            if i % 10 == 0:
                print(i)

            # create measurement from gt (using alternating sensors)
            if (i % 2) == 0:
                m_m = mvn(m_gt, cov_b[:2, :2])
                l_m = np.maximum(normal(w_gt, cov_b[3, 3]), 0.1)
                w_m = np.maximum(normal(l_gt, cov_b[4, 4]), 0.1)
                al_m = normal(al_gt, cov_b[2, 2]) + 0.5 * np.pi
                al_m %= (2 * np.pi)
                cov_m = cov_b.copy()
            else:
                m_m = mvn(m_gt, cov_a[:2, :2])
                l_m = np.maximum(normal(l_gt, cov_a[3, 3]), 0.1)
                w_m = np.maximum(normal(w_gt, cov_a[4, 4]), 0.1)
                al_m = normal(al_gt, cov_a[2, 2])
                al_m %= (2 * np.pi)
                cov_m = cov_a.copy()

            # MC approximation =========================================================================================

            # get measurement in square root space
            m_m, el_sr_m, cov_sr_m = single_particle_approx_gaussian(
                m_m, l_m, w_m, al_m, cov_m, n_particles, True)

            # get posterior in square root space
            # fuse A and B via mean and variance
            S = cov_sr_prior + cov_sr_m
            K = np.dot(cov_sr_prior, np.linalg.inv(S))
            el_sr_post = el_sr_prior + np.dot(K, el_sr_m - el_sr_prior)
            cov_sr_post = cov_sr_prior - np.dot(np.dot(K, S), K.T)

            # var_Res = var_A - np.dot(np.dot(K, S), K.T)
            el_sr_res = np.array([
                [el_sr_post[2], el_sr_post[3]],
                [el_sr_post[3], el_sr_post[4]],
            ])
            el_res = np.dot(el_sr_res, el_sr_res)

            error[i] += error_and_plotting(el_sr_post[:2],
                                           el_res,
                                           el_sr_prior[:2],
                                           l_prior,
                                           w_prior,
                                           al_prior,
                                           m_m,
                                           l_m,
                                           w_m,
                                           al_m,
                                           m_gt,
                                           l_gt,
                                           w_gt,
                                           al_gt, (r + 1) == runs,
                                           'MC Approximated Fusion',
                                           save_path +
                                           'exampleMCApprox%i.svg' % i,
                                           est_color='green')

            el_sr_prior = el_sr_post
            cov_sr_prior = cov_sr_post

            l_prior, w_prior, al_prior = get_ellipse_params(el_res)

            # regular fusion ===========================================================================================

            # get posterior in square root space
            # fuse A and B via mean and variance
            S = cov_prior_reg + cov_m
            K = np.dot(cov_prior_reg, np.linalg.inv(S))
            post_reg = prior_reg + np.dot(
                K,
                np.array([m_m[0], m_m[1], al_m, l_m, w_m]) - prior_reg)
            cov_post_reg = cov_prior_reg - np.dot(np.dot(K, S), K.T)

            if (r + 1) == runs:
                plot_ellipses(prior_reg[:2], prior_reg[3], prior_reg[4],
                              prior_reg[2], m_m, l_m, w_m, al_m, m_gt, l_gt,
                              w_gt, al_gt, post_reg[:2], post_reg[3],
                              post_reg[4], post_reg[2],
                              'Fusion of Original State',
                              save_path + 'exampleRegFus%i.svg' % i)

            error_reg[i] += gauss_wasserstein(m_gt, l_gt, w_gt, al_gt,
                                              post_reg[:2], post_reg[3],
                                              post_reg[4], post_reg[2])

            prior_reg = post_reg
            cov_prior_reg = cov_post_reg

            # best params fusion =======================================================================================
            [post_params, cov_post_params,
             k] = best_fusion(prior_params[:2], cov_prior_params,
                              prior_params[3], prior_params[4],
                              prior_params[2], m_m, cov_m, l_m, w_m, al_m)

            if (r + 1) == runs:
                plot_ellipses(prior_params[:2], prior_params[3],
                              prior_params[4], prior_params[2], m_m, l_m, w_m,
                              al_m, m_gt, l_gt, w_gt, al_gt, post_params[:2],
                              post_params[3], post_params[4], post_params[2],
                              'Best Params',
                              save_path + 'exampleBestParams%i.svg' % i)

            error_params[i] += gauss_wasserstein(m_gt, l_gt, w_gt, al_gt,
                                                 post_params[:2],
                                                 post_params[3],
                                                 post_params[4],
                                                 post_params[2])

            prior_params = post_params
            cov_prior_params = cov_post_params

            # shape mean ===============================================================================================
            post_shape_m = 0.5 * (prior_shape_m + m_m)
            rot_m = np.array([
                [np.cos(al_m), -np.sin(al_m)],
                [np.sin(al_m), np.cos(al_m)],
            ])
            m_shape = np.dot(np.dot(rot_m, np.diag([l_m, w_m])**2), rot_m.T)
            post_shape = 0.5 * (prior_shape + m_shape)

            post_shape_l, post_shape_w, post_shape_al = get_ellipse_params(
                post_shape)

            if (r + 1) == runs:
                plot_ellipses(prior_shape_m, prior_shape_l, prior_shape_w,
                              prior_shape_al, m_m, l_m, w_m, al_m, m_gt, l_gt,
                              w_gt, al_gt, post_shape_m, post_shape_l,
                              post_shape_w, post_shape_al, 'Shape Mean',
                              'exampleMean.svg')

            error_shape[i] += gauss_wasserstein(m_gt, l_gt, w_gt, al_gt,
                                                post_shape_m, post_shape_l,
                                                post_shape_w, post_shape_al)

            prior_shape_m = post_shape_m
            prior_shape = post_shape
            prior_shape_l = post_shape_l
            prior_shape_w = post_shape_w
            prior_shape_al = post_shape_al

            # linearization ============================================================================================
            m_shape_sqrt = sqrtm(m_shape)
            lin_m = np.array([
                m_m[0], m_m[1], m_shape_sqrt[0, 0], m_shape_sqrt[1, 0],
                m_shape_sqrt[1, 1]
            ])
            jac_m = get_jacobian(l_m, w_m, al_m)
            cov_m_lin = np.dot(np.dot(jac_m, cov_m), jac_m.T)
            S_lin = prior_lin_cov + cov_m_lin
            K_lin = np.dot(prior_lin_cov, np.linalg.inv(S_lin))
            post_lin = prior_lin + np.dot(K_lin, lin_m - prior_lin)
            post_lin_cov = prior_lin_cov - np.dot(np.dot(K_lin, S_lin),
                                                  K_lin.T)

            post_lin_shape = np.array([
                [post_lin[2], post_lin[3]],
                [post_lin[3], post_lin[4]],
            ])
            post_lin_shape = np.dot(post_lin_shape, post_lin_shape)
            ellipse_axis_post_lin, v_post_lin = np.linalg.eig(post_lin_shape)
            ellipse_axis_post_lin = np.sqrt(ellipse_axis_post_lin)
            post_lin_l, post_lin_w, post_lin_al = get_ellipse_params(
                post_lin_shape)

            if (r + 1) == runs:
                plot_ellipses(prior_lin[:2], prior_lin_l, prior_lin_w,
                              prior_lin_al, m_m, l_m, w_m, al_m, m_gt, l_gt,
                              w_gt, al_gt, post_lin[:2], post_lin_l,
                              post_lin_w, post_lin_al, 'Shape Mean',
                              'exampleMean.svg')

            error_lin[i] += gauss_wasserstein(m_gt, l_gt, w_gt, al_gt,
                                              post_lin[:2], post_lin_l,
                                              post_lin_w, post_lin_al)

            prior_lin = post_lin
            prior_lin_cov = post_lin_cov
            prior_lin_l = post_lin_l
            prior_line_w = post_lin_w
            prior_lin_al = post_lin_al

    error = np.sqrt(error / runs)
    error_reg = np.sqrt(error_reg / runs)
    error_shape = np.sqrt(error_shape / runs)
    error_params = np.sqrt(error_params / runs)
    error_lin = np.sqrt(error_lin / runs)

    print(error)
    print(error_params)

    # error first and last
    bars = np.array([1, 3, 5, 7, 9])
    ticks = np.array([
        'Regular', 'Mean of\n shape matrix', 'MMGW-Lin', 'Heuristic', 'MMGW-MC'
    ])
    plt.bar(0.825, error_reg[0], width=0.25, color='red', align='center')
    plt.bar(1.175, error_reg[-1], width=0.25, color='red', align='center')
    plt.bar(2.825, error_shape[0], width=0.25, color='m', align='center')
    plt.bar(3.175, error_shape[-1], width=0.25, color='m', align='center')
    plt.bar(4.825,
            error_lin[0],
            width=0.25,
            color='deepskyblue',
            align='center')
    plt.bar(5.175,
            error_lin[-1],
            width=0.25,
            color='deepskyblue',
            align='center')
    plt.bar(6.825,
            error_params[0],
            width=0.25,
            color='darkcyan',
            align='center')
    plt.bar(7.175,
            error_params[-1],
            width=0.25,
            color='darkcyan',
            align='center')
    plt.bar(8.825, error[0], width=0.25, color='green', align='center')
    plt.bar(9.175, error[-1], width=0.25, color='green', align='center')
    plt.xticks(bars, ticks)
    plt.title('GW RMSE')
    plt.ylim(0.0, 1.5)
    plt.savefig('gwRmse.svg')
    plt.show()

    plt.plot(np.arange(1, steps + 1), error)
    plt.savefig(save_path + 'MCApproxError.svg')
    plt.show()
    plt.plot(np.arange(1, steps + 1), error_reg)
    plt.savefig(save_path + 'regFusError.svg')
    plt.show()
    plt.plot(np.arange(1, steps + 1),
             error_reg,
             color='red',
             label='Regular State')
    plt.plot(np.arange(1, steps + 1),
             error_shape,
             color='m',
             label='Mean of shape matrix')
    plt.plot(np.arange(1, steps + 1),
             error_lin,
             color='deepskyblue',
             label='MMGW-Lin')
    plt.plot(np.arange(1, steps + 1),
             error_params,
             color='darkcyan',
             label='Heuristic')
    plt.plot(np.arange(1, steps + 1), error, color='green', label='MMGW-MC')
    plt.legend()
    plt.xticks([5, 10, 15, 20])
    plt.savefig(save_path + 'errorComp.svg')
    plt.show()
Beispiel #7
0
def test_convergence(steps, m_prior, l_prior, w_prior, al_prior, cov_a, cov_b,
                     n_particles):
    save_path = '/home/convergence/'
    error = np.zeros(steps)
    error_reg = np.zeros(steps)

    # create gt with sensor A as prior
    m_gt = m_prior  # mvn(m_a, cov_a[:2, :2])

    l_gt = np.maximum(normal(l_prior, cov_a[3, 3]), 0.1)
    w_gt = np.maximum(normal(w_prior, cov_a[4, 4]),
                      0.1)  # np.maximum(normal(w_gt, cov_a[4, 4]), 0.1)
    al_gt = normal(al_prior, cov_a[2, 2])  # - 0.1 * np.pi
    al_gt %= (2 * np.pi)

    # get prior in square root space
    m_prior, el_sr_prior, cov_sr_prior = single_particle_approx_gaussian(
        m_prior, l_prior, w_prior, al_prior, cov_a, n_particles, False)

    # get prior for regular state
    m_prior_reg = m_prior.copy()
    prior_reg = np.array([al_prior, l_prior, w_prior])
    cov_prior_reg = cov_a.copy()[2:, 2:]

    # test different methods ===========================================================================================
    for i in range(steps):
        if i % 10 == 0:
            print(i)

        # create measurement from gt (using alternating sensors)
        if (i % 2) == 0:
            m_m = m_gt  # mvn(m_gt, cov_b[:2, :2])
            l_m = np.maximum(normal(w_gt, cov_b[3, 3]), 0.1)
            w_m = np.maximum(normal(l_gt, cov_b[4, 4]), 0.1)
            al_m = normal(al_gt, cov_b[2, 2]) + 0.5 * np.pi
            al_m %= (2 * np.pi)
            cov_m = cov_b.copy()
        else:
            m_m = m_gt  # mvn(m_gt, cov_a[:2, :2])
            l_m = np.maximum(normal(l_gt, cov_a[3, 3]), 0.1)
            w_m = np.maximum(normal(w_gt, cov_a[4, 4]), 0.1)
            al_m = normal(al_gt, cov_a[2, 2])
            al_m %= (2 * np.pi)
            cov_m = cov_a.copy()

        # MC approximation =============================================================================================

        # get measurement in square root space
        m_m, el_sr_m, cov_sr_m = single_particle_approx_gaussian(
            m_m, l_m, w_m, al_m, cov_m, n_particles, False)

        # get posterior in square root space
        # fuse A and B via mean and variance
        S = cov_sr_prior + cov_sr_m
        K = np.dot(cov_sr_prior, np.linalg.inv(S))
        el_sr_post = el_sr_prior + np.dot(K, el_sr_m - el_sr_prior)
        cov_sr_post = cov_sr_prior - np.dot(np.dot(K, S), K.T)

        # var_Res = var_A - np.dot(np.dot(K, S), K.T)
        m_post = 0.5 * (m_prior + m_m)  # mean_Res[:2]
        el_sr_res = np.array([
            [el_sr_post[0], el_sr_post[1]],
            [el_sr_post[1], el_sr_post[2]],
        ])
        el_res = np.dot(el_sr_res, el_sr_res)

        error[i] = error_and_plotting(m_post, el_res, m_prior, l_prior,
                                      w_prior, al_prior, m_m, l_m, w_m, al_m,
                                      m_gt, l_gt, w_gt, al_gt, True,
                                      'MC Approximated Fusion',
                                      save_path + 'exampleMCApprox%i.svg' % i)

        m_prior = m_post
        el_sr_prior = el_sr_post
        cov_sr_prior = cov_sr_post

        l_prior, w_prior, al_prior = get_ellipse_params(el_res)

        # regular fusion ===============================================================================================

        # get posterior in square root space
        # fuse A and B via mean and variance
        S = cov_prior_reg + cov_m[2:, 2:]
        K = np.dot(cov_prior_reg, np.linalg.inv(S))
        post_reg = prior_reg + np.dot(K,
                                      np.array([al_m, l_m, w_m]) - prior_reg)
        cov_post_reg = cov_prior_reg - np.dot(np.dot(K, S), K.T)

        m_post_reg = 0.5 * (m_prior_reg + m_m)

        plot_ellipses(m_prior, prior_reg[1], prior_reg[2], prior_reg[0], m_m,
                      l_m, w_m, al_m, m_gt, l_gt, w_gt, al_gt, m_post_reg,
                      post_reg[1], post_reg[2], post_reg[0],
                      'Fusion of Original State',
                      save_path + 'exampleRegFus%i.svg' % i)

        error_reg[i] = gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, m_post_reg,
                                         post_reg[1], post_reg[2], post_reg[0])

        m_prior_reg = m_post_reg
        prior_reg = post_reg
        cov_prior_reg = cov_post_reg

    plt.plot(np.arange(1, steps + 1), error)
    plt.savefig(save_path + 'MCApproxError.svg')
    plt.show()
    plt.plot(np.arange(1, steps + 1), error_reg)
    plt.savefig(save_path + 'regFusError.svg')
    plt.show()
    plt.plot(np.arange(1, steps + 1), error, color='green', label='MC Fusion')
    plt.plot(np.arange(1, steps + 1),
             error_reg,
             color='red',
             label='Regular Fusion')
    plt.legend()
    plt.savefig(save_path + 'errorComp.svg')
    plt.show()