小车AI视觉交互--4.人脸跟随

一、实验目标

        结合摄像头云台实现人脸跟随。通过对人脸对象进行识别，并检测识别到的人脸的外切圆的圆形的x,y坐标与画面中心的差值，运用PID算法控制Y轴舵机和车体运动使识别目标位于画面中心位置。

二、实验源码

#bgr8转jpeg格式 bgr8 to jpeg format
import enum
import cv2
​
def bgr8_to_jpeg(value, quality=75):
    return bytes(cv2.imencode('.jpg', value)[1])
import sys
sys.path.append('/home/pi/project_demo/lib')
#导入麦克纳姆小车驱动库 Import Mecanum Car Driver Library
from McLumk_Wheel_Sports import *
​
import cv2
import mediapipe as mp
import ipywidgets.widgets as widgets
import threading
import time
import sys
import math
​
image_widget = widgets.Image(format='jpeg', width=640, height=480)
global face_x, face_y, face_w, face_h
face_x = face_y = face_w = face_h = 0
global target_valuex
target_valuex = 2048
global target_valuey
target_valuey = 2048
import PID
#xservo_pid = PID.PositionalPID(1.1, 0.4, 0.01)#1.1 0.4 0.01
direction_pid = PID.PositionalPID(0.8, 0, 0.2)
yservo_pid = PID.PositionalPID(0.8, 0.2, 0.01)
speed_pid = PID.PositionalPID(1.1, 0, 0.2)
​
# 定义 target_servox 和 target_servoy 在外部 Define target_servox and target_servoy externally
target_servox = 90
target_servoy = 25
def servo_reset():
    bot.Ctrl_Servo(1,90)
    bot.Ctrl_Servo(2,80)
servo_reset()
# 线程功能操作库 Thread function operation library
import inspect
import ctypes
def _async_raise(tid, exctype):
    """raises the exception, performs cleanup if needed"""
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        # """if it returns a number greater than one, you're in trouble,
        # and you should call it again with exc=NULL to revert the effect"""
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
        
def stop_thread(thread):
    _async_raise(thread.ident, SystemExit)
class FaceDetector:
    def __init__(self, minDetectionCon=0.5):
        self.mpFaceDetection = mp.solutions.face_detection
        self.mpDraw = mp.solutions.drawing_utils
        self.facedetection = self.mpFaceDetection.FaceDetection(min_detection_confidence=minDetectionCon)
​
    def findFaces(self, frame):
        img_RGB = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        self.results = self.facedetection.process(img_RGB)
        bboxs = []
        bbox=0,0,0,0
        center_x=center_y=0
        if self.results.detections:
            for id, detection in enumerate(self.results.detections):
                bboxC = detection.location_data.relative_bounding_box
                ih, iw, ic = frame.shape
                bbox = int(bboxC.xmin * iw), int(bboxC.ymin * ih), \
                       int(bboxC.width * iw), int(bboxC.height * ih)
                #计算中心点
                center_x = bbox[0] + bbox[2] // 2
                center_y = bbox[1] + bbox[3] // 2
                bboxs.append([id, bbox, detection.score])
                frame= self.fancyDraw(frame, bbox)
                # cv2.putText(frame, f'{int(detection.score[0] * 100)}%',
                #            (bbox[0], bbox[1] - 20), cv2.FONT_HERSHEY_PLAIN,
                #            3, (255, 0, 255), 2)
        return frame, bboxs, self.results.detections, bbox, center_x
​
    def fancyDraw(self, frame, bbox, l=30, t=5):
        x, y, w, h = bbox
        x1, y1 = x + w, y + h
        cv2.rectangle(frame, (x, y),(x + w, y + h), (0,255,0), 2)
        # Top left x,y
        cv2.line(frame, (x, y), (x + l, y), (0,255,0), t)
        cv2.line(frame, (x, y), (x, y + l), (0,255,0), t)
        # Top right x1,y
        cv2.line(frame, (x1, y), (x1 - l, y), (0,255,0), t)
        cv2.line(frame, (x1, y), (x1, y + l), (0,255,0), t)
        # Bottom left x1,y1
        cv2.line(frame, (x, y1), (x + l, y1), (0,255,0), t)
        cv2.line(frame, (x, y1), (x, y1 - l), (0,255,0), t)
        # Bottom right x1,y1
        cv2.line(frame, (x1, y1), (x1 - l, y1), (0,255,0), t)
        cv2.line(frame, (x1, y1), (x1, y1 - l), (0,255,0), t)
        return frame
