rosfacedetect.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 """
3 This program is demonstration for face and object detection using haar-like features.
4 The program finds faces in a camera image or video stream and displays a red box around them.
5 
6 Original C implementation by: ?
7 Python implementation by: Roman Stanchak, James Bowman
8 Updated: Copyright (c) 2016, Tal Regev.
9 """
10 
11 import sys
12 import os
13 from optparse import OptionParser
14 
15 import rospy
16 import sensor_msgs.msg
17 from cv_bridge import CvBridge
18 import cv2
19 import numpy
20 
21 # Parameters for haar detection
22 # From the API:
23 # The default parameters (scale_factor=2, min_neighbors=3, flags=0) are tuned
24 # for accurate yet slow object detection. For a faster operation on real video
25 # images the settings are:
26 # scale_factor=1.2, min_neighbors=2, flags=CV_HAAR_DO_CANNY_PRUNING,
27 # min_size=<minimum possible face size
28 
29 min_size = (10, 10)
30 image_scale = 2
31 haar_scale = 1.2
32 min_neighbors = 2
33 haar_flags = 0
34 
35 if __name__ == '__main__':
36 
37  # TODO add this file in the repository and make it relative to this python script. (not all people will run this from linux)
38  haarfile = '/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml'
39 
40  parser = OptionParser(usage = "usage: %prog [options]")
41  parser.add_option("-c", "--cascade", action="store", dest="cascade", type="str", help="Haar cascade file, default %default", default = haarfile)
42  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')
43  parser.add_option("-ct", "--ctopic", action="store", dest="ctopic", type="str", help="Compressed topic to find a face on, default %default", default = '/camera/rgb/image/compressed')
44  (options, args) = parser.parse_args()
45 
46  cascade = cv2.CascadeClassifier()
47  cascade.load(options.cascade)
48  br = CvBridge()
49 
50  def detect_and_draw(imgmsg):
51  img = br.imgmsg_to_cv2(imgmsg, "bgr8")
52  # allocate temporary images
53  new_size = (int(img.shape[1] / image_scale), int(img.shape[0] / image_scale))
54 
55  # convert color input image to grayscale
56  gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
57 
58  # scale input image for faster processing
59  small_img = cv2.resize(gray, new_size, interpolation = cv2.INTER_LINEAR)
60 
61  small_img = cv2.equalizeHist(small_img)
62 
63  if(cascade):
64  faces = cascade.detectMultiScale(small_img, haar_scale, min_neighbors, haar_flags, min_size)
65  if faces is not None:
66  for (x, y, w, h) in faces:
67  # the input to detectMultiScale was resized, so scale the
68  # bounding box of each face and convert it to two CvPoints
69  pt1 = (int(x * image_scale), int(y * image_scale))
70  pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
71  cv2.rectangle(img, pt1, pt2, (255, 0, 0), 3, 8, 0)
72 
73  cv2.imshow("result", img)
74  cv2.waitKey(6)
75 
76  def compressed_detect_and_draw(compressed_imgmsg):
77  img = br.compressed_imgmsg_to_cv2(compressed_imgmsg, "bgr8")
78  # allocate temporary images
79  new_size = (int(img.shape[1] / image_scale), int(img.shape[0] / image_scale))
80 
81  # convert color input image to grayscale
82  gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
83 
84  # scale input image for faster processing
85  small_img = cv2.resize(gray, new_size, interpolation = cv2.INTER_LINEAR)
86 
87  small_img = cv2.equalizeHist(small_img)
88 
89  if(cascade):
90  faces = cascade.detectMultiScale(small_img, haar_scale, min_neighbors, haar_flags, min_size)
91  if faces is not None:
92  for (x, y, w, h) in faces:
93  # the input to detectMultiScale was resized, so scale the
94  # bounding box of each face and convert it to two CvPoints
95  pt1 = (int(x * image_scale), int(y * image_scale))
96  pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
97  cv2.rectangle(img, pt1, pt2, (255, 0, 0), 3, 8, 0)
98 
99  cv2.imshow("compressed_result", img)
100  cv2.waitKey(6)
101 
102  rospy.init_node('rosfacedetect')
103  rospy.Subscriber(options.topic, sensor_msgs.msg.Image, detect_and_draw)
104  rospy.Subscriber(options.ctopic, sensor_msgs.msg.CompressedImage, compressed_detect_and_draw)
105  rospy.spin()
def detect_and_draw(imgmsg)
def compressed_detect_and_draw(compressed_imgmsg)


opencv_tests
Author(s): James Bowman
autogenerated on Thu Jun 6 2019 19:58:07