def runVision():
# construct the argument parse and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video", help="path to the (optional) video file")
ap.add_argument("-b",
"--buffer",
type=int,
default=10,
help="max buffer size")
ap.add_argument("-a",
"--min-area",
type=int,
default=500,
help="minimum area size")
args = vars(ap.parse_args())
# define the lower and upper boundaries of the "blue"
# ball in the HSV color space, then initialize the
# list of tracked points
blueLower = (48, 62, 88)
blueUpper = (151, 238, 255)
armPixLower = 140
armPixUpper = 300
armMLower = 0.930
armMUpper = 0.140
armPixZero = 340 #position of Z in pixels when Cartesian Z = 0
przelicznik = (armMLower - armMUpper) / (armPixUpper - armPixLower)
ballInArmRange = False
pts = deque(maxlen=args["buffer"])
tintervals = deque(maxlen=args["buffer"])
tPrev = 0
pRad = 0
mapix = 0
yinters = 0
passPosition = 0
pub = rospy.Publisher('positionY', Float64, queue_size=10)
joint = PoseStamped()
rospy.init_node('talker2', anonymous=True)
count = 0
#initalize Kalman parameters
tkalman = 0.01
kalman = cv2.KalmanFilter(6, 6)
kalman.measurementMatrix = np.array(
[[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]],
np.float32)
kalman.transitionMatrix = np.array(
[[1, 0, tkalman, 0, (0.5 * (tkalman**2.0)), 0],
[0, 1, 0, tkalman, 0,
(0.5 * (tkalman**2.0))], [0, 0, 1, 0, tkalman, 0],
[0, 0, 0, 1, 0, tkalman], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]],
np.float32)
kalman.processNoiseCov = np.array(
[[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]],
np.float32) * 0.01
kalman.measurementNoiseCov = np.array(
[[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]],
np.float32) * 0.00001
# if a video path was not supplied, grab the reference
# to the webcam
if not args.get("video", False):
camera = cv2.VideoCapture(0)
# otherwise, grab a reference to the video file
else:
camera = cv2.VideoCapture(args["video"])
# keep looping
#initialize background subtraction
fgbg = cv2.createBackgroundSubtractorMOG2()
while True:
# grab the current frame
(grabbed, frame) = camera.read()
# start counting time
tPrev = time.time()
# if we are viewing a video and we did not grab a frame,
# then we have reached the end of the video
if args.get("video") and not grabbed:
break
# resize the frame and apply background subtraction
frame = imutils.resize(frame, width=720)
mask = fgbg.apply(frame)
res = cv2.bitwise_and(frame, frame, mask=mask)
# blur the frame and convert it to the HSV
blurred = cv2.GaussianBlur(res, (11, 11), 0)
hsv = cv2.cvtColor(res, cv2.COLOR_BGR2HSV)
# construct a mask for the color "blue", then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv2.inRange(hsv, blueLower, blueUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# find contours in the mask and initialize the current
# (x, y) center of the ball
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
pRad = radius
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if the radius meets a minimum size
if radius > 10:
# draw the circle and centroid on the frame,
# then update the list of tracked points
cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255),
2)
cv2.circle(frame, center, 5, (0, 0, 255), -1)
# update time intervals queue
tintervals.appendleft(time.time() - tPrev)
# update the points queue
pts.appendleft(center)
# predict position of the ball
if (pRad > 0 and len(pts) > 5):
if pts[0] != None and pts[1] != None:
y0 = pts[0][1]
x0 = pts[0][0]
dy = pts[0][1] - pts[1][1]
dx = pts[0][0] - pts[1][0]
dt = tintervals[0]
vx = dx / dt
vy = dy / dt
ax = 0
ay = 98.1 / przelicznik
kalmanin = np.array((6, 1), np.float32) # measurement
kalmanout = np.zeros((6, 1),
np.float32) # tracked / prediction
kalmanin = np.array([[np.float32(x0)], [np.float32(y0)],
[np.float32(vx)], [np.float32(vy)],
[np.float32(ax)], [np.float32(ay)]])
kalman.correct(kalmanin)
kalmanout = kalman.predict()
x0 = kalmanout[0]
y0 = kalmanout[1]
vx = kalmanout[2]
vy = kalmanout[3]
ax = kalmanout[4]
ay = kalmanout[5]
listX = []
listY = []
joint.header.seq = count
joint.header.stamp = rospy.Time.now()
joint.header.frame_id = ""
joint.pose.position.x = -0.126
joint.pose.position.z = 0.136
joint.pose.orientation.x = 0.37997234165
joint.pose.orientation.y = -0.596354351601
joint.pose.orientation.z = -0.379993107133
joint.pose.orientation.w = 0.596311748028
if x0 < 360:
joint.pose.position.y = 0.515
positionY = 0.515
count += 1
if x0 > 360:
joint.pose.position.y = 0.781
positionY = 0.781
count += 1
rospy.loginfo(positionY)
pub.publish(positionY)
# loop over the set of tracked points
for i in xrange(1, len(pts)):
# if either of the tracked points are None, ignore
# them
if pts[i - 1] is None or pts[i] is None:
continue
# otherwise, compute the thickness of the line and
# draw the connecting lines
thickness = int(np.sqrt(args["buffer"] / float(i + 1)) * 2.5)
cv2.line(frame, pts[i - 1], pts[i], (0, 0, 255), thickness)
cv2.putText(frame, "passPosition: {}".format(passPosition),
(120, frame.shape[0] - 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 0, 255), 1)
cv2.putText(frame, "yinters: {}".format(yinters),
(120, frame.shape[0] - 70), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 0, 255), 1)
cv2.putText(frame, "przelicznik: {}".format(przelicznik),
(120, frame.shape[0] - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 0, 255), 1)
cv2.putText(frame, "x, y: {}".format(pts[0]),
(10, frame.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.35,
(0, 0, 255), 1)
# show the frame to our screen
cv2.imshow("Frame", frame)
key = cv2.waitKey(1) & 0xFF
# if the 'q' key is pressed, stop the loop
if key == ord("q"):
break
# cleanup the camera and close any open windows
camera.release()
cv2.destroyAllWindows()
#####################################################################
# define video capture object
cap = cv2.VideoCapture()
# define display window name
windowName = "Kalman Object Tracking"
# window name
windowName2 = "Hue histogram back projection"
# window name
windowNameSelection = "initial selected region"
# init kalman filter object
kalman = cv2.KalmanFilter(4, 2)
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kalman.transitionMatrix = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
kalman.processNoiseCov = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]], np.float32) * 0.03
measurement = np.array((2, 1), np.float32)
prediction = np.zeros((2, 1), np.float32)
# if command line arguments are provided try to read video_name
# otherwise default to capture from attached H/W camera
if (((len(sys.argv) == 2) and (cap.open(str(sys.argv[1]))))
def main(argv):
tf_device = '/cpu:0'
with tf.device(tf_device):
"""Build graph
"""
if FLAGS.color_channel == 'RGB':
input_data = tf.placeholder(
dtype=tf.float32,
shape=[None, FLAGS.input_size, FLAGS.input_size, 3],
name='input_image')
else:
input_data = tf.placeholder(
dtype=tf.float32,
shape=[None, FLAGS.input_size, FLAGS.input_size, 1],
name='input_image')
center_map = tf.placeholder(
dtype=tf.float32,
shape=[None, FLAGS.input_size, FLAGS.input_size, 1],
name='center_map')
model = cpm_hand_slim.CPM_Model(FLAGS.stages, FLAGS.joints + 1)
model.build_model(input_data, center_map, 1)
saver = tf.train.Saver()
"""Create session and restore weights
"""
sess = tf.Session()
sess.run(tf.global_variables_initializer())
if FLAGS.model_path.endswith('pkl'):
model.load_weights_from_file(FLAGS.model_path, sess, False)
else:
saver.restore(sess, FLAGS.model_path)
test_center_map = cpm_utils.gaussian_img(FLAGS.input_size,
FLAGS.input_size,
FLAGS.input_size / 2,
FLAGS.input_size / 2,
FLAGS.cmap_radius)
test_center_map = np.reshape(test_center_map,
[1, FLAGS.input_size, FLAGS.input_size, 1])
# Check weights
for variable in tf.trainable_variables():
with tf.variable_scope('', reuse=True):
var = tf.get_variable(variable.name.split(':0')[0])
print(variable.name, np.mean(sess.run(var)))
"""if not FLAGS.DEMO_TYPE.endswith(('png', 'jpg')):
cam = cv2.VideoCapture(FLAGS.cam_num)"""
# Create kalman filters
if FLAGS.KALMAN_ON:
kalman_filter_array = [
cv2.KalmanFilter(4, 2) for _ in range(FLAGS.joints)
]
for _, joint_kalman_filter in enumerate(kalman_filter_array):
joint_kalman_filter.transitionMatrix = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32)
joint_kalman_filter.measurementMatrix = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
joint_kalman_filter.processNoiseCov = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32) * FLAGS.kalman_noise
else:
kalman_filter_array = None
#classifier MLP model
model2 = Sequential()
model2.add(InputLayer(input_shape=(21, 2)))
model2.add(Flatten())
model2.add(Dense(21))
model2.add(Dense(500))
model2.add(Dense(1400))
model2.add(Dense(3000))
model2.add(Dense(3000))
model2.add(Dense(1400))
model2.add(Dense(500, activation='relu'))
model2.add(Dense(26, activation='softmax'))
model2.load_weights('models/weights/massey_joint.h5')
notepad_image = 255 * np.ones(shape=[368, 500, 3], dtype=np.uint8)
massey_static = cv2.imread('massey_static.png')
massey_static = cv2.resize(massey_static, (368, 368))
with tf.device(tf_device):
while True:
cam = cv2.VideoCapture(FLAGS.cam_num)
test_img = cpm_utils.read_image([], cam, FLAGS.input_size,
'WEBCAM')
cam.release()
test_img_resize = cv2.resize(test_img,
(FLAGS.input_size, FLAGS.input_size))
t1 = time.time()
#print('img read time %f' % (time.time() - t1))
"""if FLAGS.color_channel == 'GRAY':
test_img_resize = np.dot(test_img_resize[..., :3], [0.299, 0.587, 0.114]).reshape(
(FLAGS.input_size, FLAGS.input_size, 1))
cv2.imshow('color', test_img.astype(np.uint8))
cv2.imshow('gray', test_img_resize.astype(np.uint8))
cv2.waitKey(1)"""
test_img_input = test_img_resize / 256.0 - 0.5
test_img_input = np.expand_dims(test_img_input, axis=0)
"""if FLAGS.DEMO_TYPE.endswith(('png', 'jpg')):
# Inference
t1 = time.time()
predict_heatmap, stage_heatmap_np = sess.run([model.current_heatmap,
model.stage_heatmap,
],
feed_dict={'input_image:0': test_img_input,
'center_map:0': test_center_map})
# Show visualized image
demo_img = visualize_result(test_img, FLAGS, stage_heatmap_np, kalman_filter_array)
#demo_img = cv2.cvtColor(demo_img,cv2.COLOR_BGR2GRAY)
print(demo_img.shape)
#demo_img1 = cv2.GaussianBlur(demo_img, (9, 9), 0)
#demo_img = 2.5*demo_img-demo_img1
cv2.imshow('demo_img', demo_img.astype(np.uint8))
if cv2.waitKey(0) == ord('q'): break
print('fps: %.2f' % (1 / (time.time() - t1)))
elif FLAGS.DEMO_TYPE == 'MULTI':"""
# Inference
t1 = time.time()
#print("chandu")
key1 = cv2.waitKey(1)
#if key1 == 32:
predict_heatmap, stage_heatmap_np = sess.run([
model.current_heatmap,
model.stage_heatmap,
],
feed_dict={
'input_image:0':
test_img_input,
'center_map:0':
test_center_map
})
# Show visualized image
demo_img, joint_points = visualize_result(test_img, FLAGS,
stage_heatmap_np,
kalman_filter_array)
x = joint_points[0][0]
y = joint_points[0][1]
for i in range(21):
joint_points[i][0] = joint_points[i][0] - x
joint_points[i][1] = joint_points[i][1] - y
joint_points = np.array([joint_points])
class_num = model2.predict_classes(joint_points)
if class_num[0] == 26:
character = ' '
else:
character = chr(97 + class_num[0])
global printer_x, printer_y, printer_string
printer_string += character
notepad_image = printer.notepad(notepad_image,
(printer_x, printer_y),
printer_string)
new_note = np.concatenate((notepad_image, demo_img, massey_static),
axis=1)
cv2.imshow('notepad', new_note)
if len(printer_string) == 17:
printer_string = ''
printer_y += 25
print('fps: %.2f' % (1 / (time.time() - t1)))
if key1 == ord('q'):
cv2.destroyAllWindows()
break
(if Kalman filter works correctly,
the yellow segment should be shorter than the red one).
Pressing any key (except ESC) will reset the tracking with a different speed.
Pressing ESC will stop the program.
"""
import cv2 as cv
from math import cos, sin, sqrt
import numpy as np
if __name__ == "__main__":
img_height = 500
img_width = 500
# 创建卡尔曼滤波器对象
kalman = cv.KalmanFilter(2, 1, 0)
code = -1
cv.namedWindow("Kalman")
while True:
# state(角度，△角度)
state = 0.1 * np.random.randn(2, 1)
# 转移矩阵A[1,1;0,1]
kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]])
# 测量矩阵
kalman.measurementMatrix = 1. * np.ones((1, 2))
# 系统噪声方差阵
kalman.processNoiseCov = 1e-5 * np.eye(2)
kalman.correct(current_measurement) # 用当前测量来校正卡尔曼滤波器
current_prediction = kalman.predict() # 计算卡尔曼预测值，作为当前预测
lmx, lmy = last_measurement[0], last_measurement[1] # 上一次测量坐标
cmx, cmy = current_measurement[0], current_measurement[1] # 当前测量坐标
lpx, lpy = last_prediction[0], last_prediction[1] # 上一次预测坐标
cpx, cpy = current_prediction[0], current_prediction[1] # 当前预测坐标
# 绘制从上一次测量到当前测量以及从上一次预测到当前预测的两条线
cv2.line(frame, (lmx, lmy), (cmx, cmy), (255, 0, 0)) # 蓝色线为测量值
cv2.line(frame, (lpx, lpy), (cpx, cpy), (255, 0, 255)) # 粉色线为预测值
# 窗口初始化
cv2.namedWindow("kalman_tracker")
# opencv采用setMouseCallback函数处理鼠标事件，具体事件必须由回调（事件）函数的第一个参数来处理，该参数确定触发事件的类型（点击、移动等）
cv2.setMouseCallback("kalman_tracker", mousemove)
kalman = cv2.KalmanFilter(4, 2) # 4：状态数，包括（x，y，dx，dy）坐标及速度（每次移动的距离）；2：观测量，能看到的是坐标值
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32) # 系统测量矩阵
kalman.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) # 状态转移矩阵
kalman.processNoiseCov = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)*0.03 # 系统过程噪声协方差
while True:
cv2.imshow("kalman_tracker", frame)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
break
cv2.destroyAllWindows()
import numpy as np
import cv2
# Kalman initialisation
MIN_H_BLUE = 200
MAX_H_BLUE = 300
stateSize = 6
#[x,y,vx,vy,w,h]
measSize = 4
#[x,y,w,h]
contrSize = 0
#control vector?
kf = cv2.KalmanFilter(stateSize, measSize, contrSize)
state = np.zeros(stateSize, dtype=np.float32)
meas = np.zeros(measSize, dtype=np.float32)
# // Transition State Matrix A
# // Note: set dT at each processing step!
# // [ 1 0 dT 0 0 0 ]
# // [ 0 1 0 dT 0 0 ]
# // [ 0 0 1 0 0 0 ]
# // [ 0 0 0 1 0 0 ]
# // [ 0 0 0 0 1 0 ]
# // [ 0 0 0 0 0 1 ]
kf.transitionMatrix = np.array(np.diag(np.repeat(1, stateSize)),
dtype=np.float32)