import numpy as np import cv2 import glob import sys import argparse import serial import time #with help from: #http://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_calib3d/py_calibration/py_calibration.html #http://codeplasma.com/2012/12/03/getting-webcam-images-with-python-and-opencv-2-for-real-this-time/ def capture(args): cap = cv2.VideoCapture(args.port) while(True): ret, frame = cap.read() res = cv2.resize(frame,None,fx=.5, fy=.5, interpolation = cv2.INTER_CUBIC) cv2.imshow('frame',res) if cv2.waitKey(1) == ord('s'): cv2.imwrite("calib/test_image.jpg", frame) break cap.release() cv2.destroyAllWindows() def calibrate(args): criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) objp = np.zeros((args.m*args.n,3), np.float32) objp[:,:2] = np.mgrid[0:args.m,0:args.n].T.reshape(-1,2) objpoints = [] # 3d point in real world space imgpoints = [] # 2d points in image plane. images = glob.glob('calib/test_image*.jpg') for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, (args.m,args.n),None) print fname,ret if ret == True: objpoints.append(objp) cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) imgpoints.append(corners) cv2.drawChessboardCorners(img, (args.m,args.n), corners,ret) img = cv2.resize(img,None,fx=.5, fy=.5, interpolation = cv2.INTER_CUBIC) cv2.imshow('img',img) cv2.imwrite("corners-"+fname.split('-')[1].split('.')[0]+".jpg", img) cv2.waitKey(5000) ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None) img = cv2.imread(images[0]) h,w = img.shape[:2] #newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h)) # undistort #mapx,mapy = cv2.initUndistortRectifyMap(mtx,dist,None,newcameramtx,(w,h),5) mapx,mapy = cv2.initUndistortRectifyMap(mtx,dist,None,mtx,(w,h),5) dst = cv2.remap(img,mapx,mapy,cv2.INTER_LINEAR) # crop the image #x,y,w,h = roi #dst = dst[y:y+h, x:x+w] cv2.imwrite('calib/undistorted.jpg',dst) dst = cv2.resize(dst,None,fx=.5, fy=.5, interpolation = cv2.INTER_CUBIC) cv2.imshow('img',dst) cv2.waitKey(5000) cv2.destroyAllWindows() def process_frame(res,new): alpha = 1./args.nfs res = cv2.addWeighted(res,1-alpha,new,alpha,0) res[...,0]=0 rect = (200,100,550,500) gray = cv2.cvtColor(res,cv2.COLOR_BGR2GRAY) gray = cv2.medianBlur(gray,5) ret,thresh1 = cv2.threshold(gray,100,255,cv2.THRESH_BINARY) mask = np.zeros(thresh1.shape,np.uint8) mask[rect[1]:rect[3],rect[0]:rect[2]] = thresh1[rect[1]:rect[3],rect[0]:rect[2]] #cv2.rectangle(thresh1, (rect[0],rect[1]), (rect[2],rect[3]), (255,0,0)) contours,heirarchy = cv2.findContours(mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) areas = [cv2.contourArea(c) for c in contours] max_index = np.argmax(areas) cnt=contours[max_index] c,d,th = cv2.minAreaRect(cnt) mar = cv2.minAreaRect(cnt) box = cv2.cv.BoxPoints(mar) box = np.int0(box) cv2.drawContours(new,[box],0,(0,0,255),1) width = 9.8/216.*mar[1][np.argmax([abs(mar[2]),90-abs(mar[2])])] #take rotation into account. cv2.putText(new,"%.2f mm"%width, (rect[1]+50,(rect[1]+rect[3])/2), cv2.FONT_HERSHEY_SIMPLEX, 2, (0,0,255),2) #cv2.drawContours(new, contours, -1, (0,255,0), 3) return res,new,thresh1,width def send_command(ser,val): #print 'test:01,%d'%val ser.write('01,%d\n'%val) def clamp_int(x,minval,maxval): return int(max(min(maxval, x), minval)) class PID(object): def __init__(self,args): self.kp = args.kp self.ki = args.ki self.kd = args.kd self.cmin = args.cmin self.cmax = args.cmax self.last_error = 0 self.last_time = time.time() self.d_error = 0 self.i_error = 0 self.pwm = 0 def update(self,error): now = time.time() dt = now - self.last_time self.d_error = (error-self.last_error)/dt self.i_error += error*dt output = self.kp * error + self.ki * self.i_error + self.kd * self.d_error self.pwm = clamp_int(output,-90,90)+140 self.last_time = now self.last_error = error def run(args): ser = serial.Serial(args.mport, 9600) start_time = time.time() cap = cv2.VideoCapture(args.port) cap.set(5,25) #frame rate cap.set(3,800) #width, max=1600? cap.set(4,600) #height, max=1200? for i in range(5): ret, res = cap.read() #flush garbage width_d = args.wd pid = PID(args) data = {'widths':[],'pwms':[]} frame_n = 0 while(True): ret, new = cap.read() res,new,thresh1,width = process_frame(res,new) now = time.time() if now-start_time > args.dt: pid.update(width_d-width) send_command(ser,pid.pwm) data['widths'].append(width) data['pwms'].append(pid.pwm) start_time = now cv2.putText(new,"%d"%pid.pwm, (100,100), cv2.FONT_HERSHEY_SIMPLEX, 2, (0,255,0),2) cv2.imshow('frame',new) if frame_n%50 == 0: cv2.imwrite('frames/frame%04d.jpg'%(frame_n/50),new) frame_n += 1 if cv2.waitKey(1) == ord('q'): cv2.imwrite("calib/run_image.jpg", res) break cap.release() cv2.destroyAllWindows() np.savez('ziploc-control-data',widths=data['widths'],pwms=data['pwms']) if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('-M','--mode',choices=('capture','calibrate','run')) parser.add_argument("-p","--port",type=int,default=1,help="port of camera") parser.add_argument("-mp","--mport",help="port of microcontroller") parser.add_argument("-n","--n",type=int,default=7,help="columns in checkerboard") parser.add_argument("-m","--m",type=int,default=3,help="rows in checkerboard") parser.add_argument("-nfs","--nfs",type=int,default=10,help="number of frames to smooth for measurement") parser.add_argument("-dt","--dt",type=float,default=.02,help="seconds between serial command send") parser.add_argument("-wd","--wd",type=float,default=10.,help="initial set point for width") parser.add_argument("-kp","--kp",type=float,default=0.1,help="proportional gain") parser.add_argument("-ki","--ki",type=float,default=0.,help="integral gain") parser.add_argument("-kd","--kd",type=float,default=0.,help="derivative gain") parser.add_argument("-cmin","--cmin",type=int,default=50,help="min value for controller output") parser.add_argument("-cmax","--cmax",type=int,default=255,help="max value for controller output") args = parser.parse_args() if args.mode=='capture': capture(args) elif args.mode=='calibrate': calibrate(args) elif args.mode=='run': run(args)