1121完成了外参和棋盘格检测的版本，整理了文件夹结构

.gitignore

@@ -4,3 +4,4 @@ __pycache__/
 *.pyc
 *.pyo
 test/
+data/

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

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

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("Intrinsic matrix:\n", mtx.astype(np.float32))
-    print("Distortion coefficients:\n", dist.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)
+    # data = format_json_data(mtx, dist, image_shape)
-    # 在文件夹根目录下保存相机内参
+    # # 在文件夹根目录下保存相机内参
-    outputJsonPath = osp.join(baseFolder, "intri_calib.json")
+    # outputJsonPath = osp.join(baseFolder, "intri_calib.json")
-    write_json(data, outputJsonPath)
+    # write_json(data, outputJsonPath)
-    return mtx, dist, image_shape
+    return mtx, dist, image_shape, mean_error
 
 
 # calibrate_cameras函数中，照片按照相机编号进行分类
-def calibrate_cameras(baseFolder, chessboardSize, squareSize, visualization):
+# baseFolder: 包含图片和输出数据的文件夹，默认是./data，可以通过--folder参数指定
-    cameras = glob.glob(osp.join(baseFolder, '[0-9]'))
+def calibrate_cameras(chessboardSize, squareSize, visualization):
-    if len(cameras) == 0:
+    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:
+    for camera_path in tqdm(cameras_path, desc="Processing Cameras", ncols=100):
-        cameraId = osp.basename(camera)
+        cameraId = osp.basename(camera_path)
-        print("\nCalibrating camera{}...".format(cameraId))
+        print("\nCalibrating camera {}... ".format(cameraId))
-        mtx, dist, image_shape = calibrate_camera(camera, chessboardSize, squareSize, visualization)
+        mtx, dist, image_shape, error = calibrate_camera(camera_path, chessboardSize, squareSize, visualization)
-        data[cameraId] = format_json_data(mtx, dist, image_shape)
+        data[cameraId] = format_json_data(mtx, dist, image_shape, error)
-    write_json(data, outputJsonPath)
+    write_json(data, osp.join(json_output_path, "intri.json"))
-    print("\nCalibration data saved to: ", outputJsonPath)
+    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):
+def remove_images_distortion(mtx, dist):
-    imgPaths = read_img_paths(baseFolder)
+    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"],
+    parser.add_argument("--action", type=str, required=True, choices=["cameras", "distortion"],
-                        help=" --action camera: 标定单个相机"
+                        help=" --action cameras: 标定多个相机"
-                             " --action cameras: 标定多个相机"
                              " --action distortion: 去除图像畸变")
-    parser.add_argument("--folder", type=str, required=True, help="包含相机文件夹的基础文件夹")
+    parser.add_argument("--chessboardSize", type=str, default="11,8",
-    parser.add_argument("--chessboardSize", type=str, default="11,8", help="棋盘格尺寸，格式为'w,h'")
+                        help="棋盘格角点数 (列数, 行数)，例如 '11,8'")
-    parser.add_argument("--squareSize", type=float, default=60.0, help="棋盘格方块尺寸")
+    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":
+    if args.action == "cameras":
-        print("Calibrating camera...")
+        calibrate_cameras(chessboardSize, args.squareSize, args.vis)
-        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)
     elif args.action == "distortion":
-        print("Removing image distortion...")
+        print("Removing image distortion, require input folder")
-        data = read_json(osp.join(args.folder, "intri_calib.json"))
+        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()
-

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)
+