def undistort_img(img, K, distortion, dist_std, random_dist=False):
'''Undistorts and displays a given image'''
if random_dist:
dist = np.random.normal(distortion, dist_std)
else:
dist = distortion
h, w = img.shape[:2]
newK, roi = cv.getOptimalNewCameraMatrix(K, dist, (w, h), 1, (w, h))
#Undistort
undistorted = cv.undistort(img, K, dist, None, newK)
#Crop
x, y, w, h = roi
undistorted = undistorted[y:y + h, x:x + w]
# undist_resized = cv.resize(undistorted, (1440, 960))
#Display undistorted image
# cv.imshow('undistorted', undist_resized)
# cv.waitKey(-1)
# cv.destroyAllWindows()
return undistorted
def undistort(img, cal_dir='cal_pickle.p'):
with open(cal_dir, mode='rb') as f:
file = pickle.load(f)
mtx = file['mtx']
dist = file['dist']
dst = cv.undistort(img, mtx, dist, None, mtx)
return dst
def show_undistorted(cameras):
"""Display undistorted image(s)."""
cam_list = to_list(cameras)
stream_prefix = 'Camera'
stream_windows = initialize_windows(stream_prefix, cam_list)
focus_window(stream_windows[0])
keypress = -1
while keypress == -1:
for win, camera in zip(stream_windows, cam_list):
image = camera.stream.read()
dst = cv2.undistort(image, camera.cam_mat, camera.dist_coeff)
cv2.imshow(win, dst)
keypress = cv2.waitKey(5)
cv2.destroyAllWindows()
return
def undistort(image_path, camera_matrix, distortion_coeffs, camera_matrix_with_crop):
image = cv2.imread(image_path)
cv2.imshow('distorted', resize_image(image, (1920, 1080)))
cv2.waitKey()
undistorted_image = cv2.undistort(image, camera_matrix, distortion_coeffs, None, camera_matrix_with_crop)
cv2.imshow('undistorted', resize_image(undistorted_image, (1920, 1080)))
cv2.waitKey()
def undistort(image_path, camera_matrix, distortion_coeffs,
camera_matrix_with_crop):
from utils.image_utils import resize_image
image = cv2.imread(image_path)
cv2.imshow("distorted", resize_image(image, (1920, 1080)))
cv2.waitKey()
undistorted_image = cv2.undistort(image, camera_matrix, distortion_coeffs,
None, camera_matrix_with_crop)
cv2.imshow("undistorted", resize_image(undistorted_image, (1920, 1080)))
cv2.waitKey()
def cal_undistort(img, obj_points, img_points):
"""
Parameter:
img:
obj_points:
img_points:
Return:
dst:
"""
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
obj_points, img_points, img.shape[1::-1], None, None)
dst = cv2.undistort(img, mtx, dist, None, mtx)
return dst
def undistortion(img, mtx, dist):
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1,
(w, h))
print('roi ', roi)
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
if roi != (0, 0, 0, 0):
dst = dst[y:y + h, x:x + w]
return dst
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 ) # 平移向量 # 外参数
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (u, v), 0, (u, v))
print('newcameramtx外参',newcameramtx)
#打开摄像机
camera=cv2.VideoCapture(0)
while True:
(grabbed,frame)=camera.read()
h1, w1 = frame.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (u, v), 0, (u, v))
# 纠正畸变
dst1 = cv2.undistort(frame, mtx, dist, None, newcameramtx)
#dst2 = cv2.undistort(frame, mtx, dist, None, newcameramtx)
mapx,mapy=cv2.initUndistortRectifyMap(mtx,dist,None,newcameramtx,(w1,h1),5)
dst2=cv2.remap(frame,mapx,mapy,cv2.INTER_LINEAR)
# 裁剪图像，输出纠正畸变以后的图片
x, y, w1, h1 = roi
dst1 = dst1[y:y + h1, x:x + w1]
#cv2.imshow('frame',dst2)
#cv2.imshow('dst1',dst1)
cv2.imshow('dst2', dst2)
if cv2.waitKey(1) & 0xFF == ord('q'): # 按q保存一张图片
cv2.imwrite("../u4/frame.jpg", dst1)
break
camera.release()
def undistort(self, image):
return cv2.undistort(image, self.camera_matrix, self.distortion_coeffs,
None, self.camera_matrix_with_crop)
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 find_3d_points(image1_path, image2_path):
img1 = cv2.imread(image1_path, cv2.IMREAD_GRAYSCALE) # queryImage
img2 = cv2.imread(image2_path, cv2.IMREAD_GRAYSCALE) # trainImage
# Initial calibration matrix from camera
init_calibration_matrix = np.array(
[
[2.78228443e03, 0.00000000e00, 1.65670819e03],
[0.00000000e00, 2.77797243e03, 1.19855894e03],
[0.00000000e00, 0.00000000e00, 1.00000000e00],
]
)
distortion_coefficients = np.array(
[0.07874525, -0.07184864, -0.00619498, 0.00252332, -0.09900985]
)
# Undistort images. getOptimalNewCameraMatrix: 1 tells us that we want to see the "black hills" after undistorting. Exchanging for 0 removes them.
height, width = img1.shape[:2]
calibration_matrix, roi = cv2.getOptimalNewCameraMatrix(
init_calibration_matrix,
distortion_coefficients,
(width, height),
1,
(width, height),
)
img1_distorted = cv2.undistort(
img1, init_calibration_matrix, distortion_coefficients, None, calibration_matrix
)
img2_distorted = cv2.undistort(
img2, init_calibration_matrix, distortion_coefficients, None, calibration_matrix
)
# Crop images
x, y, w, h = roi
img1_distorted = img1_distorted[y : y + h, x : x + w]
img2_distorted = img2_distorted[y : y + h, x : x + w]
# To display the undistorted images:
# plt.imshow(img1_distorted), plt.show()
# plt.imshow(img2_distorted), plt.show()
# Create an ORB object
orb = cv2.ORB_create()
# Detect keypoints
kp1 = orb.detect(img1_distorted, None)
kp2 = orb.detect(img2_distorted, None)
# Find descriptors
kp1, des1 = orb.compute(img1_distorted, kp1)
kp2, des2 = orb.compute(img2_distorted, kp2)
# To draw the keypoints:
#img1kp = cv2.drawKeypoints(img1, kp1, None, color=(0, 255, 0), flags=0) #flags = cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS
# img2kp = cv2.drawKeypoints(img2, kp2, None, color=(0, 255, 0), flags=0)
#plt.imshow(img1kp), plt.show()
# plt.imshow(img2kp), plt.show()
# Brute-force matcher object. crossCheck=True means that it has to match both ways
brute_force = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
# Matching descriptors
matches = brute_force.match(des1, des2)
# Clean the matches by distance
matches = clean_matches(matches)
# Sort matches in order of distance
matches = sorted(matches, key=lambda x: x.distance)
# To draw the first 20 matches:
#img_matches = cv2.drawMatches(img1_distorted, kp1, img2_distorted, kp2, matches[:], None, flags = 2)
#plt.imshow(img_matches), plt.show()
# Extract coordinates
points1 = extract_coordinates(matches, kp1, "queryIdx")
points2 = extract_coordinates(matches, kp2, "trainIdx")
# Find essential Matrix
essential_matrix, _ = cv2.findEssentialMat(
points1, points2, calibration_matrix, method=cv2.RANSAC, prob=0.999, threshold=3
)
determinant = mlin.det(essential_matrix)
eps = 1e-10
if determinant > eps:
raise Exception(
"expected determinant to be close to zero, but is {}".format(determinant)
)
# Find camera2 position relative to camera1 (t is only in unit)
_, R, t, _ = cv2.recoverPose(essential_matrix, points1, points2, calibration_matrix)
# Create camera matrices
M1 = np.hstack((np.eye(3, 3), np.zeros((3, 1))))
M2 = np.hstack((R, t))
camera_matrix1 = np.dot(calibration_matrix, M1)
camera_matrix2 = np.dot(calibration_matrix, M2)
# Compute 3D points
points_3d = []
for c1, c2 in zip(points1, points2):
point = cv2.triangulatePoints(camera_matrix1, camera_matrix2, c1, c2)
points_3d.append(point)
points_3d = cv2.convertPointsFromHomogeneous(np.array(points_3d))
return points_3d, t
