In [346]:
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import skvideo.io
import cv2
import scipy.ndimage
from matplotlib import animation, rc
import fisheye
import encoder_math
from collections import namedtuple
%matplotlib inline
In [29]:
def get_frame(timestamp, fps=30):
return np.round(timestamp / (1.0 / fps)).astype(int)
In [137]:
df = pd.read_csv('C:/Users/corey/Desktop/output.csv')
df['timestamp'] = df['timestamp'] - df['timestamp'].min()
df['encoder0_count'] = df['encoder0_count'] - df['encoder0_count'].min()
df['encoder1_count'] = df['encoder1_count'] - df['encoder1_count'].min()
df['encoder0_diff'] = df['encoder0_count'].diff()
df['encoder1_diff'] = df['encoder1_count'].diff()
df['frame'] = get_frame(df['timestamp'])
df
Out[137]:
In [6]:
filename = 'C:/Users/corey/Desktop/output.mp4'
videodata = skvideo.io.vread(filename)
print(videodata.shape)
In [15]:
df.iloc[10,:].timestamp
Out[15]:
In [52]:
i = 0 # change this to get a different data row and corresponding frame
r = df.iloc[i,:]
print(r)
img = videodata[int(r.frame),:,:,:]
plt.imshow(img)
Out[52]:
In [60]:
NX, NY = 9, 6
fe = fisheye.load_model('./calib.dat')
In [70]:
undist_img = fe.undistort(img, undistorted_size=(640, 480))
plt.imsave('C:/Users/corey/Desktop/undist_img.jpg', undist_img)
plt.figure(figsize=(20,20))
plt.subplot(1,2,1)
plt.imshow(img)
plt.subplot(1,2,2)
plt.imshow(undist_img)
Out[70]:
In [86]:
src_pts = np.array([[719,836],
[904,471],
[1000,471],
[1254,836]]) / 3.0
dst_pts = np.array([[850,1900],
[850,100],
[1110,100],
[1110,1900]]) / 3.0
print(src_pts)
print(dst_pts)
M2, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
M2
Out[86]:
Sample warped image. Turns out to be pretty good representation of ground plane. Checked it out with measuring tape and various manually identified points.
In [270]:
warped = cv2.warpPerspective(undist_img, M2, (700,700))
plt.figure(figsize=(12,12))
plt.subplot(1,2,1)
plt.imshow(img)
plt.subplot(1,2,2)
plt.imshow(warped)
Out[270]:
Sample showing original + undistored + warped side by side.
In [334]:
i = 0
img = videodata[int(df.iloc[i,:].frame),:,:,:]
undist_img = fe.undistort(img, undistorted_size=(640, 480))
warped = cv2.warpPerspective(undist_img, M2, (700,700))
plt.figure(figsize=(12,12))
plt.subplot(1,3,1)
plt.imshow(img)
plt.subplot(1,3,2)
plt.imshow(undist_img)
plt.subplot(1,3,3)
plt.imshow(warped)
plt.imsave('C:/Users/corey/Desktop/warped.png', warped)
In [272]:
rows,cols,_ = warped.shape
print(df.iloc[i,:])
M = cv2.getRotationMatrix2D((350,700),-(df.iloc[i,:].orientation_x),1)
dst = cv2.warpAffine(warped,M,(700,1400))
plt.figure(figsize=(10,10))
plt.imshow(dst[:,:,:])
Out[272]:
In [206]:
np.dot(M, [350.0,700.0,1.0])
Out[206]:
In [119]:
df.orientation_x.plot()
Out[119]:
In [126]:
plt.plot(df.timestamp, df.encoder1_count.diff()/df.timestamp.diff())
Out[126]:
Timespikes likely due to write hangs from SD card. Likely causing significant drift - need to fix.
In [125]:
plt.plot(df.timestamp.diff())
Out[125]:
In [ ]:
src_pts = np.array([[719,836],
[904,471],
[1000,471],
[1254,836]])
dst_pts = np.array([[850,1900],
[850,100],
[1110,100],
[1110,1900]])
print(src_pts)
print(dst_pts)
M2, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
M2
In [362]:
points = []
curr_heading = 0
x, y = 0.0, 0.0
pix_per_tick = 4.0
meters_per_revolution = 0.315
ticks_per_revolution = 20
meters_per_tick = meters_per_revolution / ticks_per_revolution
pixels_per_meter = 104.0 / 0.370
for i,r in df.iterrows():
if i == 0:
points.append([x,y])
else:
approximate_ticks = (r.encoder0_diff + r.encoder1_diff) / 2
dist_meters = approximate_ticks * meters_per_tick
x += np.sin(curr_heading) * dist_meters
y += np.cos(curr_heading) * dist_meters
points.append([x,y])
curr_heading = np.deg2rad(r.orientation_x)
points = np.array(points)
df['x'] = points[:,0]
df['y'] = points[:,1]
df['px'] = df['x'] * pixels_per_meter
df['py'] = df['y'] * pixels_per_meter
df.px = df.px - df.px.min() + 200
df.py = df.py - df.py.min() + 200
df.py = df.py.max() - df.py + 200
map_width = df.px.max()
map_height = df.py.max()
print(map_width, map_height)
map_img = np.zeros((1500, 1200, 3))
plt.figure(figsize=(12,12))
plt.axis('equal')
plt.imshow(map_img)
plt.plot(df.px, df.py)
Out[362]:
In [453]:
import importlib
importlib.reload(encoder_math)
points = []
curr_heading = 0
x, y = 0.0, 0.0
pix_per_tick = 4.0
meters_per_revolution = 0.315
ticks_per_revolution = 20
meters_per_tick = meters_per_revolution / ticks_per_revolution
pixels_per_meter = 104.0 / 0.370
vehicle_width = 0.265
half_vehicle_width = vehicle_width / 2.0
center_position = np.array([x, y])
half_width_offset_vec = np.array([vehicle_width / 2.0, 0.0])
left_position = center_position - half_width_offset_vec
right_position = center_position + half_width_offset_vec
left_points = []
right_points = []
for i,r in df.iterrows():
if i == 0:
points.append(center_position)
left_points.append(left_position)
right_points.append(right_position)
else:
right_distance = r.encoder0_diff * meters_per_tick
left_distance = r.encoder1_diff * meters_per_tick
left_position, center_position, right_position = [np.array(x) for x in encoder_math.apply_distance(half_vehicle_width, left_position, center_position, right_position, left_distance, right_distance)]
points.append(np.array(center_position))
left_points.append(np.array(left_position))
right_points.append(np.array(right_position))
curr_heading = np.deg2rad(r.orientation_x)
points = np.array(points)
left_points = np.array(left_points)
right_points = np.array(right_points)
df['wx'] = points[:,0]
df['wy'] = points[:,1]
df['lx'] = left_points[:,0]
df['ly'] = left_points[:,1]
df['rx'] = right_points[:,0]
df['ry'] = right_points[:,1]
df['pwx'] = df['wx'] * pixels_per_meter
df['pwy'] = df['wy'] * pixels_per_meter
df.pwx = df.pwx - df.pwx.min() + 200
df.pwy = df.pwy - df.pwy.min() + 200
df.pwy = df.pwy.max() - df.pwy + 200
map_width = df.pwx.max()
map_height = df.pwy.max()
print(map_width, map_height)
map_img = np.zeros((1500, 1200, 3))
plt.figure(figsize=(12,12))
plt.axis('equal')
plt.plot(df.wx, df.wy, df.lx, df.ly, df.rx, df.ry)
Out[453]:
In [428]:
points
Out[428]:
In [434]:
df[['orientation_x','encoder0_diff','encoder1_diff','x','y','lx','ly','wx','wy','rx','ry']]
Out[434]:
In [283]:
plt.figure(figsize=(12,12))
plt.axis('equal')
plt.plot(df.x, df.y)
Out[283]:
We can use the positions to collect undistorted / warped images onto a map image. For now, don't worry about size and just paste a smaller portion.
Future:
In [291]:
Rect = namedtuple('Rect', 'x1 y1 x2 y2')
In [297]:
warped_size = np.array([700, 700])
persp_size = warped_size * [1, 2]
persp_center = persp_size // 2
print(warped_size)
print(persp_size)
print(persp_center)
In [307]:
np.clip(-10, 1, 20)
Out[307]:
In [442]:
map_size = 1200, 1500
map_img = np.zeros((map_size[1], map_size[0], 3), dtype=np.uint8)
h2 = 50
w2 = 50
n = int(df.shape[0]/2)
warped_size = 700, 700
persp_size = warped_size[0], warped_size[1] * 2
persp_half = persp_size[0] // 2, persp_size[1] // 2
persp_rot_center = (321, 699)
for i in range(0, n, 100):
rows,cols,_ = warped.shape
#print(df.iloc[i,:])
r = df.iloc[i,:]
img = videodata[int(r.frame),:,:,:]
undist_img = fe.undistort(img, undistorted_size=(640, 480))
warped = cv2.warpPerspective(undist_img, M2, warped_size)
M = cv2.getRotationMatrix2D(persp_rot_center,-int(df.iloc[i,:].orientation_x),1)
persp = cv2.warpAffine(warped,M,persp_size)
x, y = int(r.px)+25, int(r.py)
mx1 = np.clip(x - persp_half[0], 0, map_size[0]-1)
mx2 = np.clip(x + persp_half[0], 0, map_size[0]-1)
my1 = np.clip(y - persp_half[1], 0, map_size[1]-1)
my2 = np.clip(y + persp_half[1], 0, map_size[1]-1)
px1 = persp_half[0] - (x - mx1)
px2 = persp_half[0] + (mx2 - x)
py1 = persp_half[1] - (y - my1)
py2 = persp_half[1] + (my2 - y)
try:
#map_img[my1:my2,mx1:mx2,:] = np.maximum(map_img[my1:my2,mx1:mx2,:], persp[py1:py2,px1:px2,:])
map_img[my1:my2,mx1:mx2,:] += persp[py1:py2,px1:px2,:] // 1
except Exception as e:
print(e)
plt.figure(figsize=(10,10))
plt.imshow(map_img)
plt.plot(df.px[0:n], df.py[0:n], df.pwx[0:n], df.pwy[0:n])
Out[442]:
In [266]:
i = 200
img = videodata[int(df.iloc[i,:].frame),:,:,:]
undist_img = fe.undistort(img, undistorted_size=(640, 480))
warped = cv2.warpPerspective(undist_img, M2, (700,700))
plt.figure(figsize=(12,12))
plt.subplot(2,2,1)
plt.imshow(img)
plt.subplot(2,2,2)
plt.imshow(undist_img)
plt.subplot(2,2,3)
plt.imshow(warped)
plt.subplot(2,2,4)
plt.imshow(map_img)
plt.plot(df.x[0:n], df.y[0:n])
Out[266]:
In [ ]: