def run(self): state = np.matrix('0.0;0.0;0.0;0.0') # x, y, xd, yd, # P and Q matrices for EKF P = np.matrix('10.0,0.0,0.0,0.0; \ 0.0,10.0,0.0,0.0; \ 0.0,0.0,10.0,0.0; \ 0.0,0.0,0.0,10.0') Q = np.matrix('2.0,0.0,0.0,0.0; \ 0.0,2.0,0.0,0.0; \ 0.0,0.0,2.0,0.0; \ 0.0,0.0,0.0,2.0') measurement = np.matrix('0;0') np.set_printoptions( formatter={'float': lambda x: "{0:0.2f}".format(x)}) # print basic info print('python ' + platform.python_version()) print('opencv ' + cv2.__version__) print('numpy ' + np.version.version) video = cv2.VideoCapture(0) video.set(6, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G')) video.set(3, 640) video.set(4, 480) # def main(): # open camera start_time = time.time() prev_time = time.time() i = 0 counter = 0 stop_time = time.time() + 10 while (True): if time.time() > stop_time: break now_time = time.time() dt = now_time - prev_time i += 1 counter += 1 # run the model every 0.01 s if (dt > 0.005): prev_time = now_time state, P, J = run_EKF_model(state, P, Q, dt) # read camera # ret, frame = cap.read() # select.select((video,), (), ()) # image_data = video.read_and_queue() # raw_image = np.fromstring(image_data, dtype='uint8') # frame = cv2.imdecode(raw_image, cv2.IMREAD_UNCHANGED) # frame = cv2.imdecode(np.frombuffer(image_data, dtype=np.uint8), cv2.IMREAD_COLOR) ret, frame = video.read() print("frame: {}".format(i)) ret == False if ret == True: # For initilization, process the whole image, otherwise, utilize the predicted position if i < 10: mask, cimg, (x, y, r) = recognize_center_without_EKF(frame) else: mask, cimg, (x, y, r) = recognize_center(frame, state[0], state[1]) # if i == 5: # break # if x==0: # continue measurement[0] = x measurement[1] = y if (measurement[0] != 0) and (measurement[1] != 0): print("run EKF") state, P = run_EKF_measurement(state, measurement, P) else: print("no motion detected, continue") # i = 0 # continue print("x: {}, state 0: {}".format(x, state[0])) if (x != 0): cv2.circle(cimg, (int(x), int(y)), 50, (255), 5) if (state[0] != 0): cv2.circle(cimg, (int(state[0]), int(state[1])), 20, (255), 3) msg = camera_pose_xyt_t() msg.x = state[0] msg.y = state[1] self.lc.publish("CAMERA_POSE_CHANNEL", msg.encode()) # pixel_coord = np.array([state[0],state[1],1]) # world_2d_coord = transform_camera_to_2d(pixel_coord) # print(world_2d_coord) # cv2.imshow('all',cimg) # close if cv2.waitKey(0) & 0xFF == ord('q'): break print("Time {}, frames: {}".format(time.time() - start_time, counter)) # clean up video.release() cv2.destroyAllWindows()
def main(): # launch lcm lc = lcm.LCM() # Global Variables state = np.matrix('0.0;0.0;0.0;0.0') # x, y, xd, yd, # P and Q matrices for EKF P = np.matrix('10.0,0.0,0.0,0.0; \ 0.0,10.0,0.0,0.0; \ 0.0,0.0,10.0,0.0; \ 0.0,0.0,0.0,10.0') Q = np.matrix('2.0,0.0,0.0,0.0; \ 0.0,2.0,0.0,0.0; \ 0.0,0.0,2.0,0.0; \ 0.0,0.0,0.0,2.0') measurement = np.matrix('0;0') np.set_printoptions(formatter={'float': lambda x: "{0:0.2f}".format(x)}) # print basic info print('python ' + platform.python_version()) print('opencv ' + cv2.__version__) print('numpy ' + np.version.version) # def main(): # open camera print("sampling THREADED frames from webcam...") vs = VideoGet(src=0).start() stop_time = time.time() + 5 start_time = time.time() prev_time = time.time() i = 0 while (True): if time.time() > stop_time: print("stop") # vs.stop() break now_time = time.time() dt = now_time - prev_time i += 1 # run the model every 0.01 s if (dt > 0.001): prev_time = now_time state, P, J = run_EKF_model(state, P, Q, dt) # read camera # ret, frame = cap.read() frame = vs.read() ret = True print("frame: {}".format(i)) if ret == True: # For initilization, process the whole image, otherwise, utilize the predicted position if i < 5: mask, cimg, (x, y, r) = recognize_center_without_EKF(frame) else: mask, cimg, (x, y, r) = recognize_center(frame, state[0], state[1]) # if i == 5: # break # if x==0: # continue measurement[0] = x measurement[1] = y if (measurement[0] != 0) and (measurement[1] != 0): print("run EKF") state, P = run_EKF_measurement(state, measurement, P) else: print("no motion detected, continue") # continue print("x: {}, state 0: {}".format(x, state[0])) # publish lcm msg = camera_pose_xyt_t() msg.x = state[0] msg.y = state[1] lc.publish("CAMERA_POSE_CHANNEL", msg.encode()) # if(x != 0): # cv2.circle(cimg, (int(x), int(y)), 50, (255), 5) # # if(state[0] != 0): # cv2.circle(cimg, (int(state[0]),int(state[1])), 20, (255), 3) # pixel_coord = np.array([state[0],state[1],1]) # world_2d_coord = transform_camera_to_2d(pixel_coord) # print(world_2d_coord) # cv2.imshow('all',cimg) # close # if cv2.waitKey(0) & 0xFF == ord('q'): # break print("Time {}, frames: {}".format(time.time() - start_time, i)) # clean up # video.close() cv2.destroyAllWindows()
def run(self): # x, y, xd, yd, state = np.matrix('0.0;0.0;0.0;0.0') # P and Q matrices for EKF P = np.matrix('10.0,0.0,0.0,0.0; \ 0.0,10.0,0.0,0.0; \ 0.0,0.0,10.0,0.0; \ 0.0,0.0,0.0,10.0') Q = np.matrix('2.0,0.0,0.0,0.0; \ 0.0,2.0,0.0,0.0; \ 0.0,0.0,2.0,0.0; \ 0.0,0.0,0.0,2.0') measurement = np.matrix('0;0') np.set_printoptions( formatter={'float': lambda x: "{0:0.2f}".format(x)}) # print basic info print('python ' + platform.python_version()) print('opencv ' + cv2.__version__) print('numpy ' + np.version.version) # state variables i = 0 counter = 0 start_time = time.time() prev_time = time.time() stop_time = time.time() + 5 # self.fps_tracker.start() # main loop while not self.stopped: # self.fps_tracker.increment() print("tracker udpate") if counter > 1000: print("Time {}, frames: {}".format(time.time() - start_time, counter)) self.stop() # if time.time() > stop_time: # print("Time {}, frames: {}".format(time.time()-start_time, counter)) # self.stop() # break now_time = time.time() dt = now_time - prev_time i += 1 counter += 1 # run EKF model every 0.005 s if dt > 0.005: prev_time = now_time state, P, J = run_EKF_model(state, P, Q, dt) print("frame: {}".format(i)) if i < 10: mask, cimg, (x, y, r) = recognize_center_without_EKF(self.frame) else: mask, cimg, (x, y, r) = recognize_center(self.frame, state[0], state[1]) # if i == 5: # break # if x==0: # continue measurement[0] = x measurement[1] = y if (measurement[0] != 0) and (measurement[1] != 0): print("run EKF") state, P = run_EKF_measurement(state, measurement, P) else: print("no motion detected, continue") # i = 0 # continue print("x: {}, state 0: {}".format(x, state[0])) if (x != 0): cv2.circle(cimg, (int(x), int(y)), 50, (255), 5) if (state[0] != 0): cv2.circle(cimg, (int(state[0]), int(state[1])), 20, (255), 3) msg = camera_pose_xyt_t() msg.x = state[0] msg.y = state[1] self.lc.publish("CAMERA_POSE_CHANNEL", msg.encode())