Skip to content
Snippets Groups Projects
Commit bd3ced98 authored by ziminlu's avatar ziminlu
Browse files

Upload New File

parent a9ec76a4
No related tags found
No related merge requests found
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')
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment