face_tracking.py

# -*- coding: utf-8 -*-

import Adafruit_PCA9685
import cv2
import numpy as np
from tensorflow.keras.applications.mobilenet_v2 import preprocess_input
from tensorflow.keras.preprocessing.image import img_to_array
from tensorflow.keras.models import load_model
import time


def decode_fourcc(v):
    v = int(v)
    return "".join([chr((v >> 8 * i) & 0xFF) for i in range(4)])


def get_max_size_facerect(boxes):
    max_x = 0
    max_y = 0
    max_w = 0
    max_h = 0
    max_size = -1
    for (startX, startY, endX, endY) in boxes:
        size = (endX - startX) * (endY - startY)
        if size > max_size:
            max_size = size
            max_x = startX
            max_y = startY
            max_w = endX - startX
            max_h = endY - startY
    return (max_x, max_y, max_w, max_h)


def clamp(x, minimum, maximum):
    return min(maximum, max(x, minimum))

# モデルのパスを指定する
prototxtPath = '/home/user/work/face-detection/deploy.prototxt'
weightsPath = '/home/user/work/face-detection/res10_300x300_ssd_iter_140000.caffemodel'
net = cv2.dnn.readNet(prototxtPath, weightsPath)
net.setPreferableBackend(cv2.dnn.DNN_BACKEND_CUDA)
net.setPreferableTarget(cv2.dnn.DNN_TARGET_CUDA)

# カメラのキャプチャ設定
capWidth = 640
capHeight = 480
window_center = [capWidth / 2, capHeight / 2]
# capFourcc = cv2.VideoWriter_fourcc('M','J','P','G')
capFourcc = cv2.VideoWriter_fourcc('Y', 'U', 'Y', 'V')
capFps = 30

# カメラ設定
cam = cv2.VideoCapture(0)
cam.set(cv2.CAP_PROP_FOURCC, capFourcc)
cam.set(cv2.CAP_PROP_FRAME_WIDTH, capWidth)
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, capHeight)
cam.set(cv2.CAP_PROP_FPS, capFps)

# フォーマット・解像度・FPSの取得
fourcc = decode_fourcc(cam.get(cv2.CAP_PROP_FOURCC))
width = cam.get(cv2.CAP_PROP_FRAME_WIDTH)
height = cam.get(cv2.CAP_PROP_FRAME_HEIGHT)
fps = cam.get(cv2.CAP_PROP_FPS)
print("fourcc:{} fps:{}　width:{}　height:{}".format(fourcc, fps, width, height))

# Initialise the PCA9685 using desired address and/or bus:
pwm = Adafruit_PCA9685.PCA9685(address=0x40, busnum=1)
pwm.set_pwm_freq(60)

SERVO_MIN = 200
SERVO_MAX = 550
SERVO_CENTER = [375, 375]
now_pulses = [SERVO_CENTER[0], SERVO_CENTER[1]]
k_p = -0.01
for i, p in enumerate(now_pulses):
    pwm.set_pwm(i, 0, p)

print("[INFO] loading face mask detector model...")
# マスク検出用モデルファイルを設定する
model = load_model('/home/user/work/face-detection/mask_detector.model')

start = time.time()

while(True):
    # qで終了
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

    # 画像取得
    ret, image = cam.read()
    orig = image.copy()
    (h, w) = image.shape[:2]

    blob = cv2.dnn.blobFromImage(image, 1.0, (300, 300), (104.0, 177.0, 123.0))

    print("[INFO] computing face detections...")
    net.setInput(blob)
    detections = net.forward()

    boxes = []
    for i in range(0, detections.shape[2]):
        confidence = detections[0, 0, i, 2]
        if confidence > 0.5:
            box = detections[0, 0, i, 3:7] * np.array([w, h, w, h])
            (startX, startY, endX, endY) = box.astype("int")

            (startX, startY) = (max(0, startX), max(0, startY))
            (endX, endY) = (min(w - 1, endX), min(h - 1, endY))

            if startX >= endX or startY >= endY:
                print('<ERROR> invalid range')
                continue

            boxes.append([startX, startY, endX, endY])
            print(
                "startX: {}, startY: {}, endX: {}, endY {}".format(
                    startX, startY, endX, endY))
            color = (0, 255, 0)
            cv2.rectangle(image, (startX, startY), (endX, endY), color, 2)

    print('boxes:', boxes)
    if len(boxes) > 0:
        max_size_color = (0, 255, 255)
        # 最大サイズの検出情報を取り出す
        x, y, width, height = get_max_size_facerect(boxes)
        face_x = int(x + width / 2)
        face_y = int(y + height / 2)
        cv2.rectangle(
            image, (x, y), (x + width, y + height), max_size_color, 2)
        cv2.drawMarker(image, (face_x, face_y), (0, 0, 255))
        cv2.putText(image,
                    "x: {}, y: {}".format(face_x, face_y),
                    (face_x, face_y),
                    cv2.FONT_HERSHEY_SIMPLEX,
                    0.5,
                    max_size_color,
                    1,
                    cv2.LINE_AA)

        diff_x = face_x - window_center[0]
        diff_y = face_y - window_center[1]

        now_pulses[0] = clamp(
            int(now_pulses[0] + k_p * diff_x), SERVO_MIN, SERVO_MAX)
        now_pulses[1] = clamp(
            int(now_pulses[1] + k_p * diff_y), SERVO_MIN, SERVO_MAX)
        for i, p in enumerate(now_pulses):
            pwm.set_pwm(i, 0, p)

    end = time.time()
    fps = 1 / (end - start)
    start = end
    cv2.putText(image,
                "servo0: {}, servo1: {}, fps: {:.2f}".format(now_pulses[0], now_pulses[1], fps),
                (20, 30),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.5,
                (0, 255, 255),
                1,
                cv2.LINE_AA)

    # ウィンドウに表示
    cv2.imshow('frame', image)

# 終了処理
cam.release()
cv2.destroyAllWindows()