基於ROS的人臉識別

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
import cv_bridge


class FaceDetector:
    def __init__(self):
        rospy.on_shutdown(self.cleanup)

        # 創建cv_bridge
        self.bridge = cv_bridge.CvBridge()
        self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)
        self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback, queue_size=1)
        # self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback, queue_size=1)

        # 獲取haar特征的級聯表的XML文件，文件路徑在launch文件中傳入
        cascade_1 = rospy.get_param("~cascade_1", "～/catkin_ws/src/opencv/data/haar_detectors/haarcascade_frontalface_alt.xml")
        cascade_2 = rospy.get_param("~cascade_2", "～/catkin_ws/src/opencv/data/haar_detectors/haarcascade_profileface.xml")

        # 使用級聯表初始化haar特征檢測器
        self.cascade_1 = cv2.CascadeClassifier(cascade_1)
        self.cascade_2 = cv2.CascadeClassifier(cascade_2)

        # 設置級聯表的參數，優化人臉識別，可以在launch文件中重新配置
        self.haar_scaleFactor = rospy.get_param("~haar_scaleFactor", 1.2)
        self.haar_minNeighbors = rospy.get_param("~haar_minNeighbors", 2)
        self.haar_minSize = rospy.get_param("~haar_minSize", 40)
        self.haar_maxSize = rospy.get_param("~haar_maxSize", 60)
        self.color = (50, 255, 50)

    def image_callback(self, data):
        # 使用cv_bridge將ROS的圖像數據轉換成OpenCV的圖像格式
        cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        frame = np.array(cv_image, dtype=np.uint8)

        # 創建灰度圖像
        grey_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        # 創建平衡直方圖，減少光線影響
        grey_image = cv2.equalizeHist(grey_image)

        # 嘗試檢測人臉
        faces_result = self.detect_face(grey_image)

        # 在opencv的窗口中框出所有人臉區域
        if len(faces_result) > 0:
            for face in faces_result:
                x, y, w, h = face
                cv2.rectangle(cv_image, (x, y), (x + w, y + h), self.color, 2)

        # 將識別后的圖像轉換成ROS消息並發布
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))

    def detect_face(self, input_image):
        # 首先匹配正面人臉的模型
        if self.cascade_1:
            faces = self.cascade_1.detectMultiScale(input_image,
                                                    self.haar_scaleFactor,
                                                    self.haar_minNeighbors,
                                                    cv2.CASCADE_SCALE_IMAGE,
                                                    (self.haar_minSize, self.haar_maxSize))

        # 如果正面人臉匹配失敗，那么就嘗試匹配側面人臉的模型
        if len(faces) == 0 and self.cascade_2:
            faces = self.cascade_2.detectMultiScale(input_image,
                                                    self.haar_scaleFactor,
                                                    self.haar_minNeighbors,
                                                    cv2.CASCADE_SCALE_IMAGE,
                                                    (self.haar_minSize, self.haar_maxSize))

        return faces

    def cleanup(self):
        print("強制結束程序。。")
        cv2.destroyAllWindows()


if __name__ == '__main__':
    try:
        # 初始化ros節點
        rospy.init_node("face_detector")
        follower = FaceDetector()
        rospy.loginfo("人臉識別已經啟動。。。")
        rospy.loginfo("請打開opencv節點訂閱消息。。。")
        rospy.spin()
    except KeyboardInterrupt:
        print("強制結束程序。。")
        cv2.destroyAllWindows()