Delete main_system_measurement.py

archive/main_system_measurement.py

-import numpy as np
-from numpy.random import randn
-from particle_filter import particle_filter
-import matplotlib.pyplot as plt
-import math
-
-
-
-class myStruct:
-    pass
-
-def projection_matrix_left(fx, fy, cx, cy):
-    '''return the projection_matrix_left
-
-    Parameters
-    ----------
-    fx: float
-        fx = f/k, where k is the pixel density, pixels/m, f is the focus length
-        Similarly, fy = f/l
-
-    Returns
-    -------
-    3*4 matrix
-        projection_matrix_left
-
-    '''
-    P_left = np.array([[fx, 0, cx, 0],[0, fy, cy, 0],[0, 0, 1, 0]])
-    return P_left
-
-def projection_matrix_right(fx, fy, cx, cy, R, T):
-    '''return the projection_matrix_right
-
-    Parameters
-    ----------
-    fx: float
-        fx = f/k, where k is the pixel density, pixels/m, f is the focus length
-        Similarly, fy = f/l
-
-    Returns
-    -------
-    3*4 matrix
-        projection_matrix_right
-
-    '''
-    # construct the trasformation matrix, which transforms poses from the frame of camera1 to that of camera2
-    T = T.reshape((3,1))
-    RT = np.vstack((R, T))
-    last_row = np.array([0,0,0,1])
-    RT = np.hstack((RT, last_row))
-    # transform the points from the frame of camera1 to that of camera2, so we can build the Pr
-    P_right = np.array([[fx, 0, cx, 0],[0, fy, cy, 0],[0, 0, 1, 0]])
-    P_right = P_right @ RT
-
-    return P_right
-
-def linear_triangulation(x_left, x_right, P_left, P_right):
-    '''return the 3D and 2D coordinate of the point in the left camera frame
-
-    Parameters
-    ----------
-    x_left, x_right: pixel coordinate in the left image
-        can either be 2*1 or 3*1 homogeneous form
-    P_left, P_right: projection matrices of the left and right cameras
-
-    Returns
-    -------
-    3*1 matrix
-        Z: the 3D coordinate of the point in the left camera frame
-    2*1 matrix
-        Z_2D: the 2D projection coordinate
-
-    '''
-    x_left = x_left.reshape((-1,1))
-    x_right = x_right.reshape((-1,1))
-    # check if they are in homogeneous form, if not, then transform them
-    if x_left.shape == (3,1):
-        pass
-    else:
-        x_left = np.array([x_left[0,0], x_left[1,0], 1]).reshape((3,1))
-
-    if x_right.shape == (3,1):
-        pass
-    else:
-        x_right = np.array([x_right[0,0], x_right[1,0], 1]).reshape((3,1))
-
-
-    A = np.array([[x_left[0,:]*P_left[2,:]-P_left[0,:]],
-                  [x_left[1,:]*P_left[2,:]-P_left[1,:]],
-                  [x_right[0,:]*P_right[2,:]-P_right[0,:]],
-                  [x_right[1,:]*P_right[2,:]-P_right[1,:]]])
-
-    U, S_vector, V_transpose = np.linalg.svd(A, full_matrices=True)
-    V = V_transpose.T
-
-    X = V[:,3].reshape((-1,1))
-    X = X * (1/X[3,:])
-    X = X[0:3,:].reshape((3,1))
-    Z_2D = np.array([X[2,:], X[0,:]]).reshape((2,1))  # (z,x) = (mx, my), check
-    Z = X
-    return Z, Z_2D
-
-def measurement(x_left, x_right, P_left, P_right):
-    # use this function to transform the pixel position measurement to the 2D bearing and range measurement
-    Z, Z_2D = linear_triangulation(x_left, x_right, P_left, P_right)
-    mx = Z_2D[0, 0]
-    my = Z_2D[1, 0]
-    z_br = np.array([[math.atan2(my, mx)],[np.sqrt(my ** 2 + mx ** 2)]])  # z_bearing_range
-    return z_br.reshape([2, 1])
-
-def measurement_model(x_hat, mx_t_minus_1, my_t_minus_1):
-    x_hat = x_hat.reshape((-1,1))
-    mx = mx_t_minus_1
-    my = my_t_minus_1
-    h = np.array([[math.atan2(my - x_hat[1,0], mx - x_hat[0,0]) - x_hat[2,0]], [np.sqrt((my - x_hat[1,0]) ** 2 + (mx - x_hat[0,0]) ** 2)]])
-    return h.reshape([2, 1])
-
-
-
-
-def process_model(x, w):
-    """
-    Think this makes sense because we don't move at all
-    :param x: actually point with respect to frame 1
-    :param w: noise vector sampled as part of the motion model (the vibes )
-    :return:
-    """
-    f = np.array([x[0], x[1], x[2]], dtype=float).reshape([3,1]) + w
-    return f.reshape([3, 1])
-
-def measurement_model_1(K_f, p, c_1):
-    """
-    :param K_f:  intrinisic Camera 1 Matrix
-    :param p: point in frame 1
-    :param c_1: optical center of image camera 1
-    :return: process model camera 1
-    """
-    proj_func= np.array([p[0]/p[2], p[1]/p[2]], dtype=float).reshape((-1,1))
-    return np.dot(K_f, proj_func) + c_1
-
-def measurement_model_2(K_f, p, c_2):
-    """
-    :param K_f: intrinisic Camera 2 Matrix
-    :param p: point in frame 1
-    :param c_1:
-    :return:
-    """
-    p = p.reshape([3,1])
-    q = np.dot(R.T, p) - np.dot(R.T, t)
-    proj_func= np.array([q[0]/q[2], q[1]/q[2]], dtype=float).reshape((-1,1))
-    return np.dot(K_f, proj_func) + c_2
-
-
-if __name__=="__main__":
-
-    C_1 = np.loadtxt(open('data/C_1.csv'), delimiter=",").reshape(-1, 1)
-    C_2 = np.loadtxt(open('data/C_2.csv'), delimiter=",").reshape(-1, 1)
-    Kf_1 = np.loadtxt(open('data/Kf_1.csv'), delimiter=",")
-    Kf_2 = np.loadtxt(open('data/Kf_2.csv'), delimiter=",")
-    R = np.loadtxt(open('data/R.csv'), delimiter=",")
-    t = np.loadtxt(open('data/t.csv'), delimiter=",").reshape(-1, 1)
-    z_1 = np.loadtxt(open('data/z_1.csv'), delimiter=",")
-    z_2 = np.loadtxt(open('data/z_2.csv'), delimiter=",")
-
-    given_data = myStruct()
-    given_data.C_1 = C_1
-    given_data.C_2 =C_2
-    given_data.Kf_1 = Kf_1
-    given_data.Kf_2 = Kf_2
-    given_data.R = R
-    given_data.t = t
-
-
-    # initialize the state using the first measurement
-    init = myStruct()
-    init.x = np.zeros([3,1])
-    init.x[0, 0] = 0.12
-    init.x[1, 0] = 0.09
-    init.x[2, 0] = 1.5
-    init.n = 1000
-    init.Sigma = 0.01*np.eye(3) #Tune
-
-
-    # Build the system
-    sys = myStruct()
-    sys.f = process_model
-    sys.R1 = np.cov(z_1, rowvar=False)  # double check 2by 2
-    sys.R2 = np.cov(z_2, rowvar=False)  # double check 2 by 2
-    sys.h1 = measurement_model_1
-    sys.h2 = measurement_model_2
-    sys.Q1 = np.array([[0.03,0.02,0.01],
-                       [0.02,0.04, 0.01],
-                       [0.01,0.01, 0.05]]).reshape((3,3)) # vibrations
-
-
-    filter = particle_filter(sys, init, given_data)
-    x = np.empty([3, np.shape(z_1)[0]+1]) # used since picked a random init
-    x[:, 0] = [init.x[0, 0], init.x[1, 0], init.x[2, 0]]  # don't need since picked random point
-
-
-    green = np.array([0.2980, .6, 0])
-
-    # started from 0 instead cuz picked random init
-    for i in range(0, np.shape(z_1)[0], 1):
-        filter.sample_motion()
-
-        #####################################################
-        # First measurement
-        z = np.append(z_1[i, :], z_2[i, :]).reshape((-1,1))
-
-        filter.importance_mesurement_batch(z)
-        if filter.Neff < filter.n/5:   # Note sampling threshold is hidden in there
-            filter.resampling() # does this work correctly terms of shapes
-        wtot = np.sum(filter.p.w)
-
-        if wtot  > 0:
-            # a = filter.p.x
-            # b = filter.p.w
-            x[0, i+1] = np.sum(filter.p.x[: , 0] * filter.p.w.reshape(-1)) / wtot
-            x[1 ,i+1] = np.sum(filter.p.x[: , 1] * filter.p.w.reshape(-1))/ wtot
-            x[2, i+1] = np.sum(filter.p.x[:, 2] * filter.p.w.reshape(-1)) / wtot
-        else:
-            print('\033[91mWarning: Total weight is zero or nan!\033[0m')
-            x[:, i+1] = [np.nan, np.nan, np.nan]
-        ############################################################################
-
-
-    # Final Label
-    print('Final x: %.4f, y: %.4f, z: %.4f' % (x[0,-1], x[1,-1], x[2,-1]))
-
-    # quit()
-    fig = plt.figure()
-    # axis = np.linspace()
-    plt.plot(x[0,:], label='px' )
-    plt.plot(x[1,:], label='py')
-    plt.plot(x[2,:], label ='pz')
-    plt.title('Particle Filter Batch ')
-    plt.xlabel(r'time Step')
-    plt.ylabel(r'Position')
-    plt.legend()
-    plt.show()
-
-
-
-    print('here')