aruco板_关于opencv中的aruco内外参标定(python3)

pip install opencv-contrib-python

import numpy as np
np.set_printoptions(suppress=True)
import time
import cv2
import cv2.aruco as aruco
import glob
import math

def calibrateKd(im_fpath, aruco_len=60.0):
    (w, h) = (6,4)
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01)
    objp = np.zeros((w*h,3), np.float32)
    objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
    objp = objp*aruco_len
    objpoints,imgpoints = [],[]
    images = glob.glob(f'{im_fpath}/*')
    for fname in images:
        img = cv2.imread(fname)
        print('r %s'%(fname),end='')
        gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, (w,h),None)
        if ret == True:
            objpoints.append(objp)
            corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
            imgpoints.append(corners2)
            img = cv2.drawChessboardCorners(img, (6,4), corners2, ret)
            
    ret,mtx,dist,rvecs,tvecs = cv2.calibrateCamera(objpoints,imgpoints,gray.shape[::-1],None,None)
    tot_error = 0
    for i in range(len(objpoints)):
        imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
        error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
        tot_error += error
    print ('total error: ', tot_error/len(objpoints))
    return mtx, dist

def calibrateRt(im, mtx=None, dist=None, aruco_len=0.204):
    if mtx is None:
        mtx = np.array([[2341.17, 0, 1895.73],
                        [0, 2339.87, 1105.74],
                        [0,       0, 1]])
    if dist is None:
        dist = np.array([[0.22391371,-0.58384357,-0.00010736,0.00021993,0.18890625]])
    gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
    aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
    parameters = aruco.DetectorParameters_create()
    corners,ids,rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters)
    rvec,tvec,_ = aruco.estimatePoseSingleMarkers(corners, aruco_len, mtx, dist)
    if rvec.shape[0]!=1:
        print('please remove other aruco!!!')
        return None
    R,t = cv2.Rodrigues(rvec[0])[0],tvec[0][0]
#     R,t = rvec,tvec[0][0]
    return R,t