#cv.circle(frame,(int(xe[n]),int(ye[n])),int(uncertainty), (255, 255, 0),1)
cv.circle(frame, (int(xe[n]), int(ye[n])), 3, (0, 255, 255), -1)
# Draw the predicted path
for n in range(len(xp)):
uncertaintyP = (xpu[n] + ypu[n]) / 2
# Draw prediction (circles), with uncertainty as radius
cv.circle(frame, (int(xp[n]), int(yp[n])), int(uncertaintyP),
(0, 0, 255))
cv.circle(frame, (int(xp[n]), int(yp[n])), 3, (255, 255, 255), -1)
########## DISPLAY ##########
# check to see if the frame should be displayed to our screen
cv.imshow('Frame', frame)
cv.imshow('Vision', mask)
out.write(frame)
# update the FPS counter
streamfps.update()
lastframe = frame_read
################### CLEARING UP ###################
# stop the timer and display information
streamfps.stop()
print("[INFO] elasped time: {:.2f}".format(streamfps.elapsed()))
print("[INFO] approx. FPS: {:.2f}".format(streamfps.fps_tot()))
print("Measurements: " + str(listPoints))
out.release()
stream.release()
cv.destroyAllWindows()
cv.circle(graph3d,
(int(xframe[n]) - 5, -int(3 * zp[n] - frame_h / 2 - 90)),
int(uncertaintyP), (0, 0, 100))
cv.circle(graph3d,
(int(xframe[n]) - 5, -int(3 * zp[n] - frame_h / 2 - 90)),
3, (0, 0, 255), -1)
cv.putText(graph3d, format("X"), (10, 20), font, 0.5, (0, 255, 0), 2)
cv.putText(graph3d, format("Y"), (30, 20), font, 0.5, (255, 0, 0), 2)
cv.putText(graph3d, format("Z"), (50, 20), font, 0.5, (0, 0, 255), 2)
cv.imshow("Frame 1", frame1)
#cv.imshow("Vision", mask1)
cv.imshow("Frame 2", frame2)
#cv.imshow("Vision", mask2)
cv.imshow("Position", graph3d)
if not (grabbed1 is False or grabbed2 is False):
# update the FPS counter
streamfps.update()
#fps = streamfps.fps_now()
# stop the timer and display FPS information
streamfps.stop()
print("[INFO] elasped time: {:.2f}".format(streamfps.elapsed()))
print("[INFO] approx. FPS: {:.2f}".format(fps))
# do a bit of cleanup
stream1.release()
stream2.release()
cv.destroyAllWindows()
