def __init__(self, debug=False, disable_depth = True, cam_ns = "/head_kinect"):
self._debug = debug
self._disable_depth = disable_depth
self._img_topic = cam_ns + "/rgb/image_rect_color/compressed"
# self._depth_topic = "/camera/depth_registered/hw_registered/image_rect_raw/compressed"
self._depth_topic = cam_ns + "/depth_registered/hw_registered/image_rect_raw"
self._info_topic = cam_ns + "/rgb/camera_info"
# self._img_sub = rospy.Subscriber(img_topic, CompressedImage, self.img_cb, queue_size = 1)
self._img_sub = message_filters.Subscriber(self._img_topic, CompressedImage)
# self._depth_sub = message_filters.Subscriber(self._depth_topic, CompressedImage)
self._depth_sub = message_filters.Subscriber(self._depth_topic, Image)
self._info_sub = message_filters.Subscriber(self._info_topic, CameraInfo)
self._point_pub = rospy.Publisher('nearest_face', PointStamped)
self._img = None
self._img_gray = None
self._img_faces = None
self._depth = None
self._depth_vis = None
self._cam_model = None
self._face_det = cv2.CascadeClassifier('/opt/ros/hydro/share/OpenCV/haarcascades/haarcascade_frontalface_alt2.xml')
self._face_det2 = cv2.CascadeClassifier('/opt/ros/hydro/share/OpenCV/haarcascades/haarcascade_profileface.xml')
if self._debug:
rospy.loginfo("Ready (debug mode)")
# self._timer = rospy.Timer(rospy.Duration(0.1), self.timer)
self._display = threading.Thread(target=self.timer)
self._display.daemon = True
self._display.start()
else:
rospy.loginfo("Ready")
if self._disable_depth:
rospy.loginfo("Subscribing to: " + self._img_topic + " and " + self._info_topic + ".")
self._ts = ApproximateSynchronizer(0.25, [self._img_sub, self._info_sub], 3)
self._ts.registerCallback(self.kinect_cb)
else:
rospy.loginfo("Subscribing to: " + self._img_topic + ", " + self._depth_topic + " and " + self._info_topic + ".")
self._ts = ApproximateSynchronizer(0.25, [self._img_sub, self._depth_sub, self._info_sub], 3)
self._ts.registerCallback(self.kinect_depth_cb)
def test_approx(self):
m0 = MockFilter()
m1 = MockFilter()
ts = ApproximateSynchronizer(0.1, [m0, m1], 1)
ts.registerCallback(self.cb_collector_2msg)
if 0:
# Simple case, pairs of messages, make sure that they get combined
for t in range(10):
self.collector = []
msg0 = MockMessage(t, 33)
msg1 = MockMessage(t, 34)
m0.signalMessage(msg0)
self.assertEqual(self.collector, [])
m1.signalMessage(msg1)
self.assertEqual(self.collector, [(msg0, msg1)])
# Scramble sequences of length N. Make sure that TimeSequencer recombines them.
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
seq1 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
# random.shuffle(seq0)
ts = ApproximateSynchronizer(0.1, [m0, m1], N)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for msg in random.sample(seq1, N):
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
def test_approx(self):
m0 = MockFilter()
m1 = MockFilter()
ts = ApproximateSynchronizer(0.1, [m0, m1], 1)
ts.registerCallback(self.cb_collector_2msg)
if 0:
# Simple case, pairs of messages, make sure that they get combined
for t in range(10):
self.collector = []
msg0 = MockMessage(t, 33)
msg1 = MockMessage(t, 34)
m0.signalMessage(msg0)
self.assertEqual(self.collector, [])
m1.signalMessage(msg1)
self.assertEqual(self.collector, [(msg0, msg1)])
# Scramble sequences of length N. Make sure that TimeSequencer recombines them.
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [
MockMessage(rospy.Time(t), random.random()) for t in range(N)
]
seq1 = [
MockMessage(rospy.Time(t), random.random()) for t in range(N)
]
# random.shuffle(seq0)
ts = ApproximateSynchronizer(0.1, [m0, m1], N)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for msg in random.sample(seq1, N):
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
class FaceDetector:
def __init__(self, debug=False):
self._debug = debug
self._img_topic = "/camera/rgb/image_rect_color/compressed"
# self._depth_topic = "/camera/depth_registered/hw_registered/image_rect_raw/compressed"
self._depth_topic = "/camera/depth_registered/hw_registered/image_rect_raw"
self._info_topic = "/camera/rgb/camera_info"
# self._img_sub = rospy.Subscriber(img_topic, CompressedImage, self.img_cb, queue_size = 1)
self._img_sub = message_filters.Subscriber(self._img_topic, CompressedImage)
# self._depth_sub = message_filters.Subscriber(self._depth_topic, CompressedImage)
self._depth_sub = message_filters.Subscriber(self._depth_topic, Image)
self._info_sub = message_filters.Subscriber(self._info_topic, CameraInfo)
self._ts = ApproximateSynchronizer(0.1, [self._img_sub, self._depth_sub, self._info_sub], 1)
self._ts.registerCallback(self.kinect_cb)
self._point_pub = rospy.Publisher('nearest_face', PointStamped)
self._img = None
self._img_gray = None
self._img_faces = None
self._depth = None
self._depth_vis = None
self._cam_model = None
self._face_det = cv2.CascadeClassifier('/opt/ros/hydro/share/OpenCV/haarcascades/haarcascade_frontalface_alt2.xml')
self._face_det2 = cv2.CascadeClassifier('/opt/ros/hydro/share/OpenCV/haarcascades/haarcascade_profileface.xml')
if self._debug:
rospy.loginfo("Ready (debug mode)")
# self._timer = rospy.Timer(rospy.Duration(0.1), self.timer)
self._display = threading.Thread(target=self.timer)
self._display.daemon = True
self._display.start()
else:
rospy.loginfo("Ready")
def kinect_cb(self, image, depth, info):
if self._debug and self._img is None:
rospy.loginfo("Kinect data received!")
assert image.header.frame_id == depth.header.frame_id
if self._cam_model is None:
self._cam_model = image_geometry.PinholeCameraModel()
self._cam_model.fromCameraInfo(info)
np_arr = np.fromstring(image.data, np.uint8)
self._img = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
assert self._img is not None
# print "depth: " + str(len(depth.data))
# np_arr_d = np.fromstring(depth.data, np.uint16)
self._depth = np.fromstring(depth.data, np.uint16)
self._depth.resize((480, 640))
# self._depth = cv2.imdecode(np_arr_d, cv2.CV_LOAD_IMAGE_UNCHANGED)
assert self._depth is not None
self._img_gray = cv2.cvtColor(self._img, cv2.COLOR_BGR2GRAY)
self._img_gray = cv2.equalizeHist(self._img_gray)
rects = self._face_det.detectMultiScale(self._img_gray, 1.3, 4, cv2.cv.CV_HAAR_SCALE_IMAGE, (10, 10))
if len(rects) == 0:
rects = self._face_det2.detectMultiScale(self._img_gray, 1.3, 4, cv2.cv.CV_HAAR_SCALE_IMAGE, (10, 10))
if len(rects) > 0:
rospy.loginfo("backup detector worked")
max_area = 0
max_area_idx = None
# find largest face
for idx, val in enumerate(rects):
if (val[2] * val[3]) > max_area:
max_area_idx = idx
if max_area_idx is not None:
assert rects[max_area_idx][2] == rects[max_area_idx][3]
size = rects[max_area_idx][2]
#pt1 = (rects[max_area_idx][0] + size/4, rects[max_area_idx][1] + size/4)
#pt2 = (rects[max_area_idx][0] + size - size/4, rects[max_area_idx][1] + size - size/4)
pt1 = (rects[max_area_idx][0], rects[max_area_idx][1])
pt2 = (rects[max_area_idx][0] + size, rects[max_area_idx][1] + size)
if self._debug:
dbg_img = self._img_gray
cv2.rectangle(dbg_img, pt1, pt2, (127, 255, 0), 2)
self._img_faces = dbg_img
self._depth_vis = self._depth
cv2.rectangle(self._depth_vis, pt1, pt2, (127, 255, 0), 2)
dist_face = self._depth[pt1[0]:pt2[0], pt1[1]:pt2[1]]
#dist = sp.median(dist_face)
dist_face = median_filter(dist_face, 3) # TODO make filter size proportional to image size?
ar = np.array([])
for (x,y), value in np.ndenumerate(dist_face):
if (np.isnan(value)) or (value == 0) or (value == 127):
continue
ar = np.append(ar, value)
if len(ar) == 0:
dbg_img = self._img_gray
self._img_faces = dbg_img
return
dist = np.amin(ar) # min value
cx = rects[max_area_idx][0] + rects[max_area_idx][2] / 2
cy = rects[max_area_idx][1] + rects[max_area_idx][3] / 2
pts = PointStamped()
pts.header.stamp = image.header.stamp
pts.header.frame_id = image.header.frame_id
ray = self._cam_model.projectPixelTo3dRay((cx, cy))
pt = np.dot(ray, dist / 1000.0)
pts.point.x = pt[0]
pts.point.y = pt[1]
pts.point.z = pt[2]
self._point_pub.publish(pts)
else:
dbg_img = self._img_gray
self._img_faces = dbg_img
def timer(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
if self._img_faces is not None and self._depth_vis is not None:
# TODO mutex?
cv2.imshow('detected_faces', self._img_faces)
cv2.imshow('depth', self._depth_vis)
cv2.waitKey(1)
r.sleep()
