python棋盘格画图代码_使用OpenCV进行姿态估计的绘图棋盘格参考系（solvePnP + projectPoints）...

importcv2ascvimportnumpyasnpimportglobimportargparse# algorithm parametersCHECKERBOARD_WIDTH=9CHECKERBOARD_HEIGHT=6# termination criteriacriteria=(cv.TERM_CRITERIA_EPS+cv.TERM_CRITERIA_MAX_ITER,30,0.001)#=== CALIBRATE CAMERA ============================================================================#Prepare object pointsobjp=np.zeros((CHECKERBOARD_HEIGHT*CHECKERBOARD_WIDTH,3),np.float32)objp[:,:2]=np.mgrid[0:CHECKERBOARD_HEIGHT,0:CHECKERBOARD_WIDTH].T.reshape(-1,2)# Arrays to store object points and image points from all the images.objpoints=[]# 3d point in real world spaceimgpoints=[]# 2d points in image plane.# Load the imagesap=argparse.ArgumentParser()ap.add_argument('-f','--folder',required=True,help='Path to the images folder with last slash')ap.add_argument('-e','--ext',required=True,help='Extension of image files without the dot')args=vars(ap.parse_args())images=glob.glob(args['folder']+'*.'+args['ext'])#Process the imagesforfnameinimages:print('Calibrating on '+fname)img=cv.imread(fname)gray=cv.cvtColor(img,cv.COLOR_BGR2GRAY)# Find the chess board cornersret,corners=cv.findChessboardCorners(gray,(CHECKERBOARD_WIDTH,CHECKERBOARD_HEIGHT),None)# If found, add object points, image points (after refining them)ifret==True:print('Found corners')objpoints.append(objp)corners2=cv.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)imgpoints.append(corners)# Draw and display the corners as feedback to the usercv.drawChessboardCorners(img,(CHECKERBOARD_WIDTH,CHECKERBOARD_HEIGHT),corners2,ret)cv.imshow('Calibration',img)k=cv.waitKey(0)&0xFFifk==ord('s'):cv.imwrite(fname+'_calib.png',img)cv.destroyAllWindows()#Obtain camera parametersret,mtx,dist,rvecs,tvecs=cv.calibrateCamera(objpoints,imgpoints,gray.shape[::-1],None,None)#=== FIND POSE OF TARGETS ===========================================================================#Prepare object pointsobjp=np.zeros((CHECKERBOARD_HEIGHT*CHECKERBOARD_WIDTH,3),np.float32)objp[:,:2]=np.mgrid[0:CHECKERBOARD_HEIGHT,0:CHECKERBOARD_WIDTH].T.reshape(-1,2)axis=np.float32([[3,0,0],[0,3,0],[0,0,-3]]).reshape(-1,3)#Displaydefdraw(img,corners,imgpts):corner=tuple(corners[0].ravel())img=cv.line(img,corner,tuple(imgpts[0].ravel()),(255,0,0),5)img=cv.line(img,corner,tuple(imgpts[1].ravel()),(0,255,0),5)img=cv.line(img,corner,tuple(imgpts[2].ravel()),(0,0,255),5)returnimgforfnameinimages:print('Processing '+fname)img=cv.imread(fname)gray=cv.cvtColor(img,cv.COLOR_BGR2GRAY)ret,corners=cv.findChessboardCorners(gray,(CHECKERBOARD_WIDTH,CHECKERBOARD_HEIGHT),None)ifret==True:print('Found corners')corners2=cv.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)# Find the rotation and translation vectors.ret,rvecs,tvecs=cv.solvePnP(objp,corners2,mtx,dist)# project 3D points to image planeimgpts,jac=cv.projectPoints(axis,rvecs,tvecs,mtx,dist)img=draw(img,corners2,imgpts)cv.imshow('img',img)k=cv.waitKey(0)&0xFFifk==ord('s'):cv.imwrite(fname+'_output.png',img)cv.destroyAllWindows()