def main():
params = check_params(parse_args(sys.argv[1:]))
kf = KalmanFilter()
if params['file']:
template = read_template(params)
surf, flann = init_surf()
kp, des = surf.detectAndCompute(template, None)
#Start up Video processing thread
vs = PiVideoStream(resolution=(params['xRes'], params['yRes'])).start()
#Wait for camera to warm up
time.sleep(3.0)
#Used for calculating FPS
startTime = time.time()
if params['capTime'] == 0:
endTime = startTime + 1000000000
else:
endTime = startTime + params['capTime']
frames = 0
# Open up serial port with Pixhawk
if params['sendToPX4']:
port = serial.Serial('/dev/ttyAMA0', 57600)
mav = mavlink.MAVLink(port)
# Typically only need to search within a small Y-range
yRes = params['yRes']
(cropYmin, cropYmax) = (yRes * .25, yRes * .70)
#Take weighted average of last # of distances to filter out noise
notFoundCount = 1000
while time.time() < endTime:
frames += 1
frame = vs.read()
frame = frame[cropYmin : cropYmax, 0:params['xRes']]
found, [x,y], frame = FindingFuncs.findPatternSURF(frame, surf, kp, des, template, flann, params['preview'])
# Count how many frames it has been since the RPi has not found anything
if not found:
notFoundCount += 1
else:
notFoundCount = 0
kf.update(x)
# How many frames until you assume to keep going straight.
if notFoundCount > 100:
kf.reset()
x = params['xRes'] / 2
else:
x = kf.predict()
loc = boundFloat(2.0 * x / params['xRes'] - 1, -1., 1.)
if params['sendToPX4']:
message = mavlink.MAVLink_duck_leader_loc_message(loc, 5.0)
mav.send(message)
if params['debug'] :
print str(time.time() - startTime) + ", " + str(loc)
if params['preview']:
# Draw a circle on frame where the Kalman filter predicts.
cv2.circle(frame, (int(x), 10), 4, (0, 0, 255), 6)
frame = imutils.resize(frame, width=1000)
cv2.imshow("Preview", frame)
#Check for keypress, ending if Q is entered
key = cv2.waitKey(1) & 0xFF
if (key == ord("q")):
break;
totalTime = time.time() - startTime
cv2.destroyAllWindows()
vs.stop()
print "Average main FPS: " + str(float(frames) / totalTime) + "fps"
