MeanShiftを使って、カメラマウントでカメラがタチコマの方を向きながら追いかけることを実現します。
プログラムで実現することを決める
前回のプログラムに2つのサーボモーターを制御するプログラムを埋め込み、カメラが物体の方を見るようにします。
カメラの制御とArduinoの制御の両方をRaspberry Piがやります。
ArduinoがRaspberry Piからの指示を受け取るプログラムは、
Raspberry PiとArduinoを連携させるのservo_motor.inoを使います。
2つのサーボモーターを制御するプログラムを設計する
追いかけている物体が左右の中央から右にある場合は、下側のサーボモーターの角度を1度減らします。
追いかけている物体が左右の中央から左にある場合は、下側のサーボモーターの角度を1度増やします。
追いかけている物体が上下の中央から上にある場合は、上側のサーボモーターの角度を1度減らします。
追いかけている物体が上下の中央から下にある場合は、上側のサーボモーターの角度を1度増やします。
上記は、カメラマウントを組み立てるときのサーボモーターの向きで決まります。
カメラマウントを組み立てる
上記を繰り返すことで、カメラが物体を追いかけることを実現します。
下側のサーボモーターは、30度より小さい角度、150度より大きい角度とはならないようにします。
上側のサーボモーターは、60度より小さい角度、120度より大きい角度とはならないようにします。
カメラが、サーボモーターに対して重くて大きいため動き過ぎないように制限します。
但し、上記だけでは、カメラは物体を追いかけているときはいいのですが、物体が正面にあるときに小刻みに震える動作となってしまうため、閾値を設ける必要があります。
プログラムを実装する
ソースコードを掲載します。
meanshift_servo_motor.py
# -*- coding: utf-8 -*-
#! /usr/bin/python
#! /usr/bin/env python
# [USAGE]
#
# python meanshift_servo_motor.py
# ------------------------------------------------------------------------------
## Meanshift
## use servomotor
# ------------------------------------------------------------------------------
# pylint: disable=C0103
# ------------------------------------------------------------------------------
import os
import time
## library for array
import numpy as np
## OpenCV 3.2
import cv2 as open_cv
import serial
# ------------------------------------------------------------------------------
SLEEP_TIME = 0.0
WIDTH = 640
HEIGHT = 480
IMAGE_CENTER_X = WIDTH/2
IMAGE_CENTER_Y = HEIGHT/2
ROI_WIDTH = 240
ROI_HEIGHT = 240
ROI_X_START = int((WIDTH - ROI_WIDTH) / 2)
ROI_Y_START = int((HEIGHT - ROI_HEIGHT) / 2)
THRESHOLD = 100
MOVE_DEGREE = 1
# ------------------------------------------------------------------------------
## usb connect -> wait arduino boot
# S = serial.Serial('/dev/ttyACM0', 57600)
# S = serial.Serial('/dev/ttyACM0', 115200)
if os.path.exists('/dev/ttyACM0'):
S = serial.Serial('/dev/ttyACM0', 9600)
print('/dev/ttyACM0')
if os.path.exists('/dev/ttyACM1'):
S = serial.Serial('/dev/ttyACM1', 9600)
print('/dev/ttyACM1')
time.sleep(2)
# ------------------------------------------------------------------------------
## wake up camera mount
x_str = '%dx' % (90)
print('Raspberry Pi -> Arduino: ', x_str)
## y : upper servo motor
S.write(x_str.encode('utf-8'))
# print('Raspberry Pi <- Arduino: ', S.readline().decode('utf-8'))
y_str = '%dy' % (90)
print('Raspberry Pi -> Arduino: ', y_str)
## y : upper servo motor
S.write(y_str.encode('utf-8'))
# print('Raspberry Pi <- Arduino: ', S.readline().decode('utf-8'))
# ------------------------------------------------------------------------------
## create instance
video_capture = open_cv.VideoCapture(0)
if video_capture.isOpened():
print('video capture: 0')
else:
video_capture = open_cv.VideoCapture(1)
if video_capture.isOpened():
print('video capture: 1')
else:
print('can not get video capture')
quit()
# ------------------------------------------------------------------------------
## get a frame
while True:
ret, frame = video_capture.read()
if ret:
print("GET VIDEO CAPTURE")
break
else:
print("NOT GET VIDEO CAPTURE")
# ------------------------------------------------------------------------------
## Width
video_capture.set(open_cv.CAP_PROP_FRAME_WIDTH, WIDTH)
# ------------------------------------------------------------------------------
## Height
video_capture.set(open_cv.CAP_PROP_FRAME_HEIGHT, HEIGHT)
# ------------------------------------------------------------------------------
## setup ROI for tracking
## setup initial location of window
## C525 (depend on specification) -> 1280 x 720
## OpenCV (depend on setting) -> 640 x 480
## ROI (depend on setting) -> 240 x 240
# 640
# <----------------------->
# (0,0) x = 200
# -------------------------- ^
# | | |
# | w = 240 | |
# y = 120 | ------- | |
# | | | h = 240 | | 480
# | ------- | |
# | | |
# | | |
# -------------------------- v
x, y, w, h = ROI_X_START, ROI_Y_START, ROI_WIDTH, ROI_HEIGHT
track_window = (x, y, w, h)
print('x, y, w, h: ', x, y, w, h)
## Region of Interest / Region of Image
## from y to y+h
## from x to x+w
roi = frame[y:y+h, x:x+w]
# ------------------------------------------------------------------------------
## ignore dark pixel
## BGR (Blue Green Red) -> HSV (Hue Saturation Value)
## Value == Brightness
hsv_roi = open_cv.cvtColor(roi, open_cv.COLOR_BGR2HSV)
## open_cv.inRange( hsv_roi, np.array( Hue Min, Saturation Min, Value Min ), np.array( Hue Max, Saturation Max, Value Max ) )
img_mask = open_cv.inRange(hsv_roi, np.array((0., 60., 32.)), np.array((180., 255., 255.)))
# ------------------------------------------------------------------------------
## histgram
roi_hist = open_cv.calcHist([hsv_roi], [0], img_mask, [180], [0, 180])
open_cv.normalize(roi_hist, roi_hist, 0, 255, open_cv.NORM_MINMAX)
# ------------------------------------------------------------------------------
## make criteria for mean shift
## type : open_cv.TERM_CRITERIA_EPS or open_cv.TERM_CRITERIA_COUNT
## max_iter : 10
## epsilon / accuracy : 1.0
termination_criteria = (open_cv.TERM_CRITERIA_EPS | open_cv.TERM_CRITERIA_COUNT, 10, 1)
# ------------------------------------------------------------------------------
now_degree_x, now_degree_y, move_degree_x, move_degree_y = 90, 90, 0, 0
while True:
## input q -> quit
k = open_cv.waitKey(1)
if k == ord('q'):
S.write('q'.encode('utf-8'))
time.sleep(2)
break
## get a frame
ret, frame = video_capture.read()
if ret:
## BGR (Blue Green Red) -> HSV (Hue Saturation Value) ## Value == Brightness
hsv = open_cv.cvtColor(frame, open_cv.COLOR_BGR2HSV)
## open_cv.calcBackProject( images, channels, mask, hist_size, ranges )
dst = open_cv.calcBackProject([hsv], [0], roi_hist, [0, 180], 1)
## get the position
ret, track_window = open_cv.meanShift(dst, track_window, termination_criteria)
x, y, w, h = track_window
print('x, y, w, h: ', x, y, w, h)
if ret:
print('meanshift success')
## write rectangle
## color : Scalar(255,255,255) == white
## thickness : thickness of lines : 3
img_dst = open_cv.rectangle(frame, (x, y), (x+w, y+h), (255, 255, 255), 3)
## get center of meanshift
roi_center_x = int(x + (ROI_WIDTH/2))
roi_center_y = int(y + (ROI_HEIGHT/2))
print('roi_center_x, roi_center_y: ', roi_center_x, roi_center_y)
print('IMAGE_CENTER_X, IMAGE_CENTER_Y: ', IMAGE_CENTER_X, IMAGE_CENTER_Y)
## write small circle to roi center
## image
## center
## radius : diameter / 2
## color
## thickness : negative value -> filled
open_cv.circle(img_dst, (roi_center_x, roi_center_y), 10, (0, 180, 0), -1)
open_cv.imshow('SHOW MEANSHIFT IMAGE', img_dst)
# ------------------------------------------------------------------------------
## move_degree_x : +1(+2) or -1(-2)
## move_degree_y : +1(+2) or -1(-2)
## now_degree_x : 30 - 150
## now_degree_y : 60 - 120
move_degree_x = 0;
move_degree_y = 0;
if THRESHOLD < roi_center_x - IMAGE_CENTER_X:
move_degree_x = -MOVE_DEGREE;
if THRESHOLD < IMAGE_CENTER_X - roi_center_x:
move_degree_x = MOVE_DEGREE;
if THRESHOLD < roi_center_y - IMAGE_CENTER_Y:
move_degree_y = MOVE_DEGREE;
if THRESHOLD < IMAGE_CENTER_Y - roi_center_y:
move_degree_y = -MOVE_DEGREE;
now_degree_x = now_degree_x + move_degree_x
now_degree_y = now_degree_y + move_degree_y
if now_degree_x < 30:
now_degree_x = 30
if 150 < now_degree_x:
now_degree_x = 150
if now_degree_y < 60:
now_degree_y = 60
if 120 < now_degree_y:
now_degree_y = 120
print("move degree: ", move_degree_x, move_degree_y)
print("now degree: ", now_degree_x, now_degree_y)
## send value to Arduino
x_str = '%dx' % (now_degree_x)
print('Raspberry Pi -> Arduino: ', x_str)
S.write(x_str.encode('utf-8'))
# print('Raspberry Pi <- Arduino: ', S.readline().decode('utf-8'))
y_str = '%dy' % (now_degree_y)
print('Raspberry Pi -> Arduino: ', y_str)
S.write(y_str.encode('utf-8'))
# print('Raspberry Pi <- Arduino: ', S.readline().decode('utf-8'))
time.sleep(SLEEP_TIME)
# ------------------------------------------------------------------------------
else:
break
video_capture.release()
open_cv.destroyAllWindows()
x_str = '%dx' % (90)
S.write(x_str.encode('utf-8'))
y_str = '%dy' % (90)
S.write(y_str.encode('utf-8'))
動作確認する
カメラマウントが動いて、タチコマを追いかけます。
Webカメラから見た様子です。
参考
Raspberry Pi サーボモーターとOpen CVで物体追跡カメラ（Meanshift）
謝辞
ロゴをダウンロードして使わせていただきました。ありがとうございます。
Twitter