<jupyter_start><jupyter_text>Duckietown Lane Filter2 up/down arrow keys to move, enter to editEsc to exit edit modeShift + enter to run code1/2/3... to add commentdd to delete cellpress h for more shortcuts<jupyter_code>import numpy as np
from scipy.stats import multivariate_normal, entropy
from scipy.ndimage.filters import gaussian_filter
from math import floor, atan2, pi, cos, sin, sqrt
import time
from matplotlib import pyplot as plt
%matplotlib inline<jupyter_output><empty_output><jupyter_text>Environment Setup<jupyter_code># constant
WHITE = 0
YELLOW = 1
RED = 2

lanewidth = 0.4
linewidth_white = 0.04
linewidth_yellow = 0.02<jupyter_output><empty_output><jupyter_text>Generate Vote  ground projection<jupyter_code>import cv2
img = cv2.imread("images/curve-right.jpg")

dst1 = cv2.cvtColor(img,cv2.COLOR_BGR2RGB)
plt.subplot(111),plt.imshow(dst1,cmap = 'brg')
plt.title('Original Image'), plt.xticks([]), plt.yticks([])

# default homography, you should replace this by your extrinsic
H = np.array(
    [[-4.89775e-05, -0.0002150858, -0.1818273], 
     [0.00099274, 1.202336e-06, -0.3280241],
     [-0.0004281805, -0.007185673, 1]])
# normalized x, y on image plane
img_pt_u = 0.2 # from left
img_pt_v = 0.5 # from top

print H
x_img = np.array([ img_pt_u *640, img_pt_v *480, 1])
print x_img

x_gnd = np.dot(H, x_img)
print x_gnd

x_gnd /= x_gnd[2]
print 'front: ', x_gnd[0]
print 'd\t', x_gnd[1]<jupyter_output>[[ -4.89775000e-05  -2.15085800e-04  -1.81827300e-01]
 [  9.92740000e-04   1.20233600e-06  -3.28024100e-01]
 [ -4.28180500e-04  -7.18567300e-03   1.00000000e+00]]
[ 128.  240.    1.]
[-0.23971701 -0.20066482 -0.77936862]
front:  0.307578473931
d	0.257471000475<jupyter_text>Setup a line segment* left edge of white lane* right edge of white lane* left edge of yellow lane* right edge of white lane<jupyter_code># right edge of white lane
p1 = np.array([0.4, 0.257])
p2 = np.array([0.3, 0.257])
seg_color = YELLOW<jupyter_output><empty_output><jupyter_text>compute d_i, phi_i, l_i<jupyter_code>t_hat = (p2-p1)/np.linalg.norm(p2-p1)
n_hat = np.array([-t_hat[1],t_hat[0]])
d1 = np.inner(n_hat,p1)
d2 = np.inner(n_hat,p2)
l1 = np.inner(t_hat,p1)
l2 = np.inner(t_hat,p2)

print (d1, d2, l1, l2)

if (l1 < 0):
    l1 = -l1;
if (l2 < 0):
    l2 = -l2;
l_i = (l1+l2)/2
d_i = (d1+d2)/2
phi_i = np.arcsin(t_hat[1])
if seg_color == WHITE: # right lane is white
    if(p1[0] > p2[0]): # right edge of white lane
        d_i = d_i - linewidth_white
        print ('right edge of white lane')
    else: # left edge of white lane
        d_i = - d_i
        phi_i = -phi_i
        print ('left edge of white lane')
    d_i = d_i - lanewidth/2

elif seg_color == YELLOW: # left lane is yellow
    if (p2[0] > p1[0]): # left edge of yellow lane
        d_i = d_i - linewidth_yellow
        phi_i = -phi_i
        print ('right edge of yellow lane')
    else: # right edge of white lane
        d_i = -d_i
        print ('right edge of yellow lane')
    d_i =  lanewidth/2 - d_i

    
print (d_i, phi_i, l_i)<jupyter_output>(-0.25700000000000001, -0.25700000000000001, -0.40000000000000002, -0.29999999999999999)
right edge of yellow lane
(-0.056999999999999995, 0.0, 0.34999999999999998)<jupyter_text>Measurement Likelihood<jupyter_code># initialize measurement likelihood

d_min = -0.7
d_max = 0.5
delta_d = 0.02
phi_min = -pi/2
phi_max = pi/2
delta_phi = 0.02
d, phi = np.mgrid[d_min:d_max:delta_d, phi_min:phi_max:delta_phi]

measurement_likelihood = np.zeros(d.shape)

fig = plt.figure()
ax = fig.add_subplot(111)
cax = ax.matshow(measurement_likelihood, interpolation='nearest')
fig.colorbar(cax)
plt.ylabel('phi')
plt.xlabel('d')

plt.show()


i = floor((d_i - d_min)/delta_d)
j = floor((phi_i - phi_min)/delta_phi)
measurement_likelihood[i,j] = measurement_likelihood[i,j] +  1/(l_i)

print (i, j)

fig = plt.figure()
ax = fig.add_subplot(111)
cax = ax.matshow(measurement_likelihood, interpolation='nearest')
fig.colorbar(cax)
plt.ylabel('phi')
plt.xlabel('d')

plt.show()<jupyter_output>/home/robotvision/anaconda2/lib/python2.7/site-packages/ipykernel/__main__.py:3: DeprecationWarning: using a non-integer number instead of an integer will result in an error in the future
  app.launch_new_instance()<jupyter_text>Process Model  1. Initialize Belief<jupyter_code>print d.shape
pos = np.empty(d.shape + (2,))
pos[:,:,0]= d
pos[:,:,1]= phi

print pos.shape
mean_0 = [0, 0]
cov_0 = [[0.1, 0], [0, 0.01]]

RV = multivariate_normal(mean_0, cov_0)
beliefRV=RV.pdf(pos)

plt.ylabel('phi')
plt.xlabel('d')
plt.contourf(d, phi, beliefRV)<jupyter_output><empty_output><jupyter_text>2.1 Update Belief<jupyter_code>beliefRV=np.multiply(beliefRV+1,measurement_likelihood+1)-1
beliefRV=beliefRV/np.sum(beliefRV)

plt.ylabel('phi')
plt.xlabel('d')
plt.contourf(d, phi, beliefRV)
maxids = np.unravel_index(beliefRV.argmax(),beliefRV.shape)
print maxids
lanePose_d = d_min + maxids[0] * delta_d
lanePose_phi = phi_min + maxids[1] * delta_phi

print lanePose_d
print lanePose_phi<jupyter_output>(32, 78)
-0.06
-0.0107963267949
{'fraction_non_alphanumeric': 0.14044161527750149, 'fraction_numerical': 0.10145215834493734, 'mean_word_length': 4.383297644539614, 'pattern_counts': {'":': 0, '<': 31, '<?xml version=': 0, '>': 31, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 7, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '8', 'dir': 'jupyter-structured-clean-dedup', 'id': '529732', 'license': 'CC-BY-2.0', 'n_tokens_mistral': 2443, 'n_tokens_neox': 2132, 'n_words': 468, 'path': 'tutorials/python/lane_filter2.ipynb', 'repo_name': 'dimension4TW/Duckie-Servant-Robot', 'size': '5027'}