vision_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 # Copyright (c) 2010, Willow Garage Inc. 
00004 # Copyright (c) 2012, Tang Tiong Yew
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 """ vision_node.py - Version 1.0 2010-12-28
00035 
00036     Modification of the ROS OpenCV Camshift example using cv_bridge and publishing the ROI
00037     coordinates to the /roi topic.   
00038 """
00039 
00040 import roslib
00041 roslib.load_manifest('pi_head_tracking_tutorial')
00042 import sys
00043 import rospy
00044 import cv
00045 from std_msgs.msg import String
00046 from sensor_msgs.msg import Image, RegionOfInterest, CameraInfo
00047 from cv_bridge import CvBridge, CvBridgeError
00048 
00049 class vision_node:
00050     def __init__(self):
00051         rospy.init_node('vision_node')
00052                 
00053         self.ROI = rospy.Publisher("roi", RegionOfInterest)
00054         
00055         """ Give the camera driver a moment to come up. """
00056         rospy.sleep(3)
00057 
00058         """ Create the display window """
00059         self.cv_window_name = "OpenCV Image"
00060         cv.NamedWindow(self.cv_window_name, 0)
00061         
00062         """ Create the cv_bridge object """
00063         self.bridge = CvBridge()
00064         
00065         """ Subscribe to the raw camera image topic """
00066         self.image_sub = rospy.Subscriber("input", Image, self.callback)
00067         
00068         """ Set up a smaller window to display the CamShift histogram. """
00069         cv.NamedWindow("Histogram", 0)
00070         cv.MoveWindow("Histogram", 700, 10)
00071         cv.SetMouseCallback(self.cv_window_name, self.on_mouse)
00072 
00073         self.drag_start = None      # Set to (x,y) when mouse starts dragtime
00074         self.track_window = None    # Set to rect when the mouse drag finishes
00075 
00076         self.hist = cv.CreateHist([180], cv.CV_HIST_ARRAY, [(0,180)], 1 )
00077         self.backproject_mode = False
00078 
00079     def callback(self, data):
00080         """ Convert the raw image to OpenCV format using the convert_image() helper function """
00081         cv_image = self.convert_image(data)
00082         
00083         """ Apply the CamShift algorithm using the do_camshift() helper function """
00084         cv_image = self.do_camshift(cv_image)
00085         
00086         """ Refresh the displayed image """
00087         cv.ShowImage(self.cv_window_name, cv_image)
00088         
00089         """ Toggle between the normal and back projected image if user hits the 'b' key """
00090         c = cv.WaitKey(7) % 0x100
00091         if c == 27:
00092             return
00093         elif c == ord("b"):
00094             self.backproject_mode = not self.backproject_mode
00095           
00096     def convert_image(self, ros_image):
00097         try:
00098           cv_image = self.bridge.imgmsg_to_cv(ros_image, "bgr8")
00099           return cv_image
00100         except CvBridgeError, e:
00101           print e
00102 
00103     def do_camshift(self, cv_image):
00104         """ Get the image size """
00105         image_size = cv.GetSize(cv_image)
00106         image_width = image_size[0]
00107         image_height = image_size[1]
00108         
00109         """ Convert to HSV and keep the hue """
00110         hsv = cv.CreateImage(image_size, 8, 3)
00111         cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV)
00112         self.hue = cv.CreateImage(image_size, 8, 1)
00113         cv.Split(hsv, self.hue, None, None, None)
00114 
00115         """ Compute back projection """
00116         backproject = cv.CreateImage(image_size, 8, 1)
00117 
00118         """ Run the cam-shift algorithm """
00119         cv.CalcArrBackProject( [self.hue], backproject, self.hist )
00120         cv.ShowImage("Backproject", backproject)
00121         if self.track_window and is_rect_nonzero(self.track_window):
00122             crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
00123             (iters, (area, value, rect), track_box) = cv.CamShift(backproject, self.track_window, crit)
00124             self.track_window = rect
00125 
00126         """ If mouse is pressed, highlight the current selected rectangle
00127             and recompute the histogram """
00128 
00129         if self.drag_start and is_rect_nonzero(self.selection):
00130             sub = cv.GetSubRect(cv_image, self.selection)
00131             save = cv.CloneMat(sub)
00132             cv.ConvertScale(cv_image, cv_image, 0.5)
00133             cv.Copy(save, sub)
00134             x,y,w,h = self.selection
00135             cv.Rectangle(cv_image, (x,y), (x+w,y+h), (255,255,255))
00136 
00137             sel = cv.GetSubRect(self.hue, self.selection )
00138             cv.CalcArrHist( [sel], self.hist, 0)
00139             (_, max_val, _, _) = cv.GetMinMaxHistValue(self.hist)
00140             if max_val != 0:
00141                 cv.ConvertScale(self.hist.bins, self.hist.bins, 255. / max_val)
00142         elif self.track_window and is_rect_nonzero(self.track_window):
00143             """ The width and height on the returned track_box seem to be exchanged--a bug in CamShift? """
00144             ((x, y), (w, h), angle) = track_box
00145             track_box = ((x, y), (h, w), angle)
00146             cv.EllipseBox(cv_image, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0)
00147             
00148             roi = RegionOfInterest()
00149             roi.x_offset = int(min(image_width, max(0, track_box[0][0] - track_box[1][0] / 2)))
00150             roi.y_offset = int(min(image_height, max(0, track_box[0][1] - track_box[1][1] / 2)))
00151             roi.width = int(track_box[1][0])
00152             roi.height = int(track_box[1][1])
00153             self.ROI.publish(roi)
00154 
00155         cv.ShowImage("Histogram", self.hue_histogram_as_image(self.hist))
00156         
00157         if not self.backproject_mode:
00158             return cv_image
00159         else:
00160             return backproject
00161         
00162 
00163     def hue_histogram_as_image(self, hist):
00164             """ Returns a nice representation of a hue histogram """
00165     
00166             histimg_hsv = cv.CreateImage( (320,200), 8, 3)
00167             
00168             mybins = cv.CloneMatND(hist.bins)
00169             cv.Log(mybins, mybins)
00170             (_, hi, _, _) = cv.MinMaxLoc(mybins)
00171             cv.ConvertScale(mybins, mybins, 255. / hi)
00172     
00173             w,h = cv.GetSize(histimg_hsv)
00174             hdims = cv.GetDims(mybins)[0]
00175             for x in range(w):
00176                 xh = (180 * x) / (w - 1)  # hue sweeps from 0-180 across the image
00177                 val = int(mybins[int(hdims * x / w)] * h / 255)
00178                 cv.Rectangle( histimg_hsv, (x, 0), (x, h-val), (xh,255,64), -1)
00179                 cv.Rectangle( histimg_hsv, (x, h-val), (x, h), (xh,255,255), -1)
00180     
00181             histimg = cv.CreateImage( (320,200), 8, 3)
00182             cv.CvtColor(histimg_hsv, histimg, cv.CV_HSV2BGR)
00183             return histimg
00184 
00185     def on_mouse(self, event, x, y, flags, param):
00186         if event == cv.CV_EVENT_LBUTTONDOWN:
00187             self.drag_start = (x, y)
00188         if event == cv.CV_EVENT_LBUTTONUP:
00189             self.drag_start = None
00190             self.track_window = self.selection
00191         if self.drag_start:
00192             xmin = min(x, self.drag_start[0])
00193             ymin = min(y, self.drag_start[1])
00194             xmax = max(x, self.drag_start[0])
00195             ymax = max(y, self.drag_start[1])
00196             self.selection = (xmin, ymin, xmax - xmin, ymax - ymin)
00197 
00198 def is_rect_nonzero(r):
00199     (_,_,w,h) = r
00200     return (w > 0) and (h > 0)           
00201 
00202 def main(args):
00203       vn = vision_node()
00204       try:
00205         rospy.spin()
00206       except KeyboardInterrupt:
00207         print "Shutting down vision node."
00208         cv.DestroyAllWindows()
00209 
00210 if __name__ == '__main__':
00211     main(sys.argv)


eddiebot_head_tracking
Author(s): Tang Tiong Yew
autogenerated on Tue Jan 7 2014 11:10:11