import cv2

 import numpy as np

 import glob

 import matplotlib.pyplot as plt

 import matplotlib.patches as patches

 # 找棋盘格角点

 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # 阈值

 #棋盘格模板规格

 w = 9   # 10 - 1

 h = 6   # 7  - 1

 # 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵

 objp = np.zeros((w*h,3), np.float32)

 objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)

 objp = objp*18.1  # 18.1 mm

 # 储存棋盘格角点的世界坐标和图像坐标对

 objpoints = [] # 在世界坐标系中的三维点

 imgpoints = [] # 在图像平面的二维点

 images = glob.glob('./chessboard/*.jpg')  #   拍摄的十几张棋盘图片所在目录

 i = 1

 for fname in images:

     img = cv2.imread(fname)

     gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

     # 找到棋盘格角点

     ret, corners = cv2.findChessboardCorners(gray, (w,h),None)

     # 如果找到足够点对，将其存储起来

     if ret == True:

         print("i:", i)

         i = i+1

         cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)

         objpoints.append(objp)

         imgpoints.append(corners)

         # 将角点在图像上显示

         cv2.drawChessboardCorners(img, (w,h), corners, ret)

         cv2.namedWindow('findCorners', cv2.WINDOW_NORMAL)

         cv2.resizeWindow('findCorners', 810, 405)

         cv2.imshow('findCorners',img)

         cv2.waitKey(1)

 cv2.destroyAllWindows()

 #%% 标定

 ret, mtx, dist, rvecs, tvecs = \

     cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

 print("ret:",ret  )

 print("mtx:\n",mtx)      # 内参数矩阵

 print("dist:\n",dist   )   # 畸变系数   distortion cofficients = (k_1,k_2,p_1,p_2,k_3)

 print("rvecs:\n",rvecs)   # 旋转向量  # 外参数

 print("tvecs:\n",tvecs  )  # 平移向量  # 外参数

 import numpy as np

 import time

 import cv2

 import cv2.aruco as aruco  

 #with np.load('webcam_calibration_output.npz') as X:

 #    mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]

 #mtx =

 #2946.48    0    1980.53

 #0    2945.41    1129.25

 #0    0    1

 mtx = np.array([

         [2946.48,       0, 1980.53],

         [      0, 2945.41, 1129.25],

         [      0,       0,       1],

         ])

 #我的手机拍棋盘的时候图片大小是 4000 x 2250

 #ip摄像头拍视频的时候设置的是 1920 x 1080，长宽比是一样的，

 #ip摄像头设置分辨率的时候注意一下

 dist = np.array( [0.226317, -1.21478, 0.00170689, -0.000334551, 1.9892] )

 video = "http://admin:admin@192.168.1.2:8081/"   # 手机ip摄像头

 # 根据ip摄像头在你手机上生成的ip地址更改，右上角可修改图像分辨率

 cap = cv2.VideoCapture(video)

 font = cv2.FONT_HERSHEY_SIMPLEX #font for displaying text (below)  

 #num = 0

 while True:

     ret, frame = cap.read()

     # operations on the frame come here  

     gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

     aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)

     parameters =  aruco.DetectorParameters_create()  

     '''

     detectMarkers(...)

         detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedI

         mgPoints]]]]) -> corners, ids, rejectedImgPoints

     '''  

     #lists of ids and the corners beloning to each id

     corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,

                                                           aruco_dict,

                                                           parameters=parameters)  

 #    if ids != None:

     if ids is not None:

         rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, 0.05, mtx, dist)

         # Estimate pose of each marker and return the values rvet and tvec---different

         # from camera coeficcients

         (rvec-tvec).any() # get rid of that nasty numpy value array error  

 #        aruco.drawAxis(frame, mtx, dist, rvec, tvec, 0.1) #Draw Axis

 #        aruco.drawDetectedMarkers(frame, corners) #Draw A square around the markers

         for i in range(rvec.shape[0]):

             aruco.drawAxis(frame, mtx, dist, rvec[i, :, :], tvec[i, :, :], 0.03)

             aruco.drawDetectedMarkers(frame, corners)

         ###### DRAW ID #####

 #        cv2.putText(frame, "Id: " + str(ids), (0,64), font, 1, (0,255,0),2,cv2.LINE_AA)  

     else:

         ##### DRAW "NO IDS" #####

         cv2.putText(frame, "No Ids", (0,64), font, 1, (0,255,0),2,cv2.LINE_AA)  

     # Display the resulting frame

     cv2.imshow("frame",frame)  

     key = cv2.waitKey(1)

     if key == 27:         # 按esc键退出

         print('esc break...')

         cap.release()

         cv2.destroyAllWindows()

         break

     if key == ord(' '):   # 按空格键保存

 #        num = num + 1

 #        filename = "frames_%s.jpg" % num  # 保存一张图像

         filename = str(time.time())[:10] + ".jpg"

         cv2.imwrite(filename, frame)