mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
31 lines
1.2 KiB
Python
31 lines
1.2 KiB
Python
import numpy as np
|
|
import cv2
|
|
import cv2.aruco as aruco
|
|
import glob
|
|
|
|
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 *= aruco_len
|
|
objpoints, imgpoints = [], []
|
|
images = glob.glob(f'{im_fpath}/*')
|
|
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:
|
|
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.shap[::-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(tot_error)
|
|
return mtx, dist
|