顔との距離を検出する

PaPeRo iの顔検出機能を使ってPaPeRo iの首を顔に追従して動かすことが出来ますが、同時に顔までの距離も分かるので、それを利用する方法について説明します。

顔検出イベントについて

顔検出のdetectFaceイベントでは以下のデータが送られてきます。

検出した左目のピクセル座標
検出した右目のピクセル座標
検出に用いた画像のピクセルサイズ
検出時点のPaPeRo iの首の向き(角度)

顔追従のためには目の中間に向かせるように、

　(右目の座標 + 左目の座標) / 2

を使って動かすべき首の向きを計算しますが、

　(右目の座標 – 左目の座標)

によって、顔までの距離を知ることができます。
PaPeRo iの顔検出機能は傾いている顔は検出しづらいので簡単のためx座標のみで説明すると、

　(右目のx座標 – 左目のx座標) / 640(ピクセル数) * 画角(x)

がほぼカメラから見た左右の目の角度になり、

　目の間隔 / math.tan(左右の目の角度)

がほぼ目までの距離になるはずです。

サンプルプログラム

顔追従しつつ、(目の間隔が6.1cmの)人が50cm以内に寄ると胸が点滅して「こんにちは」と発話するサンプルです。
これを実際に動作させると、境界は計算値より10%程短くなるようで実動作に合わせて補正する必要がありますが、いずれにせよ目の間隔は人によってばらつきがありますので正確な距離は判別できず、ざっくり「近く」に寄ったことが分かれば良いのであれば使える方法です。

import sys
import math
from logging import (getLogger, Formatter, debug, info, warn, error, critical,
                     DEBUG, INFO, WARN, ERROR, CRITICAL, basicConfig)

import pypapero

logger = getLogger(__name__)

AOV = (54.5, 42.3)  # 画角
PIX = (640, 480)  # 画素数
HEAD_RANGE_X = (-128, 128)
HEAD_RANGE_Y = (-20, 55)

EYE_DISTANCE = 6.1


def limit_val(v, range_lower_upper):
    lower, upper = range_lower_upper
    if v < lower:
        return lower
    if v > upper:
        return upper
    return v


def main():
    simulator_id, robot_name, ws_server_addr = pypapero.get_params_from_commandline(sys.argv)
    papero = pypapero.Papero(simulator_id, robot_name, ws_server_addr)
    if papero.errOccurred != 0:
        return
    # papero.send_start_speech("顔との距離を利用するデモ")
    papero.send_start_face_detection("30")
    papero.send_turn_led_off("chest")
    papero.send_turn_led_on("forehead", ["Y3", "1", ], repeat=True)
    close = False
    while True:
        msgs = papero.papero_robot_message_recv(1.0)
        if msgs is None:
            continue
        if 0 == len(msgs):
            continue
        msg0 = msgs[0]
        nm = msg0.get("Name")
        if nm == "detectFace":
            # 顔追従
            papero.send_turn_led_on("forehead", ["G3", "1", ], repeat=True)
            left_eye_pos = pypapero.get_numstr_list_from_coord(msg0.get("LeftEyePos"))
            left_x, left_y = int(left_eye_pos[0]), int(left_eye_pos[1])
            right_eye_pos = pypapero.get_numstr_list_from_coord(msg0.get("RightEyePos"))
            right_x, right_y = int(right_eye_pos[0]), int(right_eye_pos[1])
            head_pos = pypapero.get_numstr_list_from_coord(msg0.get("HeadPos"))
            head_x, head_y = int(head_pos[1]), int(head_pos[0])
            mid_x = (left_x + right_x) / 2
            mid_y = (left_y + right_y) / 2
            to_x = head_x + int((PIX[0] / 2 - mid_x) * AOV[0] / PIX[0])
            to_y = head_y + int((PIX[1] / 2 - mid_y) * AOV[1] / PIX[1])
            to_x = limit_val(to_x, HEAD_RANGE_X)
            to_y = limit_val(to_y, HEAD_RANGE_Y)
            seq_h = ["A" + str(to_x) + "S30L"]
            seq_v = ["A" + str(to_y) + "S30L"]
            papero.send_move_head(seq_v, seq_h)
            # 顔との距離を計算
            dif = left_x - right_x
            deg = dif / PIX[0] * AOV[0]
            dist = (EYE_DISTANCE / 2) / math.tan(deg/2/180*math.pi)
            logger.info("dif={} distance={}".format(dif, dist))
            if 50 >= dist:
                # 距離が50cm以下
                if not close:
                    logger.info("in")
                    close = True
                    papero.send_turn_led_on("chest", ["RGB3", "5", "N", "5", ], repeat=True)
                    papero.send_start_speech("こんにちは")
            else:
                if close:
                    logger.info("out")
                    close = False
                    papero.send_turn_led_off("chest")
        elif nm == "undetectFace":
            papero.send_turn_led_on("forehead", ["Y3", "1", ], repeat=True)
            if close:
                logger.info("lost")
                close = False
                papero.send_turn_led_off("chest")
        elif nm == "detectButton":
            status = msg0["Status"]
            if status == "C":
                papero.send_turn_led_off("forehead")
                papero.send_turn_led_off("chest")
                break
    papero.papero_cleanup()


if __name__ == "__main__":
    basicConfig(level=INFO, format='%(asctime)s %(levelname)s %(name)s %(funcName)s %(message)s')
    main()

Share on Tumblr