ubuntu 21.10(arm64) + Raspberry Pi 4環境でmediapipe + サーボモータ + カメラを動かしてAIカメラを作る

December 19, 2021

Categories: ubuntu raspberrypi mediapipe

mediapipeで顔を認識して、顔が中心に写るように自動的に動くAIカメラっぽいものを作る。

事前準備

ソースコード

servocontroller.py

サーボ制御ライブラリ側。 faceservo.pyと同じディレクトリに入れておく。

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

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

import Adafruit_PCA9685

class ServoController:
  def __init__(self, horizontal_id=12, vertical_id=13):
    self.pwm = Adafruit_PCA9685.PCA9685()
    self.pwm.set_pwm_freq(50)
    self.horizontal_value = 0.0
    self.vertical_value   = 0.0
    self.update()

  def reset(self):
    self.horizontal_value = 0.0
    self.vertical_value = 0.0
    self.update()

  def control(self, x, y):
    x = (x - 0.5) * 2
    y = (y - 0.5) * 2

    if x > 0.1:
      self.horizontal_value -= 0.01
    elif x < -0.1:
      self.horizontal_value += 0.01
    if y > 0.1:
      self.vertical_value -= 0.01
    elif y < -0.1:
      self.vertical_value += 0.01
    self.horizontal_value = round(self.horizontal_value, 2)
    self.vertical_value = round(self.vertical_value, 2)
    print("x: {:.3f}, y: {:.3f}, horizontal: {:.3f}, vertical: {:.3f}".format(
      x, y, self.horizontal_value, self.vertical_value))
    self.update()
    

  def update(self):
    if self.horizontal_value >= 1.0:
      self.horizontal_value = 0.99
    elif self.horizontal_value <= -1.0:
      self.horizontal_value = -0.99
    if self.vertical_value >= 1.0:
      self.vertical_value = 0.99
    elif self.vertical_value <= -1.0:
      self.vertical_value = -0.99
    self.pwm.set_pwm(1, 0, self.convert_deg(self.horizontal_value * 90))
    self.pwm.set_pwm(0, 0, self.convert_deg(self.vertical_value * 90))

  def convert_deg(self, deg, freq=50):
    step = 4096
    max_pulse = 2.5
    min_pulse = 0.5
    center_pulse = (max_pulse - min_pulse) / 2 + min_pulse
    one_pulse = round((max_pulse - min_pulse) / 180, 2)
    deg_pulse = center_pulse + deg * one_pulse
    deg_num = int(deg_pulse / (1.0 / freq * 1000 / step))
    return deg_num

faceservo.py

本体。

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

#!/usr/bin/python3

import cv2
import mediapipe as mp
from time import sleep,time
from servocontroller import ServoController

mp_face_detection = mp.solutions.face_detection
mp_drawing = mp.solutions.drawing_utils

cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
cap.set(cv2.CAP_PROP_FPS, 60)
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV"))
print("{}x{}({}fps)".format(cap.get(cv2.CAP_PROP_FRAME_WIDTH),
                            cap.get(cv2.CAP_PROP_FRAME_HEIGHT),
                            cap.get(cv2.CAP_PROP_FPS)))
svc = ServoController()

with mp_face_detection.FaceDetection(
    model_selection=0, min_detection_confidence=0.5) as face_detection:
  while cap.isOpened():
    success, image = cap.read()
    if not success:
      print("Ignoring empty camera frame.")
      continue

    image.flags.writeable = False
    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    results = face_detection.process(image)
    if results.detections:
      detection = results.detections[0]
      # https://github.com/google/mediapipe/blob/e6c19885c6d3c6f410c730952aeed2852790d306/mediapipe/python/solutions/face_detection.py#L52
      # 0: 右目, 1: 左目, 2: 鼻, 3: 口, 4: 右耳, 5: 左耳
      # OpenCVのウィンドウ上だと左右が反転しているので注意
      # point = detection.location_data.relative_keypoints[2]
      point = mp_face_detection.get_key_point(
        detection, mp_face_detection.FaceKeyPoint.NOSE_TIP)
      svc.control(point.x, point.y)

cap.release()

後はfaceservo.pyを実行するだけ。

1
2
3
4
5
6
7
8
9
10
11

% ./faceservo.py
1280.0x720.0(60.0fps)
INFO: Created TensorFlow Lite XNNPACK delegate for CPU.
x: 0.088, y: -0.309, horizontal: 0.000, vertical: 0.010
x: 0.028, y: -0.294, horizontal: 0.000, vertical: 0.020
x: 0.089, y: -0.358, horizontal: 0.000, vertical: 0.030
x: 0.103, y: -0.460, horizontal: -0.010, vertical: 0.040
x: 0.186, y: -0.615, horizontal: -0.020, vertical: 0.050
x: 0.067, y: -0.534, horizontal: -0.020, vertical: 0.060
x: 0.089, y: -0.476, horizontal: -0.020, vertical: 0.070
x: 0.092, y: -0.410, horizontal: -0.020, vertical: 0.080