import os import os.path as osp import glob import cv2 as cv import numpy as np import json import datetime import argparse from tqdm import tqdm 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, "error": error } return data def write_json(data, output_path): with open(output_path, "w") as f: json.dump(data, f, indent=4) 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" 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(intri_vis_path, osp.basename(camera)) # 图片路径 imgPaths = read_img_paths(camera) if len(imgPaths) == 0: print("No images found!\n") return # 存储世界坐标和像素坐标 # 计算出棋盘格中每个网格角点的坐标，之后当成世界坐标 board_w, board_h = chessboardSize board_grid = np.zeros((board_w * board_h, 3), np.float32) board_grid[:, :2] = np.mgrid[0:board_w, 0:board_h].T.reshape(-1, 2) * squareSize pointsWorld = [] pointsPixel = [] # 遍历图片 for imgPath in imgPaths: img = cv.imread(imgPath) gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) # 查找角点 ret, corners = cv.findChessboardCorners(gray, (board_w, board_h), None) if ret: cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)) pointsWorld.append(board_grid) pointsPixel.append(corners) if visualization: cv.drawChessboardCorners(img, (board_w, board_h), corners, ret) cv.imwrite(osp.join(outputFolder, osp.basename(imgPath)), img) # 标定相机 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)) # 计算重投影误差 nimg = len(pointsWorld) img_error = np.zeros(nimg) for i in range(nimg): imgpoints2, _ = cv.projectPoints(pointsWorld[i], rvecs[i], tvecs[i], mtx, dist) error = cv.norm(pointsPixel[i], imgpoints2, cv.NORM_L2) / len(imgpoints2) img_error[i] = error # good_img = np.where(img_error < 0.5)[0] # mean_error = np.mean(img_error[good_img]) mean_error = np.mean(img_error) print("Reprojection error: ", mean_error) # 挑选出重投影误差小于1.0的图片，重新标定相机 # if len(good_img) == 0: # print("No images with error < 0.5") # elif len(good_img) == nimg: # print("All images have error < 0.5") # pass # else: # pointsWorld2 = [pointsWorld[i] for i in good_img] # pointsPixel2 = [pointsPixel[i] for i in good_img] # ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(pointsWorld2, pointsPixel2, image_shape, None, None) # 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, mean_error # calibrate_cameras函数中，照片按照相机编号进行分类 # 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 data = {} 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): h, w = img.shape[:2] newcameramtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h)) dst = cv.undistort(img, mtx, dist, None, newcameramtx) x, y, w, h = roi dst = dst[y:y + h, x:x + w] return dst # 用于去除整个文件夹中的图像畸变，保存到文件夹下的distortion_corrected_images文件夹中 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(distortion_images_path, "output_images") for imgPath in imgPaths: img = cv.imread(imgPath) dst = remove_image_distortion(img, mtx, dist) cv.imwrite(osp.join(outputFolder, osp.basename(imgPath)), dst) print("Distortion corrected images saved to: ", outputFolder) if __name__ == "__main__": parser = argparse.ArgumentParser(description="相机内参标定和图像去畸变") parser.add_argument("--action", type=str, required=True, choices=["cameras", "distortion"], help=" --action cameras: 标定多个相机" " --action distortion: 去除图像畸变") 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 == "cameras": calibrate_cameras(chessboardSize, args.squareSize, args.vis) elif args.action == "distortion": 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(mtx, dist) else: print("Invalid action!") parser.print_help()