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())