import cv2
import numpy as np
import glob

# 找棋盤(pán)格角點(diǎn)
# 閾值
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
#棋盤(pán)格模板規(guī)格
w = 7 ? #內(nèi)角點(diǎn)個(gè)數(shù)，內(nèi)角點(diǎn)是和其他格子連著的點(diǎn)
h = 7

# 世界坐標(biāo)系中的棋盤(pán)格點(diǎn),例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐標(biāo)，記為二維矩陣
objp = np.zeros((w*h,3), np.float32)
objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
# 儲(chǔ)存棋盤(pán)格角點(diǎn)的世界坐標(biāo)和圖像坐標(biāo)對(duì)
objpoints = [] # 在世界坐標(biāo)系中的三維點(diǎn)
imgpoints = [] # 在圖像平面的二維點(diǎn)

images = glob.glob('picture/*.jpg')
for fname in images:
? ? img = cv2.imread(fname)
? ? gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
? ? # 找到棋盤(pán)格角點(diǎn)
? ? # 棋盤(pán)圖像(8位灰度或彩色圖像) ?棋盤(pán)尺寸 ?存放角點(diǎn)的位置
? ? ret, corners = cv2.findChessboardCorners(gray, (w,h),None)
? ? # 如果找到足夠點(diǎn)對(duì)，將其存儲(chǔ)起來(lái)
? ? if ret == True:
? ? ? ? # 角點(diǎn)精確檢測(cè)
? ? ? ? # 輸入圖像 角點(diǎn)初始坐標(biāo) 搜索窗口為2*winsize+1 死區(qū) 求角點(diǎn)的迭代終止條件
? ? ? ? cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
? ? ? ? objpoints.append(objp)
? ? ? ? imgpoints.append(corners)
? ? ? ? # 將角點(diǎn)在圖像上顯示
? ? ? ? cv2.drawChessboardCorners(img, (w,h), corners, ret)
? ? ? ? cv2.imshow('findCorners',img)
? ? ? ? cv2.waitKey(1000)
cv2.destroyAllWindows()
#標(biāo)定、去畸變
# 輸入：世界坐標(biāo)系里的位置 像素坐標(biāo) 圖像的像素尺寸大小 3*3矩陣，相機(jī)內(nèi)參數(shù)矩陣 畸變矩陣
# 輸出：標(biāo)定結(jié)果 相機(jī)的內(nèi)參數(shù)矩陣 畸變系數(shù) 旋轉(zhuǎn)矩陣 平移向量
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
# mtx：內(nèi)參數(shù)矩陣
# dist：畸變系數(shù)
# rvecs：旋轉(zhuǎn)向量 （外參數(shù)）
# tvecs ：平移向量 （外參數(shù)）
print (("ret:"),ret)
print (("mtx:\n"),mtx) ? ? ? ?# 內(nèi)參數(shù)矩陣
print (("dist:\n"),dist) ? ? ?# 畸變系數(shù) ? distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
print (("rvecs:\n"),rvecs) ? ?# 旋轉(zhuǎn)向量 ?# 外參數(shù)
print (("tvecs:\n"),tvecs) ? ?# 平移向量 ?# 外參數(shù)
# 去畸變
img2 = cv2.imread('picture/6.jpg')
h,w = img2.shape[:2]
# 我們已經(jīng)得到了相機(jī)內(nèi)參和畸變系數(shù)，在將圖像去畸變之前，
# 我們還可以使用cv.getOptimalNewCameraMatrix()優(yōu)化內(nèi)參數(shù)和畸變系數(shù)，
# 通過(guò)設(shè)定自由自由比例因子alpha。當(dāng)alpha設(shè)為0的時(shí)候，
# 將會(huì)返回一個(gè)剪裁過(guò)的將去畸變后不想要的像素去掉的內(nèi)參數(shù)和畸變系數(shù)；
# 當(dāng)alpha設(shè)為1的時(shí)候，將會(huì)返回一個(gè)包含額外黑色像素點(diǎn)的內(nèi)參數(shù)和畸變系數(shù)，并返回一個(gè)ROI用于將其剪裁掉
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),0,(w,h)) # 自由比例參數(shù)

dst = cv2.undistort(img2, mtx, dist, None, newcameramtx)
# 根據(jù)前面ROI區(qū)域裁剪圖片
x,y,w,h = roi
dst = dst[y:y+h, x:x+w]
cv2.imwrite('calibresult.jpg',dst)

# 反投影誤差
# 通過(guò)反投影誤差，我們可以來(lái)評(píng)估結(jié)果的好壞。越接近0，說(shuō)明結(jié)果越理想。
# 通過(guò)之前計(jì)算的內(nèi)參數(shù)矩陣、畸變系數(shù)、旋轉(zhuǎn)矩陣和平移向量，使用cv2.projectPoints()計(jì)算三維點(diǎn)到二維圖像的投影，
# 然后計(jì)算反投影得到的點(diǎn)與圖像上檢測(cè)到的點(diǎn)的誤差，最后計(jì)算一個(gè)對(duì)于所有標(biāo)定圖像的平均誤差，這個(gè)值就是反投影誤差。
total_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)
? ? total_error += error
print (("total error: "), total_error/len(objpoints))