저번 글에 이어서 포스팅을 하겠다.
드디어 프로젝트를 완성하였다!
아래 그림은 MCU가 어떻게 연결 되어있는지에 대한 그림들과 비디오이다.
Finally, We made this robot completely!
Following image is about how MCU was connected. And video.
<MCU>
<BB Soft Air Gun>
<Final Demonstration>
<Team members>
결과는 과제, 팀내 개별점수, 팀점수 등을 종합하여 등수로는 1등, A+ 학점을 받았다.
We made this robot in 3 months. We can learned about python language and openCV by ourselves. We helped by foreigners through a Google. Image processing part is very useful part for us. Because our major is Mechanical Information Engineering. This project was very good chance for experience about this part.
As a result, I got a first place in ranking and I got A+. (Including individual scores between team members, and Team scores etc..)
아래는 우리의 코드와 참고문헌들이다.
Following is our codes and references.
<Code>
<Raspberry Pi B2- python>
# This code was based on rosebrock's code.
# http://www.computervisiononline.com/blog/tutorial-using-camshift-track-objects-video
# And we used simondlevy's code.
# https://github.com/simondlevy/OpenCV-Python-Hacks/tree/master/Kalman2D
#import the necessary packages
import serial
import string
import numpy as np
import argparse
import cv2
import time
import picamera
import picamera.array
from datetime import datetime
from datetime import time
from datetime import timedelta
from PIL import Image
from sys import exit
from kalman2d import Kalman2D
#initialize the current frame of the video
#and define global variables
frame = None
roiPts = []
inputMode = False
Width = 480
Height = 480
wl = Width*4.5/10
wr = Width*5.5/10
ht = Height*4.5/10
hb = Height*5.5/10
targetBox = np.array([[wl,ht], [wr,ht], [wr,hb], [wl,hb]])
ser = serial.Serial('/dev/ttyAMA0', 9600, timeout=1)
ser.open()
#Center Information
class CenterInfo(object):
def __init__(self): #Init center information
self.x, self.y = -1, -1
def __str__(self): #for print center information
return '%4d %4d' % (self.x, self.y)
#Select RoiBox points from mouse click
def selectROI(event, x, y, flags, param):
global frame, roiPts, inputMode
if inputMode and event == cv2.EVENT_LBUTTONDOWN and len(roiPts) < 4:
roiPts.append((x,y))
cv2.circle(frame,(x,y),4,(0,255,0),2)
cv2.imshow("frame",frame)
#main
def main():
global frame, roiPts, inputMode, roiBoxWidth, roiBoxHeight
cnt = 0 #count for new roiBox from kalmanfilter
centerX = 0
centerY = 0
toggle = True #toggle for imshow
flag = False #flag for moving
kalman2d = Kalman2D() #Create new Kalman filter and initailize
cv2.namedWindow("frame") #window name
cv2.setMouseCallback("frame",selectROI) #mouseCallback
termination = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT,10,3) #camshift termination condition
roiBox = None
time2 = datetime.now() #<<<check time>>>
serial_flag_bit = 1 #Sending data flag
delta = timedelta(seconds = 1) #sleep time for init RoiBox
#Using capture_continuous by defining with 'with'
with picamera.PiCamera() as camera:
stream = picamera.array.PiRGBArray(camera)
camera.resolution = (Width,Height) #set resolution
time3 = datetime.now()
try:
for foo in enumerate(camera.capture_continuous(stream,'bgr',use_video_port = True)): #capture from capture_continuous
time1 = datetime.now() #<<<check time>>>
print(time1 - time2) #period time of 1 cycle of loop
time2 = time1 #<<<check time>>>
frame = stream.array #save image array to variable
stream.seek(0) #initialize stream for next iteration
stream.truncate()
if roiBox is not None:# define roiBox
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) #change image color RGB to HSV
backProj = cv2.calcBackProject([hsv],[0],roiHist, [0,180], 1) #make backprojection
if roiBoxWidth > 0 and roiBoxHeight > 0: #Check camshift is failed
(r,roiBox) = cv2.CamShift(backProj, roiBox, termination) #Do camshift
#r is set of 4points about roiBox
#roiBox is an array, consist of x and y of topLeft, and box's width and height
roiBoxWidth = roiBox[2]
roiBoxHeight = roiBox[3]
serial_flag_bit = 1
else :
#Init roiBox when camshift was failed
print "roiBox init!!!!!!!!!!!!!!!!"
tl[0] = 1
tl[1] = 1
br[0] = Width -1
br[1] = Height -1
roiBox = (tl[0], tl[1], br[0], br[1])
#Camshift
(r,roiBox) = cv2.CamShift(backProj, roiBox, termination)
roiBoxWidth = roiBox[2]
roiBoxHeight = roiBox[3]
serial_flag_bit = 0
time3 = datetime.now()
#transform r to pts to draw Box with polylines
pts = np.int0(cv2.cv.BoxPoints(r))
#Draw roiBox
cv2.polylines(frame,[pts],True, (0,255,0),2)
#Calculate center x,y
centerX = (pts[0][0] + pts[2][0])/2
centerY = (pts[0][1] + pts[2][1])/2
#Update x,y to kalman filter and get estimated x,y
CenterInfo.x = centerX
CenterInfo.y = centerY
#Send center x,y to Arduino
if CenterInfo.x / 10 == 0:
tempCenterX = '00' + str(CenterInfo.x)
elif CenterInfo.x / 100 == 0:
tempCenterX = '0' + str(CenterInfo.x)
else:
tempCenterX = str(CenterInfo.x)
if CenterInfo.y / 10 == 0:
tempCenterY = '00' + str(CenterInfo.y)
elif CenterInfo.y / 100 == 0:
tempCenterY = '0' + str(CenterInfo.y)
else:
tempCenterY = str(CenterInfo.y)
centerData = str(int(flag)) + tempCenterX + tempCenterY
print centerData
#check the time and flag to send information to Arduino. (Data is not sent until delta seconds)
if datetime.now() > time3 + delta :
if serial_flag_bit == 1:
ser.write('%s' %centerData)
#Update Kalman
kalman2d.update(CenterInfo.x, CenterInfo.y)
estimated = [int (c) for c in kalman2d.getEstimate()]
estimatedX = estimated[0]
estimatedY = estimated[1]
#Calculate delta x,y
deltaX = estimatedX - centerX
deltaY = estimatedY - centerY
#Apply new roiBox from kalmanfilter
if cnt > 1:
roiBox = (roiBox[0]+deltaX, roiBox[1]+deltaY, br[0], br[1])
cnt = 0
#Draw estimated center x,y from kalman filter for test
#cv2.circle(frame,(estimatedX,estimatedY), 4, (0, 255, 255),2)
#Change a color when target is in target box
if wl < centerX and wr > centerX and centerY < hb and centerY > ht :
cv2.circle(frame,(centerX,centerY), 4, (255,0,0),2)
flag = False
else :
cv2.circle(frame,(centerX,centerY), 4, (0,255,0),2)
flag = True
cnt = cnt+1 #count for apply new box from kalman filter
#Draw kalman top left point for test
#cv2.circle(frame,(roiBox[0],roiBox[1]), 4, (0,0,255),2)
#Draw target box
cv2.circle(frame,(Width/2,Height/2) , 4, (255,255,255),2)
cv2.polylines(frame, np.int32([targetBox]), 1, (255,255,255))
#Show or unshow imshow
if toggle is True:
cv2.imshow("frame",frame) #if you want speed up, delete this line
key=cv2.waitKey(1) & 0xFF
#Init roiBox
if key == ord("i") and len(roiPts) < 4:
inputMode = True
orig = frame.copy()
#wait for 4th click
while len(roiPts) < 4:
cv2.imshow("frame",frame)
cv2.waitKey(0)
#set data from 4 clicks
roiPts = np.array(roiPts)
s = roiPts.sum(axis=1)
tl = roiPts[np.argmin(s)]
br = roiPts[np.argmax(s)]
#make color histogram from roi
roi = orig[tl[1]:br[1], tl[0]:br[0]]
roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
roiHist = cv2.calcHist([roi],[0],None,[16],[0,180])
roiHist = cv2.normalize(roiHist, roiHist,0,255,cv2.NORM_MINMAX)
roiBox = (tl[0], tl[1], br[0], br[1])
#Set roiBox width, height
roiBoxWidth = roiBox[2]
roiBoxHeight = roiBox[3]
#Calculate center x,y
CenterInfo.x = tl[0]+br[0]/2
CenterInfo.y = tl[1]+br[1]/2
#Init x,y to kalman filter and set first roiBox
CenterInfo.x = centerX
CenterInfo.y = centerY
kalman2d.update(CenterInfo.x, CenterInfo.y)
estimated = [int (c) for c in kalman2d.getEstimate()]
estimatedX = estimated[0]
estimatedY = estimated[1]
#Calculate delta x,y
deltaX = estimatedX - centerX
deltaY = estimatedY - centerY
#set first roiBox
roiBox = (tl[0]+deltaX, tl[1]+deltaY, br[0], br[1])
#toggle for imshow
elif key == ord("r"):
toggle = not toggle
#quit
elif key == ord("q"):
break
finally:
cv2.destroyAllWindows()
if __name__== "__main__":
main()
| cs |
<Arduino Due - c>
#include <Servo.h>
int servo_tilt_pin= 9; //tilt servo pin
int dirpin = 28; //pan direction
int steppin = 26; //pan pwm
int laser_pin_1 = 6;
int laser_pin_2 = 7;
int cx = 240; //center x
int cy = 240; //center y
int tilt_gain=25; //gain
int tilt_angle; //angle
int err = 0; //pan err
int count = 0; //pan count
Servo servo_tilt; //tilt motor
void setup() {
analogWriteResolution(12); //Set up PWM resolution
pinMode(laser_pin_1, OUTPUT); //Set up lazer pin
pinMode(laser_pin_2, OUTPUT); //Set up lazer pin
pinMode(dirpin, OUTPUT); //Set up Stepping motor Dir pin
pinMode(steppin, OUTPUT); //Set up Stepping motor pin
servo_tilt.attach(servo_tilt_pin); //setup tilt servo
Serial2.begin(9600); //Serial (Arduino & RaspberryPi)
}
void loop() {
int tmp, flag, x, y;
tilt_angle=113; //init tilt angle : 30 degrees.
servo_tilt.write(tilt_angle);
analogWrite(laser_pin_1, 2048); // Turn on the Laser_1(50%).
analogWrite(laser_pin_2, 2048); // Turn on the Laser_2(50%).
while(1)
{
if(Serial2.available()>0)
{
char buffer[7]= "";
Serial2.readBytes(buffer,7); //Get center data from Raspberry Pi
tmp = atoi(buffer); //string to int
flag = tmp/1000000; //Get flag data
if(flag == 1) //if flag is 1, move braket.
{
x = tmp/1000 - flag*1000; //Get x data
y = tmp - (flag*1000000) - (x*1000); // Get y data
tilt( y); //tilt control
pan( x); //pan control
}
}
}
}
void tilt (int y) //move tilt_angle.
{
if((cy-y) > 0) //object is upper than center y.
{
if(tilt_angle>52) //if tilt_angle < 90 degrees.
{
tilt_angle -= (cy-y)/tilt_gain; // increase tilt_angle.
servo_tilt.write(tilt_angle);
}
}
if((cy-y) < 0) //object is lower than center y.
{
if(tilt_angle<144) //if tilt_angle > 0 degrees.
{
tilt_angle += (y-cy)/tilt_gain; // decrease tilt_angle.
servo_tilt.write(tilt_angle);
}
}
}
void pan (int x)
{
int tmp, i, angle ;
tmp = x-cx; //delta x
if( tmp < 0 ) //If target is left.
{
digitalWrite(dirpin, HIGH);// Set direction.CCW
angle = (int)(60*((float)(-tmp)/240));
if(angle - err < 0) //error handling
angle =err;
if(count > -400) //check pan angle limit (-90 degree)
{
count = count - (angle-err);
for (i = 0; i< angle-err; i++) //move stepping motor
{
digitalWrite(steppin, LOW);
digitalWrite(steppin, HIGH);
delayMicroseconds(1000);
}
err = (int)(0.1*(angle+err)); //Update error
}
}
else if( tmp > 0 ) //If target is right.
{
digitalWrite(dirpin, LOW); // Set the direction. CW
angle = (int)(60*((float)(tmp)/240));
if(angle - err < 0) //error handling
angle = err;
if (count < 400) //check pan angle limit (90 degree)
{
count = count + (angle-err);
for (i = 0; i< angle-err; i++) //move stepping motor
{
digitalWrite(steppin, LOW);
digitalWrite(steppin, HIGH);
delayMicroseconds(1000);
}
err = (int)(0.1*(angle + err)); //Update error
}
}
}
| cs |
<Code link>
You can also download our codes below.
You have to install PIL and openCV before using our code.
https://drive.google.com/folderview?id=0B50lLiCDqflbfnRpQWhkQmtaZEloMWlpdVZRc0JzYVA4UHBmWFpSU2dudlFGX3BPSnlyRWM&usp=sharing
install PIL ( python imaging library )
sudo apt-get install python-imaging
You can also download our codes below.
You have to install PIL and openCV before using our code.
https://drive.google.com/folderview?id=0B50lLiCDqflbfnRpQWhkQmtaZEloMWlpdVZRc0JzYVA4UHBmWFpSU2dudlFGX3BPSnlyRWM&usp=sharing
install PIL ( python imaging library )
sudo apt-get install python-imaging
<Reference>
<참고 논문> :
● “CAMshift 기법과 칼만 필터를 결합한 객체 추적 시스템” : 김대영, 박재완, 이칠우 – 멀티미디어학회 논문지 제16권 제 5호(2013.5)
→이 논문을 통하여 Camshift 알고리즘과 Kalman filter를 결합하는 방법에 대해 참고하였다.
<오픈소스> :
●OpenCV의 Camshift를 이용해 python 언어로 구성한 오픈소스코드
→이 코드를 토대로 구성하였으나 Kalman filter를 적용하고 예외처리를 추가하였고, fps의 병목현상의 원인을 찾아내어 capture_continuous를 사용하여 성능을 개선하였고 Arduino에게 데이터를 보내는 부분을 추가하였다.
●Kalman 2D mouse tracking python 코드 소스
→x,y 좌표만을 이용하는 kalman filter 코드였고 python을 이용한 코드였기 때문에 우리 코드에 활용하기 용이했다. 이 코드를 우리의 코드에 import시킨 후 x, y 좌표를 인자로 주어서 kalman filter 기능을 사용하였다.
<참고 사이트> :
●Kalman filter 이론 참고 자료
→Kalman filter에 대한 이해를 위해 참고하였다.
●OpenCV 설치 방법
→OpenCV를 Raspberry Pi에 설치하는 방법을 참고하였다.
●OpenCV documentation 페이지
→OpenCV 함수에 대한 내용을 참고하였다.
●Pi Camera documentation 페이지
댓글 없음:
댓글 쓰기