7 This is a demo that shows mean-shift based tracking
8 You select a color objects such as your face and it tracks it.
9 This reads from video camera (0 by default, or the camera number the user enters)
11 http://www.robinhewitt.com/research/track/camshift.html
15 camshift.py [<video source>]
17 To initialize tracking, select the object with mouse
22 b - toggle back-projected probability visualization
26 from sensor_msgs.msg import Image
29 from cv_bridge import CvBridge, CvBridgeError
33 rospy.init_node('camshift')
34 rospy.Subscriber("/usb_cam/image_raw", Image, self.cmdImageReceived)
35 self.bridge = CvBridge()
39 self.drag_start = None
40 self.tracking_state = 0
41 self.show_backproj = False
43 def onmouse(self, event, x, y, flags, param):
44 x, y = np.int16([x, y]) # BUG
45 if event == cv2.EVENT_LBUTTONDOWN:
46 self.drag_start = (x, y)
47 self.tracking_state = 0
49 if event == cv2.EVENT_LBUTTONUP:
50 self.drag_start = None
51 if self.selection is not None:
52 self.tracking_state = 1
53 elif flags & cv2.EVENT_FLAG_LBUTTON:
54 h, w = self.frame.shape[:2]
55 xo, yo = self.drag_start
56 x0, y0 = np.maximum(0, np.minimum([xo, yo], [x, y]))
57 x1, y1 = np.minimum([w, h], np.maximum([xo, yo], [x, y]))
59 if x1-x0 > 0 and y1-y0 > 0:
60 self.selection = (x0, y0, x1, y1)
63 bin_count = self.hist.shape[0]
65 img = np.zeros((256, bin_count*bin_w, 3), np.uint8)
66 for i in xrange(bin_count):
68 cv2.rectangle(img, (i*bin_w+2, 255), ((i+1)*bin_w-2, 255-h), (int(180.0*i/bin_count), 255, 255), -1)
69 img = cv2.cvtColor(img, cv2.COLOR_HSV2BGR)
70 cv2.imshow('hist', img)
74 cv2.destroyAllWindows()
76 def cmdImageReceived(self, msg):
78 cv2.namedWindow('camshift')
79 cv2.namedWindow('hist')
80 cv2.setMouseCallback('camshift', self.onmouse)
81 self.first_run = False
83 self.frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
84 vis = self.frame.copy()
85 hsv = cv2.cvtColor(self.frame, cv2.COLOR_BGR2HSV)
86 mask = cv2.inRange(hsv, np.array((0., 60., 32.)), np.array((180., 255., 255.)))
89 x0, y0, x1, y1 = self.selection
90 self.track_window = (x0, y0, x1-x0, y1-y0)
91 hsv_roi = hsv[y0:y1, x0:x1]
92 mask_roi = mask[y0:y1, x0:x1]
93 hist = cv2.calcHist( [hsv_roi], [0], mask_roi, [16], [0, 180] )
94 cv2.normalize(hist, hist, 0, 255, cv2.NORM_MINMAX);
95 self.hist = hist.reshape(-1)
98 vis_roi = vis[y0:y1, x0:x1]
99 cv2.bitwise_not(vis_roi, vis_roi)
102 if self.tracking_state == 1 and self.track_window != (0, 0, 0, 0):
103 self.selection = None
104 prob = cv2.calcBackProject([hsv], [0], self.hist, [0, 180], 1)
106 term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1 )
107 track_box, self.track_window = cv2.CamShift(prob, self.track_window, term_crit)
109 if self.show_backproj:
110 vis[:] = prob[...,np.newaxis]
111 try: cv2.ellipse(vis, track_box, (0, 0, 255), 2)
112 except: print track_box
114 cv2.imshow('camshift', vis)
116 ch = 0xFF & cv2.waitKey(5)
118 rospy.signal_shutdown("Exit from keyboard")
120 self.show_backproj = not self.show_backproj
123 if __name__ == '__main__':