rosfacedetect.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 """
00003 This program is demonstration for face and object detection using haar-like features.
00004 The program finds faces in a camera image or video stream and displays a red box around them.
00005 
00006 Original C implementation by:  ?
00007 Python implementation by: Roman Stanchak, James Bowman
00008 """
00009 
00010 import sys
00011 import os
00012 from optparse import OptionParser
00013 
00014 import rospy
00015 import sensor_msgs.msg
00016 from cv_bridge import CvBridge
00017 import cv2
00018 import numpy
00019 
00020 # Parameters for haar detection
00021 # From the API:
00022 # The default parameters (scale_factor=2, min_neighbors=3, flags=0) are tuned 
00023 # for accurate yet slow object detection. For a faster operation on real video 
00024 # images the settings are: 
00025 # scale_factor=1.2, min_neighbors=2, flags=CV_HAAR_DO_CANNY_PRUNING, 
00026 # min_size=<minimum possible face size
00027 
00028 min_size = (10, 10)
00029 image_scale = 2
00030 haar_scale = 1.2
00031 min_neighbors = 2
00032 haar_flags = 0
00033 
00034 if __name__ == '__main__':
00035 
00036     haarfile = '/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml'
00037 
00038     parser = OptionParser(usage = "usage: %prog [options]")
00039     parser.add_option("-c", "--cascade", action="store", dest="cascade", type="str", help="Haar cascade file, default %default", default = haarfile)
00040     parser.add_option("-t", "--topic", action="store", dest="topic", type="str", help="Topic to find a face on, default %default", default = '/camera/rgb/image_raw')
00041     (options, args) = parser.parse_args()
00042 
00043     cascade = cv2.CascadeClassifier()
00044     cascade.load(options.cascade)
00045     br = CvBridge()
00046 
00047     def detect_and_draw(imgmsg):
00048         img = br.imgmsg_to_cv2(imgmsg, "bgr8")
00049         # allocate temporary images
00050         new_size = (int(img.shape[1] / image_scale), int(img.shape[0] / image_scale))
00051 
00052         # convert color input image to grayscale
00053         gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
00054 
00055         # scale input image for faster processing
00056         small_img = cv2.resize(gray, new_size, interpolation = cv2.INTER_LINEAR)
00057 
00058         small_img = cv2.equalizeHist(small_img)
00059 
00060         if(cascade):
00061             faces = cascade.detectMultiScale(small_img, haar_scale, min_neighbors, haar_flags, min_size)
00062             if faces is not None:
00063                 for (x, y, w, h) in faces:
00064                     # the input to detectMultiScale was resized, so scale the 
00065                     # bounding box of each face and convert it to two CvPoints
00066                     pt1 = (int(x * image_scale), int(y * image_scale))
00067                     pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
00068                     cv2.rectangle(img, pt1, pt2, (255, 0, 0), 3, 8, 0)
00069 
00070         cv2.imshow("result", img)
00071         cv2.waitKey(6)
00072 
00073     rospy.init_node('rosfacedetect')
00074     rospy.Subscriber(options.topic, sensor_msgs.msg.Image, detect_and_draw)
00075     rospy.spin()


opencv_tests
Author(s): James Bowman
autogenerated on Mon Oct 6 2014 08:56:35