From 17e965b379186a168a310e829c1a2fdc9e0341a0 Mon Sep 17 00:00:00 2001 From: fuly Date: Thu, 21 Nov 2024 22:39:22 +0800 Subject: [PATCH] =?UTF-8?q?1121=E5=AE=8C=E6=88=90=E4=BA=86=E5=A4=96?= =?UTF-8?q?=E5=8F=82=E5=92=8C=E6=A3=8B=E7=9B=98=E6=A0=BC=E6=A3=80=E6=B5=8B?= =?UTF-8?q?=E7=9A=84=E7=89=88=E6=9C=AC=EF=BC=8C=E6=95=B4=E7=90=86=E4=BA=86?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E5=A4=B9=E7=BB=93=E6=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 3 +- calib_tools.py | 31 ++++++++ calibrate_extri.py | 137 ++++++++++++++++++++++++++++++++ calibrate_intri.py | 100 ++++++++++++----------- detect_chessboard.py | 185 +++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 410 insertions(+), 46 deletions(-) create mode 100644 calib_tools.py create mode 100644 calibrate_extri.py create mode 100644 detect_chessboard.py diff --git a/.gitignore b/.gitignore index 60c9895..5cd282d 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,5 @@ __pycache__/ *.pyc *.pyo -test/ \ No newline at end of file +test/ +data/ \ No newline at end of file diff --git a/calib_tools.py b/calib_tools.py new file mode 100644 index 0000000..f39a3fe --- /dev/null +++ b/calib_tools.py @@ -0,0 +1,31 @@ +import os +import numpy as np +import cv2 as cv +import glob +import os.path as osp +import json +from tqdm import tqdm + +def write_json(data, output_path): + with open(output_path, "w") as f: + json.dump(data, f, indent=None, separators=(',', ':')) + + +def read_json(input): + with open(input, "r") as f: + data = json.load(f) + return data + + +def read_img_paths(imgFolder): + imgPaths = [] + for extension in ["jpg", "png", "jpeg", "bmp"]: + imgPaths += glob.glob(osp.join(imgFolder, "*.{}".format(extension))) + return imgPaths + + +def create_output_folder(baseFolder, outputFolder): + folder = osp.join(baseFolder, outputFolder) + if not osp.exists(folder): + os.makedirs(folder) + return folder \ No newline at end of file diff --git a/calibrate_extri.py b/calibrate_extri.py new file mode 100644 index 0000000..b13916d --- /dev/null +++ b/calibrate_extri.py @@ -0,0 +1,137 @@ +import os +from glob import glob +from os.path import join +import numpy as np +import cv2 as cv +import json + +def read_json(input): + with open(input, "r") as f: + data = json.load(f) + return data + +def solvePnP(k3d, k2d, K, dist, flag, tryextri=False): + k2d = np.ascontiguousarray(k2d[:, :2]) # 保留前两列 + # try different initial values: + if tryextri: # 尝试不同的初始化外参 + def closure(rvec, tvec): + ret, rvec, tvec = cv.solvePnP(k3d, k2d, K, dist, rvec, tvec, True, flags=flag) + points2d_repro, xxx = cv.projectPoints(k3d, rvec, tvec, K, dist) + kpts_repro = points2d_repro.squeeze() + err = np.linalg.norm(points2d_repro.squeeze() - k2d, axis=1).mean() + return err, rvec, tvec, kpts_repro + + # create a series of extrinsic parameters looking at the origin + height_guess = 2.7 # 相机的初始高度猜测 + radius_guess = 4. # 相机的初始水平距离猜测，圆的半径，需要根据自己的实际情况调整 + infos = [] + for theta in np.linspace(0, 2 * np.pi, 180): + st = np.sin(theta) + ct = np.cos(theta) + center = np.array([radius_guess * ct, radius_guess * st, height_guess]).reshape(3, 1) + R = np.array([ + [-st, ct, 0], + [0, 0, -1], + [-ct, -st, 0] + ]) + tvec = - R @ center + rvec = cv.Rodrigues(R)[0] + err, rvec, tvec, kpts_repro = closure(rvec, tvec) + infos.append({ + 'err': err, + 'repro': kpts_repro, + 'rvec': rvec, + 'tvec': tvec + }) + infos.sort(key=lambda x: x['err']) + err, rvec, tvec, kpts_repro = infos[0]['err'], infos[0]['rvec'], infos[0]['tvec'], infos[0]['repro'] + else: + # 直接求解的初值是零向量 + ret, rvec, tvec = cv.solvePnP(k3d, k2d, K, dist, flags=flag) + points2d_repro, xxx = cv.projectPoints(k3d, rvec, tvec, K, dist) + kpts_repro = points2d_repro.squeeze() + err = np.linalg.norm(points2d_repro.squeeze() - k2d, axis=1).mean() + # print(err) + return err, rvec, tvec, kpts_repro + +# 对单个相机进行外参标定 +def _calibrate_extri(k3d, k2d, K, dist, flag, tryfocal=False): + extri = {} + methods = [cv.SOLVEPNP_ITERATIVE] + # 检查关键点数据的数量是否匹配 + if k3d.shape[0] != k2d.shape[0]: + print('k3d {} doesnot match k2d {}'.format(k3d.shape, k2d.shape)) + length = min(k3d.shape[0], k2d.shape[0]) + k3d = k3d[:length] + k2d = k2d[:length] + valididx = k2d[:, 2] > 0 # k2d第三列是置信度，检查是否大于0 + if valididx.sum() < 4: # 筛选出有效的2D和3D关键点，数量大于4 + rvec = np.zeros((1, 3)) # 初始话旋转和平移为0并标记为失败 + tvec = np.zeros((3, 1)) + extri['Rvec'] = rvec + extri['R'] = cv.Rodrigues(rvec)[0] + extri['T'] = tvec + print('[ERROR] Failed to initialize the extrinsic parameters') + return extri + k3d = k3d[valididx] + k2d = k2d[valididx] + # 优化相机焦距 + # 如果启用焦距优化 + if tryfocal: + infos = [] + for focal in range(500, 5000, 10): # 遍历焦距范围 + # 设置焦距值 + K[0, 0] = focal # 更新 K 的 fx + K[1, 1] = focal # 更新 K 的 fy + for method in methods: + # 调用 solvePnP + err, rvec, tvec, kpts_repro = solvePnP(k3d, k2d, K, dist, method) + # 保存结果 + infos.append({ + 'focal': focal, + 'err': err, + 'rvec': rvec, + 'tvec': tvec, + 'repro': kpts_repro + }) + # 根据重投影误差选择最佳焦距 + infos.sort(key=lambda x: x['err']) + best_result = infos[0] + focal = best_result['focal'] + err, rvec, tvec, kpts_repro = best_result['err'], best_result['rvec'], best_result['tvec'], best_result['repro'] + # 更新内参中的焦距 + K[0, 0] = focal + K[1, 1] = focal + print(f'[INFO] Optimal focal length found: {focal}, reprojection error: {err:.3f}') + else: + # 如果不优化焦距，直接调用 solvePnP + err, rvec, tvec, kpts_repro = solvePnP(k3d, k2d, K, dist, flag) + + # 保存外参结果 + extri['Rvec'] = rvec + extri['R'] = cv.Rodrigues(rvec)[0] + extri['T'] = tvec + center = - extri['R'].T @ tvec + print(f'[INFO] Camera center: {center.squeeze()}, reprojection error: {err:.3f}') + return extri + +def calibrate_extri(kpts_path, intri_path, flag, tryfocal=False, tryextri=False): + extri = {} + intri_data = read_json(intri_path) + kpts_data = read_json(kpts_path) + # 获取内参 + camnames = list(intri_data.keys()) + for cam in camnames: + print(f'[INFO] Processing camera: {cam}') + K = np.array(intri_data[cam]['K']) + dist = np.array(intri_data[cam]['dist']) + k3d = np.array(kpts_data[cam]['keypoints3d']) + k2d = np.array(kpts_data[cam]['keypoints2d']) + + extri[cam] = _calibrate_extri(k3d, k2d, K, dist, flag, tryfocal=tryfocal) + + return extri + + +if __name__ == "__main__": + pass diff --git a/calibrate_intri.py b/calibrate_intri.py index 320506a..d7ac325 100644 --- a/calibrate_intri.py +++ b/calibrate_intri.py @@ -7,13 +7,16 @@ import json import datetime import argparse +from tqdm import tqdm -def format_json_data(mtx, dist, image_shape): + +def format_json_data(mtx, dist, image_shape, error): data = { "time": datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S"), "K": mtx.tolist(), "dist": dist.tolist(), - "image_shape": image_shape + "image_shape": image_shape, + "error": error } return data @@ -28,27 +31,37 @@ def read_json(input): data = json.load(f) return data + def read_img_paths(imgFolder): imgPaths = [] for extension in ["jpg", "png", "jpeg", "bmp"]: imgPaths += glob.glob(osp.join(imgFolder, "*.{}".format(extension))) return imgPaths + def create_output_folder(baseFolder, outputFolder): folder = osp.join(baseFolder, outputFolder) if not osp.exists(folder): os.makedirs(folder) return folder -def calibrate_camera(baseFolder, chessboardSize, squareSize, visualization): + +base_path = "data" +intri_img_path = osp.join(base_path, "chessboard", "intri") +intri_vis_path = osp.join(base_path, "vis", "intri") +json_output_path = osp.join(base_path, 'output_json') +distortion_images_path = osp.join(base_path, "distortion_images") + + +def calibrate_camera(camera, chessboardSize, squareSize, visualization): # 设置输出目录 if visualization: - outputFolder = create_output_folder(baseFolder, "calibrateOutputImages") + outputFolder = create_output_folder(intri_vis_path, osp.basename(camera)) # 图片路径 - imgPaths = read_img_paths(baseFolder) + imgPaths = read_img_paths(camera) if len(imgPaths) == 0: - print("No images found!") + print("No images found!\n") return # 存储世界坐标和像素坐标 @@ -79,8 +92,8 @@ def calibrate_camera(baseFolder, chessboardSize, squareSize, visualization): # 标定相机 image_shape = gray.shape[::-1] ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(pointsWorld, pointsPixel, image_shape, None, None) - print("Intrinsic matrix:\n", mtx.astype(np.float32)) - print("Distortion coefficients:\n", dist.astype(np.float32)) + # print("Intrinsic matrix:\n", mtx.astype(np.float32)) + # print("Distortion coefficients:\n", dist.astype(np.float32)) # 计算重投影误差 nimg = len(pointsWorld) @@ -93,7 +106,7 @@ def calibrate_camera(baseFolder, chessboardSize, squareSize, visualization): # good_img = np.where(img_error < 0.5)[0] # mean_error = np.mean(img_error[good_img]) mean_error = np.mean(img_error) - print("\nReprojection error: ", mean_error) + print("Reprojection error: ", mean_error) # 挑选出重投影误差小于1.0的图片，重新标定相机 # if len(good_img) == 0: @@ -108,28 +121,30 @@ def calibrate_camera(baseFolder, chessboardSize, squareSize, visualization): # print("Intrinsic matrix:\n", mtx.astype(np.float32)) # print("Distortion coefficients:\n", dist.astype(np.float32)) - data = format_json_data(mtx, dist, image_shape) - # 在文件夹根目录下保存相机内参 - outputJsonPath = osp.join(baseFolder, "intri_calib.json") - write_json(data, outputJsonPath) - return mtx, dist, image_shape + # data = format_json_data(mtx, dist, image_shape) + # # 在文件夹根目录下保存相机内参 + # outputJsonPath = osp.join(baseFolder, "intri_calib.json") + # write_json(data, outputJsonPath) + return mtx, dist, image_shape, mean_error # calibrate_cameras函数中，照片按照相机编号进行分类 -def calibrate_cameras(baseFolder, chessboardSize, squareSize, visualization): - cameras = glob.glob(osp.join(baseFolder, '[0-9]')) - if len(cameras) == 0: +# baseFolder: 包含图片和输出数据的文件夹，默认是./data，可以通过--folder参数指定 +def calibrate_cameras(chessboardSize, squareSize, visualization): + cameras_path = glob.glob(osp.join(intri_img_path, "cam[0-7]")) + if len(cameras_path) == 0: print("No camera folders found!") return - outputJsonPath = osp.join(baseFolder, "intri_calib.json") + data = {} - for camera in cameras: - cameraId = osp.basename(camera) - print("\nCalibrating camera{}...".format(cameraId)) - mtx, dist, image_shape = calibrate_camera(camera, chessboardSize, squareSize, visualization) - data[cameraId] = format_json_data(mtx, dist, image_shape) - write_json(data, outputJsonPath) - print("\nCalibration data saved to: ", outputJsonPath) + for camera_path in tqdm(cameras_path, desc="Processing Cameras", ncols=100): + cameraId = osp.basename(camera_path) + print("\nCalibrating camera {}... ".format(cameraId)) + mtx, dist, image_shape, error = calibrate_camera(camera_path, chessboardSize, squareSize, visualization) + data[cameraId] = format_json_data(mtx, dist, image_shape, error) + write_json(data, osp.join(json_output_path, "intri.json")) + print("Calibration data saved to: ", osp.join(json_output_path, "intri.json")) + # 去除图像畸变 def remove_image_distortion(img, mtx, dist): @@ -140,14 +155,15 @@ def remove_image_distortion(img, mtx, dist): dst = dst[y:y + h, x:x + w] return dst + # 用于去除整个文件夹中的图像畸变，保存到文件夹下的distortion_corrected_images文件夹中 -def remove_images_distortion(baseFolder, mtx, dist): - imgPaths = read_img_paths(baseFolder) +def remove_images_distortion(mtx, dist): + imgPaths = read_img_paths(distortion_images_path) if len(imgPaths) == 0: print("No images found!") return - outputFolder = create_output_folder(baseFolder, "distortion_corrected_images") + outputFolder = create_output_folder(distortion_images_path, "output_images") for imgPath in imgPaths: img = cv.imread(imgPath) @@ -159,31 +175,25 @@ def remove_images_distortion(baseFolder, mtx, dist): if __name__ == "__main__": parser = argparse.ArgumentParser(description="相机内参标定和图像去畸变") - parser.add_argument("--action", type=str, required=True, choices=["camera", "cameras", "distortion"], - help=" --action camera: 标定单个相机" - " --action cameras: 标定多个相机" + parser.add_argument("--action", type=str, required=True, choices=["cameras", "distortion"], + help=" --action cameras: 标定多个相机" " --action distortion: 去除图像畸变") - parser.add_argument("--folder", type=str, required=True, help="包含相机文件夹的基础文件夹") - parser.add_argument("--chessboardSize", type=str, default="11,8", help="棋盘格尺寸，格式为'w,h'") - parser.add_argument("--squareSize", type=float, default=60.0, help="棋盘格方块尺寸") + parser.add_argument("--chessboardSize", type=str, default="11,8", + help="棋盘格角点数 (列数, 行数)，例如 '11,8'") + parser.add_argument("--squareSize", type=float, default=60.0, + help="棋盘格方块的实际边长（单位与数据一致，例如 mm 或 m）") parser.add_argument("--no-vis", dest="vis", action="store_false", help="禁用标定结果的可视化输出") args = parser.parse_args() chessboardSize = tuple(map(int, args.chessboardSize.split(","))) - if args.action == "camera": - print("Calibrating camera...") - mtx, dist, image_shape = calibrate_camera(args.folder, chessboardSize, args.squareSize, args.vis) - outputJsonPath = osp.join(args.folder, "intri_calib.json") - print("Calibration data saved to: ", outputJsonPath) - elif args.action == "cameras": - calibrate_cameras(args.folder, chessboardSize, args.squareSize, args.vis) + if args.action == "cameras": + calibrate_cameras(chessboardSize, args.squareSize, args.vis) elif args.action == "distortion": - print("Removing image distortion...") - data = read_json(osp.join(args.folder, "intri_calib.json")) + print("Removing image distortion, require input folder") + data = read_json(osp.join(json_output_path, "intri.json")) mtx = np.array(data["K"]) dist = np.array(data["dist"]) - remove_images_distortion(args.folder, mtx, dist) + remove_images_distortion(mtx, dist) else: print("Invalid action!") parser.print_help() - diff --git a/detect_chessboard.py b/detect_chessboard.py new file mode 100644 index 0000000..118ecbc --- /dev/null +++ b/detect_chessboard.py @@ -0,0 +1,185 @@ +import os +import numpy as np +import cv2 as cv +import glob +import os.path as osp +import json +from tqdm import tqdm + + +# 先想清楚文件夹结构 +# extri文件夹：存放棋盘格照片，命名规则是cam1.jpg, cam2.jpg, cam3.jpg, ... +# 另一种模式是只检测2d点，不生成3d点，需要指定文件夹和输出路径 + + +def write_json(data, output_path): + with open(output_path, "w") as f: + json.dump(data, f) + + +def read_json(input): + with open(input, "r") as f: + data = json.load(f) + return data + + +def read_img_paths(imgFolder): + imgPaths = [] + for extension in ["jpg", "png", "jpeg", "bmp"]: + imgPaths += glob.glob(osp.join(imgFolder, "*.{}".format(extension))) + return imgPaths + + +def create_output_folder(baseFolder, outputFolder): + folder = osp.join(baseFolder, outputFolder) + if not osp.exists(folder): + os.makedirs(folder) + return folder + +base_path = "data" +extri_img_path = osp.join(base_path, "chessboard", "extri") +extri_vis_path = osp.join(base_path, "vis", "extri") +json_output_path = osp.join(base_path, 'output_json') + +def _findChessboardCorners(img, pattern): + "basic function" + criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) + retval, corners = cv.findChessboardCorners(img, pattern, + flags=cv.CALIB_CB_ADAPTIVE_THRESH + cv.CALIB_CB_FAST_CHECK + cv.CALIB_CB_FILTER_QUADS) + if not retval: + return False, None + corners = cv.cornerSubPix(img, corners, (11, 11), (-1, -1), criteria) + corners = corners.squeeze() + return True, corners + + +def _findChessboardCornersAdapt(img, pattern): + "Adapt mode" + img = cv.adaptiveThreshold(img, 255, cv.ADAPTIVE_THRESH_GAUSSIAN_C, \ + cv.THRESH_BINARY, 21, 2) + return _findChessboardCorners(img, pattern) + +# 检测棋盘格角点并且可视化 +def findChessboardCorners(img_path, pattern, show=False): + img = cv.imread(img_path) + if img is None: + raise FileNotFoundError(f"Image not found at {img_path}") + gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + + # Find the chess board corners + for func in [_findChessboardCorners, _findChessboardCornersAdapt]: + ret, corners = func(gray, pattern) + if ret: break + else: + return None + # 附加置信度 1.0 并返回 + kpts2d = np.hstack([corners, np.ones((corners.shape[0], 1))]) + + if show: + # Draw and display the corners + img_with_corners = cv.drawChessboardCorners(img, pattern, corners, ret) + # 标出棋盘格的原点 + origin = tuple(corners[0].astype(int)) # 原点的像素坐标 + cv.circle(img_with_corners, origin, 10, (0, 0, 255), -1) # 绘制原点 + cv.putText(img_with_corners, "Origin", (origin[0] + 10, origin[1] - 10), + cv.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) + + # 标出最后一个点 + last_point = tuple(corners[-1].astype(int)) # 角点数组的最后一个点 + cv.circle(img_with_corners, last_point, 10, (0, 255, 0), -1) # 绿色圆点 + cv.putText(img_with_corners, "Last", (last_point[0] + 15, last_point[1] - 15), + cv.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) # 添加文字 "Last" + + # 显示图像 + cv.imwrite(osp.join(extri_vis_path, osp.basename(img_path)), img_with_corners) + return kpts2d + + +# 根据棋盘格生成三维坐标，棋盘格坐标系原点在左上角（同时也是全局坐标原点） +# 设定标定板z轴朝上，yx表示棋盘在yx平面上 +# easymocap +# 注意，采用11x8的棋盘格，长边是y轴11，短边是x轴8，可以用opencv试一下 +def getChessboard3d(pattern, gridSize, axis='yx'): + # 注意：这里为了让标定板z轴朝上，设定了短边是x，长边是y + template = np.mgrid[0:pattern[0], 0:pattern[1]].T.reshape(-1, 2) # 棋盘格的坐标 + object_points = np.zeros((pattern[1] * pattern[0], 3), np.float32) # 3d坐标，默认向上的坐标轴为0 + # 长边是x,短边是z + if axis == 'xz': + object_points[:, 0] = template[:, 0] + object_points[:, 2] = template[:, 1] + elif axis == 'yx': + object_points[:, 0] = template[:, 1] + object_points[:, 1] = template[:, 0] + else: + raise NotImplementedError + object_points = object_points * gridSize + return object_points + + +# 检测文件夹下的所有棋盘格图片，生成3d点和2d点，存入json文件 +# 图片应该按照cam0.jpg, cam1.jpg, cam2.jpg, ...的命名方式，要和内参文件夹对应 +def detect_chessboard(pattern, gridSize): + imgPaths = read_img_paths(extri_img_path) + if len(imgPaths) == 0: + print("No images found!") + return + + data = {} + for imgPath in tqdm(imgPaths): + camname = osp.basename(imgPath).split(".")[0] + keypoints2d = findChessboardCorners(imgPath, pattern, show=True) + if keypoints2d is not None: + keypoints3d = getChessboard3d(pattern, gridSize) + data[camname] = { + "keypoints2d": keypoints2d.tolist(), + "keypoints3d": keypoints3d.tolist(), + "pattern": pattern, + "gridSize": gridSize + } + json_path = osp.join(json_output_path, "chessboard_keypoints.json") + write_json(data, json_path) + print(f"Saved keypoints to {json_path}") + + +# 只检测2d点存入json文件 +def detect_chessboard_2d(imgFolder, pattern, outJsonPath): + pass + + +def test_findChessboardCorners(img_path, pattern, saveDir): + imgpaths = read_img_paths(img_path) + for imgpath in imgpaths: + img = cv.imread(imgpath) + gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + ret, corners = cv.findChessboardCorners(gray, pattern) + if ret: + # 在棋盘格上绘制角点 + img_with_corners = cv.drawChessboardCorners(img, pattern, corners, ret) + + # 标出原点 + origin = tuple(corners[0][0]) # 角点数组的第一个点作为原点 + cv.circle(img_with_corners, (int(origin[0]), int(origin[1])), 10, (0, 0, 255), -1) # 红色圆点 + cv.putText(img_with_corners, "Origin", (int(origin[0]) + 15, int(origin[1]) - 15), + cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1) # 添加文字 "Origin" + + # 标出最后一个点 + last_point = tuple(corners[-1][0]) # 角点数组的最后一个点 + cv.circle(img_with_corners, (int(last_point[0]), int(last_point[1])), 10, (0, 255, 0), -1) # 绿色圆点 + cv.putText(img_with_corners, "Last", (int(last_point[0]) + 15, int(last_point[1]) - 15), + cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) # 添加文字 "Last" + + # 保存带角点的图像 + cv.imwrite(osp.join(saveDir, osp.basename(imgpath)), img_with_corners) + else: + print(f"Failed to detect chessboard corners in {imgpath}") + print(f"Saved images to {saveDir}") + +if __name__ == '__main__': + # test1 + img_path = "data/chessboard/extri" + pattern = (11, 8) + # saveDir = "data/test1" + # os.makedirs(saveDir, exist_ok=True) + # test_findChessboardCorners(img_path, pattern, saveDir) + detect_chessboard(pattern, 60) +