memory_optical = 0.5 * np.ones(central_complex.N_CPU4)
cx_gps = cx_rate.CXRate(noise=0)
tb1_gps = np.zeros(central_complex.N_TB1)
memory_gps = 0.5 * np.ones(central_complex.N_CPU4)
cpu4_gps = np.zeros(16)
# initialize camera and optical flow
frame_num = 0
picam = picameraThread(1, "picamera_video", resolution, 30)
picam.start()
fw, fh = resolution
time.sleep(0.1) # allow the camera to warmup
print("Frame size: {}*{}".format(fw, fh))
optflow = Optical_flow(resolution)
temp = picam.get_frame()
prvs = optflow.undistort(temp)
(fh, fw) = prvs.shape
print("Undistorted frame size: {0}*{1}".format(fw, fh))
left_filter, right_filter = optflow.get_filter(fh, fw)
# Define the codec and create VideoWriter object
if RECORDING == 'true':
fname = 'video/' + time_string + '.avi'
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter(fname, fourcc, 20.0, (fw, fh), False)
# take off adn set to mission mode.
state = arm_and_takeoff(drone, HEIGHT)
drone.mode = VehicleMode("AUTO")
fw, fh = resolution
scale = 2
'''
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,fw)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,fh)
cap.set(cv2.CAP_PROP_FPS, 30)
fw = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
fh = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
print("Frame size: {}*{}".format(fw, fh))
'''
picam = picameraThread(1, "picamera_video", resolution, 30)
picam.start()
optflow = Optical_flow(resolution);
picture_num = 100
column_num = 0
time.sleep(2)
while True:
gray = picam.get_frame()
# undistorte image
# gray = optflow.undistort(gray)
# draw lines on image for calibration angles
# gray = cv2.line(gray,(1, column_num),(fw, column_num),(255,255,0),1)
# gray = cv2.line(gray,(1, int(fh/2)),(fw, int(fh/2)),(255,255,0),1)
# gray = cv2.line(gray,(column_num, 1),(column_num, fh),(255,255,0),1)
# gray = cv2.line(gray,(int(fw/2), 1),(int(fw/2), fh),(255,255,0),1)
img = np.ones([fh,fw,3])
vector_map = draw_flow(img, matched_filter)
cv2.imshow('filter1', vector_map)
D = np.zeros([fh,fw,3])
for i in range(fh):
for j in range(fw):
D[i,j]=np.array([np.tan(horizontal_views[j]), np.tan(vertical_views[i]), -1])
D[i,j] /= LA.norm(D[i,j])
#a = np.array([1/np.sqrt(2), 1/np.sqrt(2), 0])
a = np.array([0, 0, -1])
#matched_filter = np.cross(np.cross(D,a),D)[:,:,0:2]
matched_filter = np.cross(a, D, axisa=0, axisb=2, axisc=2)[:,:,0:2]
#matched_filter = np.ones([fh,fw,2]) * np.array([0,1])
# show vector map
img = np.ones([fh,fw,3])
vector_map = draw_flow(img, matched_filter)
cv2.imshow('filter2', vector_map)
'''
optflow = Optical_flow((324, 248))
left_filter, right_filter, rot_filter = optflow.get_filter(110, 200)
img = np.ones([110, 200, 3])
vector_map = draw_flow(img, left_filter)
cv2.imshow('filter_left', vector_map)
img = np.ones([110, 200, 3])
vector_map = draw_flow(img, rot_filter)
cv2.imshow('rotation_right', vector_map)
cv2.waitKey()
cv2.destroyAllWindows()
# initialize CX model
cx = cx_rate.CXRate(noise = 0)
tb1 = np.zeros(central_complex.N_TB1)
memory = 0.5 * np.ones(central_complex.N_CPU4)
# initialize camera
frame_num = 0
cap = cv2.VideoCapture(sys.argv[1])
for i in range(1):
ret, frame1 = cap.read() # Skip frames
frame_num += 1
temp = cv2.cvtColor(frame1,cv2.COLOR_BGR2GRAY)
dim = temp.shape[::-1]
# intialise optical flow object
optflow = Optical_flow((312,286));
prvs = temp #optflow.undistort(temp)
(fh, fw) = prvs.shape
print("Frame size: {0}*{1}".format(fw,fh))
# log information
str = sys.argv[1]
try:
error_log_path = 'log/'+str.split('.')[0].split('/')[1]+'.log'
with open(error_log_path) as f:
data = f.read()
data = data.split('\n')
while data[0].split(':')[2] != 'sl':
print data[0]
del data[0]
navigation_info = []
resolution = FRAME_DIM['medium']
fw, fh = resolution
scale = 2
'''
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,fw)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,fh)
cap.set(cv2.CAP_PROP_FPS, 30)
fw = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
fh = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
print("Frame size: {}*{}".format(fw, fh))
'''
picam = picameraThread(1, "picamera_video", resolution, 30)
picam.start()
optflow = Optical_flow(resolution)
picture_num = 100
column_num = 0
time.sleep(2)
while True:
gray = picam.get_frame()
# undistorte image
gray = optflow.undistort(gray)
# draw lines on image for calibration angles
gray = cv2.line(gray, (1, column_num), (fw, column_num), (255, 255, 0), 1)
gray = cv2.line(gray, (1, int(fh / 2)), (fw, int(fh / 2)), (255, 255, 0),
1)
gray = cv2.line(gray, (column_num, 1), (column_num, fh), (255, 255, 0), 1)
gray = cv2.line(gray, (int(fw / 2), 1), (int(fw / 2), fh), (255, 255, 0),