​
image = cv2.VideoCapture(0)
image.set(3,320)
image.set(4,240)
# image.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
# image.set(cv2.CAP_PROP_BRIGHTNESS, 62) #设置亮度 -64 - 64  0.0 Set Brightness -64 - 64 0.0
# image.set(cv2.CAP_PROP_CONTRAST, 63) #设置对比度 -64 - 64  2.0 Set Contrast -64 - 64 2.0
# image.set(cv2.CAP_PROP_EXPOSURE, 4800) #设置曝光值 1.0 - 5000  156.0 Set the exposure value 1.0 - 5000 156.0
​
#csi
# from picamera2 import Picamera2, Preview
# import libcamera
# picam2 = Picamera2()  
# camera_config = picam2.create_preview_configuration(main={"format":'RGB888',"size":(320,240)})
# camera_config["transform"] = libcamera.Transform(hflip=1, vflip=1)
# picam2.configure(camera_config) 
# picam2.start()  
image = cv2.VideoCapture(0)
image.set(3,320)
image.set(4,240)
# image.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
# image.set(cv2.CAP_PROP_BRIGHTNESS, 62) #设置亮度 -64 - 64  0.0 Set Brightness -64 - 64 0.0
# image.set(cv2.CAP_PROP_CONTRAST, 63) #设置对比度 -64 - 64  2.0 Set Contrast -64 - 64 2.0
# image.set(cv2.CAP_PROP_EXPOSURE, 4800) #设置曝光值 1.0 - 5000  156.0 Set the exposure value 1.0 - 5000 156.0
​
#csi
# from picamera2 import Picamera2, Preview
# import libcamera
# picam2 = Picamera2()  
# camera_config = picam2.create_preview_configuration(main={"format":'RGB888',"size":(320,240)})
# camera_config["transform"] = libcamera.Transform(hflip=1, vflip=1)
# picam2.configure(camera_config) 
# picam2.start()  

带死区控制，跟随实时性差一些，舵机在死区范围内不运动，抖动较稳定 

def Face_Follow():
    global x,w,y,h
    speed=30
    face_detector = FaceDetector(0.75)
    while 1:
        ret, frame = image.read()
        #frame = picam2.capture_array()
        faces,_,descore,bbox,center_x= face_detector.findFaces(frame)
        x,y,w,h = bbox
        if descore:
            direction_pid.SystemOutput = center_x
            direction_pid.SetStepSignal(250)
            direction_pid.SetInertiaTime(0.01, 0.05)
            target_valuex = int(direction_pid.SystemOutput+65)
            # 输入Y轴方向参数PID控制输入 Input Y-axis direction parameter PID control input
            if math.fabs(180 - (y + h/2)) > 40:
                yservo_pid.SystemOutput = y + h/2
                yservo_pid.SetStepSignal(280)
                yservo_pid.SetInertiaTime(0.01, 0.05)
                target_valuey = int(1150+yservo_pid.SystemOutput)
                target_servoy = int((target_valuey-500)/10)                   
                #print("target_servoy %d", target_servoy)  
                if target_servoy > 100:
                    target_servoy = 100
                if target_servoy < 0:
                    target_servoy = 0        
                bot.Ctrl_Servo(2, target_servoy)
​
            speed_pid.SystemOutput = int(h/2)
            speed_pid.SetStepSignal(80)
            speed_pid.SetInertiaTime(0.01, 0.1)               
            speed_value = int(speed_pid.SystemOutput)
​
            # 打印文本到图像
            text = f"color_radius {int(h/2)} target_valuex {target_valuex}"
            font = cv2.FONT_HERSHEY_SIMPLEX
            font_scale = 1
            color = (255, 0, 0)  # 白色
            thickness = 2
            text_position = (10, 60)  # 文本位置
            cv2.putText(faces, text, text_position, font, font_scale, color, thickness)
            #print("color_radius %d target_valuex%d", h/2,target_valuex)
​
            if speed_value > 255:
                speed_value = 255
            if speed_value < 0:
                speed_value = 0
                
            if(target_valuex>50):
                rotate_left(int(speed/5))# speed
            elif(target_valuex<-50):
                rotate_right(int(speed/5))
            elif(75<h/2<100):#调试目标半径75~100 Debug target radius 65~90
                stop_robot()
            elif(h/2>60):#调试目标半径58 Debug target radius 58
                if(abs(target_valuex)<30):
                    move_backward(speed)
            elif(20<h/2<55):
                if(abs(target_valuex)<30):
                    move_forward(speed_value)
            else:stop_robot()
​
            #bot.Ctrl_Servo(2,target_servoy)
        
        else:
            stop_robot()
        try:
            image_widget.value = bgr8_to_jpeg(faces)
        except:
            continue

display(image_widget)
thread1 = threading.Thread(target=Face_Follow)
thread1.daemon=True
thread1.start()
​
#picam2.stop()
#picam2.close()
#结束进程，释放摄像头，需要结束时执行 End the process, release the camera, and execute when it is finished
stop_thread(thread1)
#释放摄像头资源 Release camera resources
image.release()
#复位舵机 Reset servo
bot.Ctrl_Servo(1,90)
bot.Ctrl_Servo(2,25)

三、实验现象

        代码块运行后，我们将人脸出现在摄像头前，摄像头识别人脸后会控制云台的Y轴和车体，跟随人脸移动方向移动。