def _exact_pose(gray, mtx, dist):
"""Return a pair (rotation, translation) vectors describing
what we think is the pose of the 3d model.
For the chessboard, this uses corner location and PNP matching
to get a high-accuracy pose.
"""
ret, corners = cv2.findChessboardCorners(gray, (5, 8), None)
if ret is True:
# refine corner locations
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30,
0.001)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
objp = np.zeros((5 * 8, 3), np.float32)
objp[:, :2] = np.mgrid[0:5, 0:8].T.reshape(-1, 2)
# Find the rotation and translation vectors.
_, rotation_vec, translation_vec, inliers = cv2.solvePnPRansac(
objp, corners2, mtx, dist)
return rotation_vec, translation_vec
else:
# If no frame is found, guess the pose
return np.array([0.1, 0.1, 0.1]), np.array([0.0, 0.0, 10.0])
def calibrate():
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
Nx_cor = 9
Ny_cor = 6
objp = np.zeros((Nx_cor * Ny_cor, 3), np.float32)
objp[:, :2] = np.mgrid[0:Nx_cor, 0:Ny_cor].T.reshape(-1, 2)
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
count = 0 # count 用来标志成功检测到的棋盘格画面数量
while (True):
ret, frame = cap.read()
if cv2.waitKey(1) & 0xFF == ord(' '):
# Our operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (Nx_cor, Ny_cor),
None) # Find the corners
# If found, add object points, image points
if ret == True:
corners = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
criteria)
objpoints.append(objp)
imgpoints.append(corners)
cv2.drawChessboardCorners(frame, (Nx_cor, Ny_cor), corners,
ret)
count += 1
if count > 20:
break
# Display the resulting frame
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
global mtx, dist
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
print(mtx, dist)
mean_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)
mean_error += error
print("total error: ", mean_error / len(objpoints))
# # When everything done, release the capture
np.savez('calibrate.npz', mtx=mtx, dist=dist[0:4])
def calibrateOne(img_path, cameraParam):
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)
#设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
#获取标定板角点的位置
objp = np.zeros((4 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:4].T.reshape(-1, 2) #将世界坐标系建在标定板上
obj_points = [] #3D points
img_points = [] #2D points
images = glob.glob(img_path)
i = 0
for fname in images:
img = cv2.imread(fname)
#print(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
h, w = gray.shape[:2]
ret, corners = cv2.findChessboardCorners(gray, (7, 4), None)
if ret:
obj_points.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
criteria)
#在原角点的基础上寻找亚像素点
#print(corners2)
if [corners2]:
img_points.append(corners2)
else:
img_points.append(corners)
cv2.drawChessboardCorners(img, (7, 4), corners, ret)
i += 1
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points,
size, None, None,
criteria)
#dist——>dist coffients(就是k、p的值)
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
mtx, dist, (w, h), 1, (w, h)) #显示更大范围的图片（正常重映射之后会删掉一部分图像）
#将fx，fy（世界坐标系的焦距）投射到fu，fv（图像坐标系的焦距）上，立体矫正时会用
#dst = cv2.undistort(img,mtx,dist,None,newcameramtx)
#x,y,w,h = roi
cameraParam['ret'] = ret
cameraParam['mtx'] = mtx
cameraParam['dist'] = dist
cameraParam['rvecs'] = rvecs
cameraParam['tvecs'] = tvecs
cameraParam['img_points'] = img_points
cameraParam['obj_points'] = obj_points
cameraParam['size'] = size
cameraParam['newmtx'] = newcameramtx
print("Calibrate One Camera Success")
def get_corners(img, param1=2, param2=3, param3=0.04):
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
gray = np.float32(gray)
dst = cv2.cornerHarris(gray, param1, param2, param3)
ret, dst = cv2.threshold(dst, 0.1 * dst.max(), 255, 0)
dst = np.uint8(dst)
ret, labels, stats, centroids = cv2.connectedComponentsWithStats(dst)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
corners = cv2.cornerSubPix(gray, np.float32(centroids), (5, 5), (-1, -1), criteria)
return corners
def augmentation3d(self):
plt.close('all')
with np.load('output.npz') as X:
mtx, dist, _, _ = [
X[i] for i in ('arr_0', 'arr_1', 'arr_2', 'arr_3')
]
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30,
0.001)
objp = np.zeros((11 * 8, 3), np.float32)
objp[:, :2] = np.mgrid[0:11, 0:8].T.reshape(-1, 2)
axis = np.float32([[1, 1, 0], [5, 1, 0], [3, 5, 0], [3, 3, -3]])
filepath = list()
for i in range(5):
path = os.path.join('Datasets/Q3_Image', str(i + 1) + '.bmp')
filepath.append(path)
ims = list()
fig = plt.figure('augumentation3d')
for fname in filepath:
img = cv.imread(fname)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
ret, corners = cv.findChessboardCorners(gray, (11, 8), None)
if ret == True:
corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
# Find the rotation and translation vectors.
_, rvecs, tvecs, _ = cv.solvePnPRansac(objp, corners2, mtx,
dist)
# project 3D points to image plane
imgpts, _ = cv.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = self.draw(img, corners2, imgpts)
plt_img = img[:, :, ::-1]
im = plt.imshow(plt_img, animated=True)
ims.append([im])
_ = animation.ArtistAnimation(fig,
ims,
interval=500,
blit=True,
repeat_delay=500)
plt.show()
def calculate_camera_matrix_and_distortion_coefficients(camera_model_name):
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(7,5,0)
object_points = np.zeros((6 * 8, 3), np.float32)
object_points[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
real_world_points = [] # 3d point in real world space
image_points = [] # 2d points in image plane.
image_paths = glob.glob("calibration_images/*.png")
for path_to_image in image_paths:
img = cv2.imread(path_to_image)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
chess_board_successfully_detected, corners = cv2.findChessboardCorners(
gray, (8, 6), None)
print("Found chess board in: " + path_to_image + " : " +
str(chess_board_successfully_detected))
# If found, add object points, image points (after refining them)
if chess_board_successfully_detected:
real_world_points.append(object_points)
refined_corners = cv2.cornerSubPix(gray, corners, (11, 11),
(-1, -1), criteria)
image_points.append(refined_corners)
# Draw and display the corners
# img = cv2.drawChessboardCorners(img, (7, 6), refined_corners, chess_board_successfully_detected)
# cv2.imshow('img', resize_image(img, (1920, 1080)))
# cv2.waitKey()
cv2.destroyAllWindows()
ret, camera_matrix, distortion_coeffs, rvecs, tvecs = cv2.calibrateCamera(
real_world_points, image_points, gray.shape[::-1], None, None)
h, w = gray.shape[:2]
camera_matrix_with_crop, roi = cv2.getOptimalNewCameraMatrix(
camera_matrix, distortion_coeffs, (w, h), 1, (w, h))
file_name = f"camera_calibration_{camera_model_name}.npz"
np.savez(file_name,
camera_matrix=camera_matrix,
distortion_coeffs=distortion_coeffs,
camera_matrix_with_crop=camera_matrix_with_crop)
def find_calibration_matrix(images):
# Prepare real world points
rwp = np.zeros((7 * 9, 3), np.float32)
rwp[:, :2] = np.mgrid[0:7, 0:9].T.reshape(-1, 2)
# Arrays to store 3D points in the real world and 2D points in image plane
rw_points = []
image_points = []
for image in images:
img = cv2.imread(image)
img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Take in a 8-bit grayscale image and the number of internal corners of chessboard and flags.
# CLAIB_CV_FAST_CHECK = check quickly for corners and end if none to save time
found, corners = cv2.findChessboardCorners(
img_gray, (7, 9), flags=cv2.CALIB_CB_FAST_CHECK
)
if found:
rw_points.append(rwp)
# Needs accuracy below pixel level
corners = cv2.cornerSubPix(
img_gray,
corners,
(11, 11),
(-1, -1),
(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001),
)
image_points.append(corners)
# Draws the corners on picture
# cv2.drawChessboardCorners(img, (7, 9), cornersnew, retval)
# cv2.imshow('img', img)
# cv2.waitKey()
else:
print("no corners found in " + image, file=sys.stderr)
cv2.destroyAllWindows()
_, cameramatrix, distortioncoefficients, _, _ = cv2.calibrateCamera(
rw_points, image_points, img_gray.shape[::-1], None, None
)
return cameramatrix, distortioncoefficients
def get_callibration_stuff():
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
cap = cv2.VideoCapture('pictures/ChessboardPattern/11.mp4')
nx, ny = 8, 6
objp = np.zeros((8 * 6, 3), np.float32)
objp[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2)
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
fps = get_fps(cap)
num_frames = get_frames(cap)
frames_with_corners = 0
def skip_frame(video):
ret2, frame2 = video.read()
if ret2:
curr_frame = video.get(cv2.CAP_PROP_POS_FRAMES)
if curr_frame <= num_frames:
cap.set(cv2.CAP_PROP_POS_FRAMES, curr_frame + 0.5 * fps)
return frame2
while (cap.isOpened()):
ret, frame = cap.read()
if ret == True:
frame = skip_frame(cap)
r_frame_bw = get_resized_and_bw(frame, 40)
found_corners, corners = cv2.findChessboardCorners(
r_frame_bw, (nx, ny), None)
if found_corners:
frames_with_corners += 1
objpoints.append(objp)
corners2 = cv2.cornerSubPix(r_frame_bw, corners, (11, 11),
(-1, -1), criteria)
imgpoints.append(corners2)
if frames_with_corners == 16:
break
cap.release()
cv2.destroyAllWindows()
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
r_frame_bw.shape[::-1],
None, None)
return ret, mtx, dist, rvecs, tvecs
def AxisAndBox():
# 读取相机参数
with open(path2+'//calibration_parameters.yaml') as file:
documents = yaml.full_load(file)
data = list(documents.values())
mtx = np.asarray(data[2])
#dist = np.asarray(data[1])
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((13 * 6, 3), np.float32)
objp[:, :2] = np.mgrid[0:13, 0:6].T.reshape(-1, 2)
axis = np.float32([[0, 0, 0], [0, 3, 0], [3, 3, 0], [3, 0, 0],
[0, 0, -3], [0, 3, -3], [3, 3, -3], [3, 0, -3]])
axis2 = np.float32([[4, 0, 0], [0, 4, 0], [0, 0, -4]]).reshape(-1, 3)
num = 0 # 计数器
images2 = glob.glob(path2+'//tianyi_gao_undistorted*.jpg')
for fname in images2:
num = num+1
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (13, 6), None)
if ret is True:
corners2 = cv2.cornerSubPix(
gray, corners, (11, 11), (-1, -1), criteria)
# 计算外参,估计相机位姿
_, rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, None)
# 将3-D点投影到像平面
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, None)
imgpts2, jac2 = cv2.projectPoints(axis2, rvecs, tvecs, mtx, None)
img = drawBox(img, corners2, imgpts)
img = drawAxis(img, corners2, imgpts2)
cv2.imwrite(path2+"//tianyi_gao_"+str(num)+".jpg", img)
if len(images2) < 16: # 图片过多时，不在UI中展示
cv2.namedWindow('press any key to continue', cv2.WINDOW_NORMAL)
cv2.imshow('press any key to continue', img)
cv2.waitKey(0)
print('Draw Done')
cv2.destroyAllWindows()
return num, path2#返回处理图片数和结果存储路径
def findcorners(self):
plt.close('all')
# termination criteria
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30,
0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((11 * 8, 3), np.float32)
objp[:, :2] = np.mgrid[0:11, 0:8].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
for i in range(1, 16):
img_name = str(i) + '.bmp'
fname = os.path.join('Datasets/Q2_Image', img_name)
img = cv.imread(fname)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv.findChessboardCorners(gray, (11, 8), None)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
imgpoints.append(corners)
# Draw and display the corners
cv.drawChessboardCorners(img, (11, 8), corners2, ret)
plt_img = img[:, :, ::-1]
plt.figure(fname)
plt.imshow(plt_img)
ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(
objpoints, imgpoints, gray.shape[::-1], None, None)
np.savez('output.npz', mtx, dist, rvecs, tvecs)
np.savez('img_obj_points.npz', imgpoints, objpoints)
plt.show()
for image_path in tqdm(calibration_paths):
#Load image
image = cv2.imread(image_path)
gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
print("Image loaded, Analyzing...")
#find chessboard corners
ret,corners = cv2.findChessboardCorners(gray_image, chessboard_size, None)
if ret == True:
print("Chessboard detected!")
print(image_path)
#define criteria for subpixel accuracy
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
#refine corner location (to subpixel accuracy) based on criteria.
cv2.cornerSubPix(gray_image, corners, (5,5), (-1,-1), criteria)
obj_points.append(objp)
img_points.append(corners)
#Calibrate camera
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points,gray_image.shape[::-1], None, None)
#Save parameters into numpy file
np.save("./camera_params/ret", ret)
np.save("./camera_params/K", K)
np.save("./camera_params/dist", dist)
np.save("./camera_params/rvecs", rvecs)
np.save("./camera_params/tvecs", tvecs)
#Get exif data in order to get focal length.
None) # Define the number of chees corners we are looking for
retL, cornersL = cv2.findChessboardCorners(ChessImaL,
(ChessWidth, ChessHeight),
None) # Left side
cv2.drawChessboardCorners(originR, (ChessWidth, ChessHeight), cornersR,
retR)
cv2.drawChessboardCorners(originL, (ChessWidth, ChessHeight), cornersL,
retL)
while True:
cv2.imshow('Corner', np.hstack((originL, originR)))
if (cv2.waitKey(1) & 0xFF == ord('n')):
print("Couple with " + t + " has been added")
if (True == retR) & (True == retL):
objpoints.append(objp)
cv2.cornerSubPix(ChessImaR, cornersR, (11, 11), (-1, -1),
criteria)
cv2.cornerSubPix(ChessImaL, cornersL, (11, 11), (-1, -1),
criteria)
imgpointsR.append(cornersR)
imgpointsL.append(cornersL)
break
elif (cv2.waitKey(2) & 0xFF == ord('q')):
break
cv2.destroyAllWindows()
# Determine the new values for different parameters
# Right Side
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
objpoints, imgpointsR, ChessImaR.shape[::-1], None, None)
hR, wR = ChessImaR.shape[:2]
OmtxR, roiR = cv2.getOptimalNewCameraMatrix(mtxR, distR, (wR, hR), 1,
(wR, hR))
for fname in images:
img = cv2.imread(fname)
# 获取画面中心点
#获取图像的长宽
h1, w1 = img.shape[0], img.shape[1]
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
u, v = img.shape[:2]
# 找到棋盘格角点
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', 640, 480)
cv2.imshow('findCorners',img)
cv2.waitKey(200)
cv2.destroyAllWindows()
#%% 标定
print('正在计算')
#标定
ret, mtx, dist, rvecs, tvecs = \
cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
def calibration(images):
# termination criteria
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((7 * 10, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:10].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
for fname in images:
img = cv.imread(fname)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv.findChessboardCorners(gray, (7, 10), None)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
imgpoints.append(corners)
# Draw and display the corners
cv.drawChessboardCorners(img, (7, 10), corners2, ret)
img_resized = cv.resize(img, (1400, 700))
cv.imshow('img', img_resized)
cv.waitKey(10)
cv.destroyAllWindows()
ret, K, dist, rvecs, tvecs, std_int, std_ext, pVE = \
cv.calibrateCameraExtended(objpoints, imgpoints, gray.shape[::-1], None, None)
mean_error = []
error_vecs = np.zeros(
(70 * len(images),
2)) # vertical stack of [x, y] errors for all points in all pictures
for i in range(len(objpoints)): # calculating errors
imgpoints2, _ = cv.projectPoints(objpoints[i], rvecs[i], tvecs[i], K,
dist)
error = cv.norm(imgpoints[i], imgpoints2, cv.NORM_L2) / len(imgpoints2)
mean_error.append(error)
imgpoints2 = np.array(imgpoints2)
imgpoints2 = imgpoints2[:, 0, :]
imgpoints1 = np.array(imgpoints[i])
imgpoints1 = imgpoints1[:, 0, :]
error_vecs[i * 70:(i + 1) * 70, :] = imgpoints1 - imgpoints2
fig = plt.figure(1) # mean reprojection error plot
img_nr = [f"{i+1}" for i in range(len(images))]
plt.bar(img_nr, mean_error)
plt.ylabel("mean reprojection error")
plt.xlabel("image number")
plt.savefig("Calibration_errors")
plt.show()
fig2 = plt.figure(2)
plt.scatter(error_vecs[:, 0], error_vecs[:, 1])
plt.ylabel("y error")
plt.xlabel("x error")
plt.savefig("Reprojection_scatter")
plt.show()
print('Standard errors')
print('Focal length and principal point')
print('--------------------------------')
print('fx: %g +/- %g' % (K[0, 0], std_int[0]))
print('fy: %g +/- %g' % (K[1, 1], std_int[1]))
print('cx: %g +/- %g' % (K[0, 2], std_int[2]))
print('cy: %g +/- %g' % (K[1, 2], std_int[3]))
print('Distortion coefficients')
print('--------------------------------')
print('k1: %g +/- %g' % (dist[0, 0], std_int[4]))
print('k2: %g +/- %g' % (dist[0, 1], std_int[5]))
print('p1: %g +/- %g' % (dist[0, 2], std_int[6]))
print('p2: %g +/- %g' % (dist[0, 3], std_int[7]))
print('k3: %g +/- %g' % (dist[0, 4], std_int[8]))
np.savetxt('cam_matrix.txt', K)
np.savetxt('dist.txt', dist)
np.savetxt('stdInt.txt', std_int)
np.savetxt('stdExt.txt', std_ext)
def calibrate():
#棋盘角点数col*row
col = 13
row = 6
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((row * col, 3), np.float32)
# 用于标定的棋盘每个方格边长为22mm
objp[:, :2] = 22*np.mgrid[0:col, 0:row].T.reshape(-1, 2)
objpoints = [] # 世界坐标系下的点坐标
imgpoints = [] # 像素平面坐标系下的点坐标
print("请选择标定用到的照片所在的文件夹", "\n")
root = tkinter.Tk()
root.withdraw()
global path # 用于标定的照片所在目录
path = tkinter.filedialog.askdirectory(
title="选择标定用到的照片所在的文件夹") # 选择标定用到的照片所在的文件夹
images = glob.glob(path+"/*.jpg")
found = 0 # 记录用于标定的图像数目
for k, fname in enumerate(images):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (col, row), None)
# 角点检测
if ret is True:
print("读取", fname)
objpoints.append(objp)
# 角点检测精度会影响标定的精度
corners2 = cv2.cornerSubPix(
gray, corners, (11, 11), (-1, -1), criteria)#亚像素角点位置
# corners2=corners
#_,corners2=cv2.find4QuadCornerSubpix(gray, corners, (11, 11))
imgpoints.append(corners2)
img = cv2.drawChessboardCorners(img, (col, row), corners2, ret)#标记角点
found += 1
if len(images) < 16: # 图片过多时，不在UI中展示，避免弹窗过多
cv2.namedWindow('press any key to continue', cv2.WINDOW_NORMAL)
cv2.imshow('press any key to continue', img)
cv2.waitKey(0)
#image_name = path2 + "//corner"+str(found) + '.png'
#cv2.imwrite(image_name, img)
#存储已标出角点的照片
global path2 # 存放结果的目录（含记录相机参数的文件，和畸变矫正后的照片，3-D box照片）
path2 = tkinter.filedialog.askdirectory(
title="选择结果存放的文件夹(应与用于标定的照片所在的文件夹不同)") # 选择结果存放的文件夹
print("Number of images used for calibration: ", found)
# 相机标定
ret2, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None, None)
print("reprojection error:", ret2)
print("内参矩阵:", mtx)
print("畸变系数：", dist)
print("旋转向量：", rvecs)
print("平移向量：", tvecs)
images = glob.glob(path+"//*.jpg")
for i, fname in enumerate(images):
img = cv2.imread(fname)
if img is None:
continue
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1,
(w, h))
dst = cv2.undistort(img, mtx, dist, None, newcameramtx) # 矫正畸变
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]#裁剪
outpath = path2+"//tianyi_gao_undistorted" + str(i + 1) + ".jpg"
cv2.imwrite(outpath, dst)
print("新内参矩阵：", newcameramtx)
data = {
'camera_matrix': np.asarray(mtx).tolist(),
'dist_coeff': np.asarray(dist).tolist(),
'new_camera_matrix': np.asarray(newcameramtx).tolist(),
'rvecs': np.asarray(rvecs).tolist(),
'tvecs': np.asarray(tvecs).tolist(),
'reprojection_error': np.asarray(ret2).tolist()
}
# 存储相机参数(yaml)
with open(path2+"//calibration_parameters.yaml", "w") as f:
yaml.dump(data, f)
# 存储相机参数(txt)
with open(path2+"//tianyi_gao_cam.txt", "w") as f2:
name = list(data.keys())
value = list(data.values())
for i in range(len(name)):
f2.write(name[i] + ":" + "\n" + str(value[i]) + "\n")
print('Calibrate Done')
cv2.destroyAllWindows()
return mtx, dist, rvecs, tvecs, ret2, path2
def main():
drone = tellopy.Tello()
drone.connect()
drone.wait_for_connection(60.0)
retry = 3
container = None
while container is None and 0 < retry:
retry -= 1
try:
container = av.open(drone.get_video_stream())
except av.AVError as ave:
print(ave)
print('retry...')
allCorners = []
allIds = []
decimator = 0
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100,
0.00001) # Subpixel corner detection criteria
capture_images = True
frame_skip = 300 # Skip first frames
while capture_images:
for frame in container.decode(video=0):
# If transforms are taking too long then start skipping frames
if frame_skip > 0:
frame_skip -= 1
continue
start_time = time.time()
image = cv2.cvtColor(np.array(frame.to_image()),
cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
image, aruco_dict)
if len(corners) > 0:
# SUB PIXEL DETECTION
for corner in corners:
cv2.cornerSubPix(image,
corner,
winSize=(3, 3),
zeroZone=(-1, -1),
criteria=criteria)
res2 = cv2.aruco.interpolateCornersCharuco(
corners, ids, image, board)
if res2[1] is not None and res2[2] is not None and len(
res2[1]) > 3 and decimator % 1 == 0:
allCorners.append(res2[1])
allIds.append(res2[2])
decimator += 1
# Key presses give the drone a speed, and not a distance to move. Press x to stop all movement
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
capture_images = False
break
# Display image
cv2.imshow('Drone', image)
# Determine if need to skip any frames
if frame.time_base < 1.0 / 60:
time_base = 1.0 / 60
else:
time_base = frame.time_base
frame_skip = int((time.time() - start_time) / time_base)
frame_skip += 30
print('.', )
# Done with calibration
drone.quit()
cv2.destroyAllWindows()
imsize = image.shape
ret, mtx, dist, rvecs, tvecs = calibrate_camera(allCorners, allIds, imsize)
print('ret', ret)
print('mtx', mtx)
print('dist', dist)
print('rvect', rvecs)
print('tvect', tvecs)
cv2.imshow('gray1', gray)
gray = cv2.threshold(gray, 90, 255, cv2.THRESH_BINARY)[1] # 设置过滤无关边界内容
gray = cv2.GaussianBlur(gray, (3, 3), 0) # 使图像模糊，便于减少无关图像干扰
gray = np.float32(gray)
dst = cv2.cornerHarris(gray, 2, 3, 0.04) # 找到Harris角点
dst = cv2.dilate(dst, None) # 膨胀操作
ret, dst = cv2.threshold(dst, 0.01 * dst.max(), 255, 0) # 二值化，除去不相关图像
dst = cv2.GaussianBlur(dst, (3, 3), 0) # 使图像模糊，便于减少无关图像干扰
dst = np.uint8(dst)
centroids = cv2.connectedComponentsWithStats(dst)[3] # 寻找连通区域
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001
) # 设置终止条件，迭代30次或移动0.001
corners = cv2.cornerSubPix(gray, np.float32(centroids), (5, 5), (-1, -1),
criteria) # cv2.cornerSubPix（）检测十分精细，但不便于显示
res = np.hstack((centroids, corners))
res = np.int0(res)
# cv2.cornerHarris检测出的角点，粗略
pointImg[dst > 0.1012 * dst.max()] = [0, 0, 255]
# cv2.cornerSubPix检测出的角点，精确
pointImg[res[:, 1], res[:, 0]] = [0, 0, 255]
# 绿色点为修正点
pointImg[res[:, 3], res[:, 2]] = [0, 255, 0]
cv2.imshow('dst2', pointImg)
cv2.waitKey(0)
cv2.destroyAllWindows()
har[dst>0.01*dst.max()]=[0,0,255]
cv2.imshow('har',har)
# Corner with SubPixel Accuracy
# Sometimes, you may need to find the corners with maximum accuracy. OpenCV comes with a function cv2.cornerSubPix() which further refines the corners detected with sub-pixel accuracy. Below is an example. As usual, we need to find the harris corners first. Then we pass the centroids of these corners (There may be a bunch of pixels at a corner, we take their centroid) to refine them. Harris corners are marked in red pixels and refined corners are marked in green pixels. For this function, we have to define the criteria when to stop the iteration. We stop it after a specified number of iteration or a certain accuracy is achieved, whichever occurs first. We also need to define the size of neighbourhood it would search for corners.
ret, dst = cv2.threshold(dst,0.01*dst.max(),255,0)
dst = np.uint8(dst)
# find centroids
ret, labels, stats, centroids = cv2.connectedComponentsWithStats(dst)
# define the criteria to stop and refine the corners
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
corners = cv2.cornerSubPix(gray,np.float32(centroids),(5,5),(-1,-1),criteria)
# Now draw them
res = np.hstack((centroids,corners))
res = np.uint8(res)
subpix = img.copy()
subpix[res[:,1],res[:,0]]=[0,0,255]
subpix[res[:,3],res[:,2]] = [0,255,0]
cv2.imshow('sub pix',subpix)
cv2.waitKey(0)
cv2.destroyAllWindows()
img_points = [] # 存储2D点
i = 0
for i in range(26):
image_name = "c-" + str(i + 1).zfill(4) # 读取图像，文件命名补齐函数zfill
img = cv2.imread(image_name + ".ppm")
print(image_name)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
# 提取角点
ret, corners = cv2.findChessboardCorners(gray, (8, 6), None)
# print("ret = ", ret)
# print(corners)
if ret:
obj_points.append(objp)
# 找寻亚像素角点
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
criteria) # 在原角点的基础上寻找亚像素角点
# print(corners2)
if [corners2]:
img_points.append(corners2)
else:
img_points.append(corners)
# 把角点画出来
cv2.drawChessboardCorners(img, (8, 6), corners,
ret) # 记住，OpenCV的绘制函数一般无返回值
# cv2.imwrite('c-'+str(0).zfill(4)+'.png', img) # 文件命名补齐函数zfill
# 保存绘制角度后的图像
# cv2.imwrite(image_name +".png", img)
# print(len(img_points))
# cv2.destroyAllWindows()
# 标定
return frame2
while (cap.isOpened()):
ret, frame = cap.read()
if ret == True:
frame = skip_frame(cap)
r_frame = cv2_helpers.get_resized(frame, 40)
r_frame_bw = cv2_helpers.get_resized_and_bw(frame, 40)
cv2.imshow("something", r_frame)
found_corners, corners = cv2.findChessboardCorners(
r_frame_bw, (nx, ny), None)
if found_corners:
frames_with_corners += 1
objpoints.append(objp)
corners2 = cv2.cornerSubPix(r_frame_bw, corners, (11, 11),
(-1, -1), criteria)
imgpoints.append(corners2)
new_img = cv2.drawChessboardCorners(r_frame, (nx, ny), corners,
ret)
cv2.imshow("test", new_img)
if frames_with_corners == 16:
break
cap.release()
cv2.destroyAllWindows()
print("callibrating the given video...")
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
r_frame_bw.shape[::-1],
None, None)
