scripts/camshift.py

   1 #!/usr/bin/env python
   2
   3 '''
   4 Camshift tracker
   5 ================
   6
   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)
  10
  11 http://www.robinhewitt.com/research/track/camshift.html
  12
  13 Usage:
  14 ------
  15         camshift.py [<video source>]
  16
  17         To initialize tracking, select the object with mouse
  18
  19 Keys:
  20 -----
  21         ESC   - exit
  22         b        - toggle back-projected probability visualization
  23 '''
  24
  25 import rospy
  26 from sensor_msgs.msg import Image
  27 import numpy as np
  28 import cv2
  29 from cv_bridge import CvBridge, CvBridgeError
  30
  31 class App:
  32         def __init__(self):
  33                 rospy.init_node('camshift')
  34                 rospy.Subscriber("/usb_cam/image_raw", Image, self.cmdImageReceived)
  35                 self.bridge = CvBridge()
  36
  37                 self.first_run = True
  38                 self.selection = None
  39                 self.drag_start = None
  40                 self.tracking_state = 0
  41                 self.show_backproj = False
  42
  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
  48                 if self.drag_start:
  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]))
  58                                 self.selection = None
  59                                 if x1-x0 > 0 and y1-y0 > 0:
  60                                         self.selection = (x0, y0, x1, y1)
  61
  62         def show_hist(self):
  63                 bin_count = self.hist.shape[0]
  64                 bin_w = 24
  65                 img = np.zeros((256, bin_count*bin_w, 3), np.uint8)
  66                 for i in xrange(bin_count):
  67                         h = int(self.hist[i])
  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)
  71
  72         def run(self):
  73                 rospy.spin()
  74                 cv2.destroyAllWindows()
  75
  76         def cmdImageReceived(self, msg):
  77                 if self.first_run:
  78                         cv2.namedWindow('camshift')
  79                         cv2.namedWindow('hist')
  80                         cv2.setMouseCallback('camshift', self.onmouse)
  81                         self.first_run = False
  82
  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.)))
  87
  88                 if self.selection:
  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)
  96                         self.show_hist()
  97
  98                         vis_roi = vis[y0:y1, x0:x1]
  99                         cv2.bitwise_not(vis_roi, vis_roi)
 100                         vis[mask == 0] = 0
 101
 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)
 105                         prob &= mask
 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)
 108
 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
 113
 114                 cv2.imshow('camshift', vis)
 115
 116                 ch = 0xFF & cv2.waitKey(5)
 117                 if ch == 27:
 118                         rospy.signal_shutdown("Exit from keyboard")
 119                 if ch == ord('b'):
 120                         self.show_backproj = not self.show_backproj
 121
 122
 123 if __name__ == '__main__':
 124         App().run()