SVCAM_examples/face_recognition.py at main · AppliedRobotics/SVCAM_examples · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
#!/usr/bin/env python3
from __future__ import print_function

import roslib
import sys
import rospy
import cv2
import numpy as np
import message_filters
from std_msgs.msg import String
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import math

class get_face_distance_from_camera:

  def __init__(self):

    self.bridge = CvBridge()

    self.camera_info_sub = message_filters.Subscriber('/trackingcam3d_client_ros/trackingcam3d0/left/camera_info', CameraInfo)

    self.image_sub = message_filters.Subscriber("/trackingcam3d_client_ros/trackingcam3d0/left/image_raw",Image)
    self.depth_sub = message_filters.Subscriber("/trackingcam3d_client_ros/trackingcam3d0/depth/image_rect",Image)

    self.ts = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.depth_sub, self.camera_info_sub], queue_size=10, slop=0.5)
    self.ts.registerCallback(self.callback)

    self.pub = rospy.Publisher('/trackingcam3d_client_ros/faces_recognition', Image, queue_size=1)

  def callback(self, rgb_data, depth_data, camera_info):

    try:
      camera_info_K = np.array(camera_info.K)

      # Intrinsic camera matrix for the raw (distorted) images.
      #     [fx  0 cx]
      # K = [ 0 fy cy]
      #     [ 0  0  1]

      m_fx = camera_info.K[0]
      m_fy = camera_info.K[4]
      m_cx = camera_info.K[2]
      m_cy = camera_info.K[5]
      inv_fx = 1. / m_fx
      inv_fy = 1. / m_fy


      cv_rgb = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
      depth_image = self.bridge.imgmsg_to_cv2(depth_data, "32FC1")
      depth_image_re = cv2.resize(depth_image, (640, 480))
      depth_array = np.array(depth_image_re, dtype=np.float32)
      cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
      depth_8 = (depth_array * 255).round().astype(np.uint8)
      cv_depth = np.zeros_like(cv_rgb)
      cv_depth[:,:,0] = depth_8
      cv_depth[:,:,1] = depth_8
      cv_depth[:,:,2] = depth_8

      face_cascade = cv2.CascadeClassifier('/root/catkin_ws/src/applied_robotics-trackingcam3d_client_ros/cfg/haar_cascade.xml')
      gray = cv2.cvtColor(cv_rgb, cv2.COLOR_BGR2GRAY)
      faces = face_cascade.detectMultiScale(gray, 1.3, 5)

      rgb_height, rgb_width, rgb_channels = cv_rgb.shape
      for (x,y,w,h) in faces:
        cv2.rectangle(cv_rgb,(x,y),(x+w,y+h),(255,0,0),2)
        cv2.rectangle(cv_depth,(x,y),(x+w,y+h),(255,0,0),2)
        cv2.rectangle(cv_rgb,(x+30,y+30),(x+w-30,y+h-30),(0,0,255),2)
        cv2.rectangle(cv_depth,(x+30,y+30),(x+w-30,y+h-30),(0,0,255),2)
        roi_depth = depth_image_re[y+30:y+h-30, x+30:x+w-30]

        n = 0
        sum = 0
        for i in range(0,roi_depth.shape[0]):
            for j in range(0,roi_depth.shape[1]):
                value = roi_depth.item(i, j)
                if value > 0.:
                    n = n + 1
                    sum = sum + value
        if n !=0:
          mean_z = sum / n

          point_z = mean_z * 0.001; # distance in meters
          point_x = ((x + w/2) - m_cx) * point_z * inv_fx
          point_y = ((y + h/2) - m_cy) * point_z * inv_fy

          x_str = "X: " + str(format(point_x, '.2f'))
          y_str = "Y: " + str(format(point_y, '.2f'))
          z_str = "Z: " + str(format(point_z, '.2f'))


          dist = math.sqrt(point_x * point_x + point_y * point_y + point_z * point_z)

          dist_str = "dist:" + str(format(dist, '.2f')) + "m"
          print(dist_str)
          cv2.putText(cv_rgb, dist_str, (x+w, y+60), cv2.FONT_HERSHEY_SIMPLEX,
                    0.7, (0,255,0), 1, cv2.LINE_AA)

    except CvBridgeError as e:
      print(e)

    rgbd = np.concatenate((cv_rgb, cv_depth), axis=1)

    #convert opencv format back to ros format and publish result
    try:
      faces_message = self.bridge.cv2_to_imgmsg(rgbd, "bgr8")
      self.pub.publish(faces_message)
    except CvBridgeError as e:
      print(e)


def main(args):
  rospy.init_node('unibas_face_distance_calculator', anonymous=True)
  fd = get_face_distance_from_camera()
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")

if __name__ == '__main__':
    main(sys.argv)