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')