#はじめに
機械学習により学習したレーザポインタの輝点をXavierNXを用いて自動追尾するカメラをつくりました。
前回まででレーザポインタの輝点の位置は取得できたので、ステッピングモータのコントロールと合わせ、最終的に輝点を自動追尾します。
一連のエントリはこちら
1，USB-IOのコントロール
2，USB-IO経由でステッピングモータのコントロール
3，レーザポインタを物体検出するための学習データの作成
3.5. Google Colabを使って学習
4，レーザポインタを追いかけるカメラ −−いまここ
#やったこと
2，USB-IO経由でステッピングモータのコントロールでステッピングモータを2個コントロールし、カメラの方向制御ができているので、これと3，レーザポインタを物体検出するための学習データの作成で学習したデータを合わせ、レーザポインタの輝点を追いかけるようにしました。
#スクリプト
Darknetのサンプルのdarknet_video.pyにモータ制御をくっつけただけです。
dvl.py
from ctypes import *
import math
import random
import os
import cv2
import numpy as np
import time
import darknet
import StepMotor
import threading
StepMotorY = StepMotor.C28BYJ48(IN1=0 ,IN2=1, IN3=2, IN4=3)
StepMotorX = StepMotor.C28BYJ48(IN1=4, IN2=5, IN3=6, IN4=7)
def convertBack(x, y, w, h):
xmin = int(round(x - (w / 2)))
xmax = int(round(x + (w / 2)))
ymin = int(round(y - (h / 2)))
ymax = int(round(y + (h / 2)))
return xmin, ymin, xmax, ymax
def cvDrawBoxes(detections, img):
global StepMotorX , StepMotorY
found=0
for detection in detections:
x, y, w, h = detection[2][0],\
detection[2][1],\
detection[2][2],\
detection[2][3]
xmin, ymin, xmax, ymax = convertBack(
float(x), float(y), float(w), float(h))
pt1 = (xmin, ymin)
pt2 = (xmax, ymax)
cv2.rectangle(img, pt1, pt2, (0, 255, 0), 1)
cv2.putText(img,
detection[0].decode() +
" [" + str(round(detection[1] * 100, 2)) + "]",
(pt1[0], pt1[1] - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
[0, 255, 0], 2)
if ( detection[0]==b'laser' ) :
print( x , y )
if x > 304+20 :
StepMotorX.ThreadStep( -5 , 0.001)
if x > 304+60 :
StepMotorX.ThreadStep( -20 , 0.001)
if x < 304-20 :
StepMotorX.ThreadStep( +5 , 0.001)
if x < 304-60 :
StepMotorX.ThreadStep( +20 , 0.001)
if y > 304+20 :
StepMotorY.ThreadStep( +5 , 0.001)
if y > 304+60 :
StepMotorY.ThreadStep( +20 , 0.001)
if y < 304-20 :
StepMotorY.ThreadStep( -5 , 0.001)
if y < 304-60 :
StepMotorY.ThreadStep( -20 , 0.001)
found=found+1
return img,found
netMain = None
metaMain = None
altNames = None
def YOLO():
global metaMain, netMain, altNames
global StepMotorX , StepMotorY
configPath = "./yolov3-voc.cfg"
weightPath = "./yolov3-voc_2000.weights"
metaPath = "./voc.data"
if not os.path.exists(configPath):
raise ValueError("Invalid config path `" +
os.path.abspath(configPath)+"`")
if not os.path.exists(weightPath):
raise ValueError("Invalid weight path `" +
os.path.abspath(weightPath)+"`")
if not os.path.exists(metaPath):
raise ValueError("Invalid data file path `" +
os.path.abspath(metaPath)+"`")
if netMain is None:
netMain = darknet.load_net_custom(configPath.encode(
"ascii"), weightPath.encode("ascii"), 0, 1) # batch size = 1
if metaMain is None:
metaMain = darknet.load_meta(metaPath.encode("ascii"))
if altNames is None:
try:
with open(metaPath) as metaFH:
metaContents = metaFH.read()
import re
match = re.search("names *= *(.*)$", metaContents,
re.IGNORECASE | re.MULTILINE)
if match:
result = match.group(1)
else:
result = None
try:
if os.path.exists(result):
with open(result) as namesFH:
namesList = namesFH.read().strip().split("\n")
altNames = [x.strip() for x in namesList]
except TypeError:
pass
except Exception:
pass
src = 'v4l2src device= /dev/video0 ! image/jpeg,width=1280,height=720 !jpegdec !videoconvert ! appsink'
StepMotorX.ThreadWait()
StepMotorY.ThreadWait()
cap = cv2.VideoCapture(src)
# cap = cv2.VideoCapture("output.avi")
# cap.set(3, 1280)
# cap.set(4, 720)
out = cv2.VideoWriter(
"laser.mp4", cv2.VideoWriter_fourcc(*"mp4v"), 10.0,
(darknet.network_width(netMain), darknet.network_height(netMain)))
print("Starting the YOLO loop...")
# Create an image we reuse for each detect
darknet_image = darknet.make_image(darknet.network_width(netMain),
darknet.network_height(netMain),3)
fpsgt=0
frameTotal=0
foundTotal=0
while True:
prev_time = time.time()
# ret, frame_read = cap.read()
ret, frame_read = cap.read()
ret, frame_read = cap.read()
frame_rgb = cv2.cvtColor(frame_read, cv2.COLOR_BGR2RGB)
frame_resized = cv2.resize(frame_rgb,
(darknet.network_width(netMain),
darknet.network_height(netMain)),
interpolation=cv2.INTER_LINEAR)
darknet.copy_image_from_bytes(darknet_image,frame_resized.tobytes())
detections = darknet.detect_image(netMain, metaMain, darknet_image, thresh=0.70)
image ,found= cvDrawBoxes(detections, frame_resized)
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
fps = 1/(time.time()-prev_time)
fpsgt = fpsgt + fps
frameTotal=frameTotal+1
foundTotal=foundTotal+found
# print( "AVG FPS="+str(fpsgt / frameTotal) + " FRAME="+str(frameTotal)+" BIRD="+str(foundTotal))
cv2.imshow('Demo', image)
out.write( image )
cv2.waitKey(3)
cap.release()
out.release()
if __name__ == "__main__":
YOLO()
StepMotor.py
import time
import hid
import threading
class USBIO:
VENDOR_ID = 0x1352 # Km2Net
PRODUCT_ID = 0x0121 # USB-IO2.0(AKI)
lock = threading.Lock()
# Command list
CMD_READ_WRITE = 0x20
CMD_CONFIG_READ = 0xf8
CMD_CONFIG_WRITE = 0xf9
usb = hid.device(VENDOR_ID, PRODUCT_ID)
def __init__(self ):
#writePin( 0,0 )
self.pin = [0] *12
def writePin(self,p1,p2): # Prioro to use this , all pins need to be configured as output pins
data = [int(0)] * 64
data[0] = self.CMD_READ_WRITE
data[1] = 1
data[2] = p1
data[3] = 2
data[4] = p2
data[63] =89 # dummy to confirm USB-IO recognition
self.lock.acquire() # make sure just a thread using USB
self.usb.write(data)
self.lock.release()
return(self.usb.read(64))
def setPinLevel(self, pin_no , level ):
self.pin[pin_no] = level
def outputToPin( self ):
p1 = self.pin[0] + self.pin[1] * 2 + self.pin[2] * 4 + self.pin[3] *8
p1 = p1 + self.pin[4]*16 + self.pin[5] * 32+ self.pin[6] * 64+ self.pin[7] *128
p2 = self.pin[8] + self.pin[9] * 2 + self.pin[10]* 4 + self.pin[11]*8
self.writePin( p1 , p2 )
UsbIO=USBIO( ) # To use USBIO as a kind of singleton
class C28BYJ48():
def __init__(self, IN1, IN2, IN3, IN4):
self.mPin = [IN1, IN2, IN3, IN4] # USBIO pin number 4 of 0-12
self.stepThread=None
#Setting related Sequence
#1step angle = 1/4096[deg]
self.nPos = 0
self.mSeq = [[1,0,0,1],[1,0,0,0],[1,1,0,0],[0,1,0,0],[0,1,1,0],[0,0,1,0],[0,0,1,1],[0,0,0,1]]
#default speed = max speed
self.SetWaitTime(0.001)
def SetPinsVoltage(self, nSeq):
for pin in range(0, 4):
if self.mSeq[nSeq][pin]!=0:
UsbIO.setPinLevel( self.mPin[pin] , 1 )
else:
UsbIO.setPinLevel( self.mPin[pin] , 0 )
def SetWaitTime(self, wait):
self.mStep_wait = wait
def Step(self, step, wait):
self.SetWaitTime(wait)
for i in range(0, abs(step) ):
if( step > 0 ):
self.nPos += 1
else :
self.nPos -= 1
self.SetPinsVoltage(self.nPos % 8)
UsbIO.outputToPin()
time.sleep(self.mStep_wait)
# Entry point to control the stepping motor.
def ThreadStep( self , step , wait ):
if self.stepThread!=None:
# When motor is moving , it wait the end of moving before creating thread.
self.stepThread.join()
self.stepThread = threading.Thread( target=self.Step, args=(step,wait ) )
self.stepThread.start()
# This function was added from last entry
def ThreadWait(self ):
if self.stepThread!=None:
# When motor is moving , it wait the end of moving before creating thread.
self.stepThread.join()
def Cleanup(self):
if self.stepThread!=None:
# When motor is moving , it wait the end of moving before creating thread.
self.stepThread.join()
for pin in range(0, 4):
UsbIO.setPinLevel( self.mPin[pin] , 0 )
UsbIO.outputToPin()
if __name__ == '__main__':
StepMoterX = C28BYJ48(IN1=0 ,IN2=1, IN3=2, IN4=3)
StepMoterX.ThreadStep(-300,0.001)
Sleep(5)
StepMoterX.Cleanup()
#動作
動かして、レーザポインタをおっかけてる姿が以下。。
部屋が写るのを最小にするのに、いまいち面白いビデオではないですが。。。
白い箱の中身はUSB-IOとモータドライバ
JetsonXavierNXはUSBで大分はなれたところにおいてあります。
JetsonNanoだと、Yolov3だと、現状の簡単なクローズドループでの実装だと遅すぎて発振しちゃうかもしれません。
カメラがみているのは以下の感じです。
