# -- coding: utf-8 -- """ 交互式相机内参标定程序 使用方格标定板（15mm方格边长），实时预览，终端交互拍照，自动标定内参矩阵。 """ import sys import platform import os from ctypes import * import numpy as np import cv2 import json # ===== 标定参数配置 ===== SQUARE_SIZE = 15.0 # 方格边长 (mm) CHESSBOARD_COLS = 11 # 内角点列数 CHESSBOARD_ROWS = 8 # 内角点行数 DETECT_INTERVAL = 1 # 每帧都检测棋盘格 # ======================= currentsystem = platform.system() print(f"current system: {currentsystem}") if currentsystem == 'Windows': sys.path.append(os.getenv('MVCAM_COMMON_RUNENV') + "/Samples/Python/MvImport") else: sys.path.append("/opt/MVS/Samples/aarch64/Python/MvImport") from MvCameraControl_class import * if sys.version_info[0] < 3: input_func = raw_input else: input_func = input # ===== 全局状态 ===== g_capture_flag = False g_captured_frames = [] g_image_dir = "calibration_images" g_frame_count = 0 g_cb_detected = False # 当前帧是否检测到棋盘格 g_cb_corners_display = None # 角点坐标（显示图像坐标系） stFrame = POINTER(MV_FRAME_OUT) fun_ctype = get_platform_functype() FrameInfoCallBack2 = fun_ctype(None, stFrame, c_void_p, c_bool) def IsHBPixelFormat(enPixelType=0): if enPixelType in (PixelType_Gvsp_HB_Mono8, PixelType_Gvsp_HB_Mono10, PixelType_Gvsp_HB_Mono10_Packed, PixelType_Gvsp_HB_Mono12, PixelType_Gvsp_HB_Mono12_Packed, PixelType_Gvsp_HB_Mono16, PixelType_Gvsp_HB_RGB8_Packed, PixelType_Gvsp_HB_BGR8_Packed, PixelType_Gvsp_HB_RGBA8_Packed, PixelType_Gvsp_HB_BGRA8_Packed, PixelType_Gvsp_HB_RGB16_Packed, PixelType_Gvsp_HB_BGR16_Packed, PixelType_Gvsp_HB_RGBA16_Packed, PixelType_Gvsp_HB_BGRA16_Packed, PixelType_Gvsp_HB_YUV422_Packed, PixelType_Gvsp_HB_YUV422_YUYV_Packed, PixelType_Gvsp_HB_BayerGR8, PixelType_Gvsp_HB_BayerRG8, PixelType_Gvsp_HB_BayerGB8, PixelType_Gvsp_HB_BayerBG8, PixelType_Gvsp_HB_BayerRBGG8, PixelType_Gvsp_HB_BayerGB10, PixelType_Gvsp_HB_BayerGB10_Packed, PixelType_Gvsp_HB_BayerBG10, PixelType_Gvsp_HB_BayerBG10_Packed, PixelType_Gvsp_HB_BayerRG10, PixelType_Gvsp_HB_BayerRG10_Packed, PixelType_Gvsp_HB_BayerGR10, PixelType_Gvsp_HB_BayerGR10_Packed, PixelType_Gvsp_HB_BayerGB12, PixelType_Gvsp_HB_BayerGB12_Packed, PixelType_Gvsp_HB_BayerBG12, PixelType_Gvsp_HB_BayerBG12_Packed, PixelType_Gvsp_HB_BayerRG12, PixelType_Gvsp_HB_BayerRG12_Packed, PixelType_Gvsp_HB_BayerGR12, PixelType_Gvsp_HB_BayerGR12_Packed): return True else: return False def IsMonoPixelFormat(enPixelType=0): if enPixelType in (PixelType_Gvsp_Mono8, PixelType_Gvsp_Mono10, PixelType_Gvsp_Mono10_Packed, PixelType_Gvsp_Mono12, PixelType_Gvsp_Mono12_Packed, PixelType_Gvsp_Mono14, PixelType_Gvsp_Mono16): return True else: return False def image_callback(pstFrame, pUser, bAutoFree): global g_capture_flag, g_captured_frames, g_frame_count global g_cb_detected, g_cb_corners_display stFrame = cast(pstFrame, POINTER(MV_FRAME_OUT)).contents User = ctypes.cast(pUser, ctypes.py_object).value if not stFrame: return while True: stDecodeParam = MV_CC_HB_DECODE_PARAM() stConvertParam = MV_CC_PIXEL_CONVERT_PARAM_EX() memset(byref(stConvertParam), 0, sizeof(stConvertParam)) result = IsHBPixelFormat(stFrame.stFrameInfo.enPixelType) if True == result: DecodeBufferlen = stFrame.stFrameInfo.nWidth * stFrame.stFrameInfo.nHeight * 3 DecodeBuffer = (c_ubyte * DecodeBufferlen)() stDecodeParam.pSrcBuf = stFrame.pBufAddr stDecodeParam.nSrcLen = stFrame.stFrameInfo.nFrameLen stDecodeParam.pDstBuf = DecodeBuffer stDecodeParam.nDstBufSize = DecodeBufferlen ret = User.MV_CC_HBDecode(stDecodeParam) if ret != 0: print("HB Decode fail! ret[0x%x]" % ret) break stConvertParam.pSrcData = stDecodeParam.pDstBuf stConvertParam.nSrcDataLen = stDecodeParam.nDstBufLen stConvertParam.enSrcPixelType = stDecodeParam.enDstPixelType else: stConvertParam.pSrcData = stFrame.pBufAddr stConvertParam.nSrcDataLen = stFrame.stFrameInfo.nFrameLen stConvertParam.enSrcPixelType = stFrame.stFrameInfo.enPixelType enDstPixelType = PixelType_Gvsp_Undefined nChannelNum = 1 bMono = IsMonoPixelFormat(stConvertParam.enSrcPixelType) if True == bMono: enDstPixelType = PixelType_Gvsp_Mono8 nChannelNum = 1 else: enDstPixelType = PixelType_Gvsp_BGR8_Packed nChannelNum = 3 convertdestBufflen = nChannelNum * stFrame.stFrameInfo.nWidth * stFrame.stFrameInfo.nHeight DstBuffer = (c_ubyte * convertdestBufflen)() DstBufferlen = convertdestBufflen stConvertParam.nWidth = stFrame.stFrameInfo.nWidth stConvertParam.nHeight = stFrame.stFrameInfo.nHeight stConvertParam.enDstPixelType = enDstPixelType stConvertParam.pDstBuffer = DstBuffer stConvertParam.nDstBufferSize = DstBufferlen ret = User.MV_CC_ConvertPixelTypeEx(stConvertParam) if ret != 0: print("convert pixel fail! ret[0x%x]" % ret) break # 构建全分辨率图像 if nChannelNum == 1: numpy_image = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape( stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth) display_image = cv2.cvtColor(numpy_image, cv2.COLOR_GRAY2BGR) else: numpy_image = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape( stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth, 3) display_image = cv2.resize(numpy_image, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA) # 每隔 DETECT_INTERVAL 帧在显示图像上检测棋盘格（速度快） if g_frame_count % DETECT_INTERVAL == 0: gray_display = cv2.cvtColor(display_image, cv2.COLOR_BGR2GRAY) if nChannelNum == 3 else numpy_image ret_cb, corners_disp = cv2.findChessboardCorners( gray_display, (CHESSBOARD_COLS, CHESSBOARD_ROWS), cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE ) g_cb_detected = ret_cb if ret_cb: criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) g_cb_corners_display = cv2.cornerSubPix( gray_display, corners_disp, (5, 5), (-1, -1), criteria) else: g_cb_corners_display = None else: ret_cb = g_cb_detected # 在显示图像上绘制检测结果 if g_cb_detected and g_cb_corners_display is not None: cv2.drawChessboardCorners(display_image, (CHESSBOARD_COLS, CHESSBOARD_ROWS), g_cb_corners_display, True) # 处理拍照指令（保存全分辨率图像，标定时会重新精确检测角点） if g_capture_flag: if g_cb_detected: g_captured_frames.append(numpy_image.copy()) idx = len(g_captured_frames) path = os.path.join(g_image_dir, f"calib_{idx:02d}.png") cv2.imwrite(path, numpy_image) print(f" [OK] 已捕获第 {idx} 张图像 -> {path}") g_capture_flag = False # 若未检测到棋盘格，保持标志等待下一帧 # 绘制状态信息 status_text = f"Captured: {len(g_captured_frames)}" if g_cb_detected: cv2.putText(display_image, "CHESSBOARD DETECTED", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) if g_capture_flag and not g_cb_detected: cv2.putText(display_image, "Waiting for chessboard...", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) cv2.putText(display_image, status_text, (10, display_image.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.imshow('Calibration - Live View', display_image) cv2.waitKey(1) g_frame_count += 1 break if bAutoFree == False: if pUser != None: User.MV_CC_FreeImageBuffer(stFrame) else: print("user is null, invalid") CALL_BACK_FUN = FrameInfoCallBack2(image_callback) def run_calibration(distortion_free=False): """使用已捕获的全分辨率图像进行相机内参标定""" if len(g_captured_frames) == 0: print("错误：没有捕获到任何图像！") return print("\n正在对全分辨率图像进行亚像素角点检测...") objp = np.zeros((CHESSBOARD_COLS * CHESSBOARD_ROWS, 3), np.float32) objp[:, :2] = np.mgrid[0:CHESSBOARD_COLS, 0:CHESSBOARD_ROWS].T.reshape(-1, 2) * SQUARE_SIZE objpoints = [] imgpoints = [] criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) for i, img in enumerate(g_captured_frames): if len(img.shape) == 2: gray = img elif img.shape[2] == 1: gray = img[:, :, 0] if hasattr(img, 'shape') and len(img.shape) == 3 else img else: gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) if len(gray.shape) == 3: gray = cv2.cvtColor(gray, cv2.COLOR_BGR2GRAY) ret_cb, corners = cv2.findChessboardCorners( gray, (CHESSBOARD_COLS, CHESSBOARD_ROWS), cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE ) if ret_cb: corners_sub = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) objpoints.append(objp) imgpoints.append(corners_sub) print(f" 图像 {i + 1}/{len(g_captured_frames)}: 角点检测成功") else: print(f" 图像 {i + 1}/{len(g_captured_frames)}: 角点检测失败，跳过") if len(objpoints) < 3: print(f"\n错误：有效角点图像不足（需要至少3张，当前{len(objpoints)}张）！") return print(f"\n使用 {len(objpoints)} 张有效图像进行标定...") img_size = gray.shape[::-1] if distortion_free: flags = cv2.CALIB_ZERO_TANGENT_DIST | cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 print("畸变模型: 无畸变（所有畸变系数固定为 0）") else: flags = 0 print("畸变模型: 自由估计（k1, k2, p1, p2, k3）") ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera( objpoints, imgpoints, img_size, None, None, flags=flags ) # 计算平均重投影误差 mean_error = 0 for i in range(len(objpoints)): projected, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist) error = cv2.norm(imgpoints[i], projected, cv2.NORM_L2) / len(projected) mean_error += error mean_error /= len(objpoints) print("\n" + "=" * 55) print(" 相机内参标定结果") print("=" * 55) print(f"\n 标定图像数: {len(objpoints)}") print(f" 图像分辨率: {img_size[0]} x {img_size[1]}") print(f" 方格边长: {SQUARE_SIZE} mm") print(f"\n 内参矩阵 K (pixels):") print(f" [{mtx[0,0]:10.4f} {mtx[0,1]:10.4f} {mtx[0,2]:10.4f}]") print(f" [{mtx[1,0]:10.4f} {mtx[1,1]:10.4f} {mtx[1,2]:10.4f}]") print(f" [{mtx[2,0]:10.4f} {mtx[2,1]:10.4f} {mtx[2,2]:10.4f}]") print(f"\n 畸变系数 (k1, k2, p1, p2, k3):") print(f" {dist.ravel()}") print(f"\n 平均重投影误差: {mean_error:.4f} pixels") print("=" * 55) # 保存为 JSON 文件 save_path = "calibration_result.json" result = { "mtx": mtx.tolist(), "dist": dist.tolist(), "img_size": list(img_size), "square_size": SQUARE_SIZE, "error": float(mean_error), } with open(save_path, "w") as f: json.dump(result, f, indent=2) print(f"\n结果已保存到: {save_path}") # 打印 JSON 内容，方便直接粘贴到 pnp_solve.py print(f"\n--- 复制以下 JSON 到 pnp_solve.py 的 CALIB_JSON 变量中 ---") print(json.dumps(result, indent=2)) print(f"----------------------------------------------------------") return mtx, dist if __name__ == "__main__": print("=" * 55) print(" 交互式相机内参标定程序") print(f" 棋盘格: {CHESSBOARD_COLS}x{CHESSBOARD_ROWS} 内角点") print(f" 方格边长: {SQUARE_SIZE} mm") print("=" * 55) print() # 输入拍照数量 while True: try: n_photos = int(input_func("请输入要拍摄的标定图像数量: ")) if n_photos >= 3: break print("至少需要3张图像！") except ValueError: print("请输入有效数字！") # 是否为无畸变镜头 while True: ans = input_func("是否为无畸变镜头？(y/n): ").strip().lower() if ans in ('y', 'n'): break print("请输入 y 或 n") use_distortion_free = (ans == 'y') if use_distortion_free: print("将使用无畸变模型（畸变系数固定为 0）") else: print("将自由估计畸变系数") print(f"\n将拍摄 {n_photos} 张图像。") print("操作说明：") print(" 1. 将棋盘格标定板以不同角度/位置放在相机视野中") print(" 2. 看到绿色 'CHESSBOARD DETECTED' 提示后，输入 y 并回车拍照") print(" 3. 重复直到拍完所有图像") print() # ===== 初始化相机 ===== MvCamera.MV_CC_Initialize() deviceList = MV_CC_DEVICE_INFO_LIST() tlayerType = (MV_GIGE_DEVICE | MV_USB_DEVICE | MV_GENTL_CAMERALINK_DEVICE | MV_GENTL_CXP_DEVICE | MV_GENTL_XOF_DEVICE) ret = MvCamera.MV_CC_EnumDevices(tlayerType, deviceList) if ret != 0: print("enum devices fail! ret[0x%x]" % ret) sys.exit() if deviceList.nDeviceNum == 0: print("find no device!") sys.exit() print("find %d device(s)" % deviceList.nDeviceNum) for i in range(0, deviceList.nDeviceNum): mvcc_dev_info = cast(deviceList.pDeviceInfo[i], POINTER(MV_CC_DEVICE_INFO)).contents if mvcc_dev_info.nTLayerType == MV_GIGE_DEVICE or mvcc_dev_info.nTLayerType == MV_GENTL_GIGE_DEVICE: print("\ngige device: [%d]" % i) strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName if c != 0]) print("device model name: %s" % strModeName) nip1 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24) nip2 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16) nip3 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8) nip4 = (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff) print("current ip: %d.%d.%d.%d\n" % (nip1, nip2, nip3, nip4)) elif mvcc_dev_info.nTLayerType == MV_USB_DEVICE: print("\nu3v device: [%d]" % i) strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName if c != 0]) print("device model name: %s" % strModeName) strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chSerialNumber if c != 0]) print("user serial number: %s" % strSerialNumber) nConnectionNum = 0 if int(nConnectionNum) >= deviceList.nDeviceNum: print("intput error!") sys.exit() cam = MvCamera() stDeviceList = cast(deviceList.pDeviceInfo[int(nConnectionNum)], POINTER(MV_CC_DEVICE_INFO)).contents ret = cam.MV_CC_CreateHandle(stDeviceList) if ret != 0: print("create handle fail! ret[0x%x]" % ret) sys.exit() ret = cam.MV_CC_OpenDevice(MV_ACCESS_Exclusive, 0) if ret != 0: print("open device fail! ret[0x%x]" % ret) sys.exit() if stDeviceList.nTLayerType == MV_GIGE_DEVICE or stDeviceList.nTLayerType == MV_GENTL_GIGE_DEVICE: nPacketSize = cam.MV_CC_GetOptimalPacketSize() if int(nPacketSize) > 0: ret = cam.MV_CC_SetIntValue("GevSCPSPacketSize", nPacketSize) if ret != 0: print("Warning: Set Packet Size fail! ret[0x%x]" % ret) else: print("Warning: Get Packet Size fail! ret[0x%x]" % nPacketSize) ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF) if ret != 0: print("set trigger mode fail! ret[0x%x]" % ret) sys.exit() ret = cam.MV_CC_RegisterImageCallBackEx2(CALL_BACK_FUN, ctypes.py_object(cam), True) if ret != 0: print("register image callback fail! ret[0x%x]" % ret) sys.exit() ret = cam.MV_CC_StartGrabbing() if ret != 0: print("start grabbing fail! ret[0x%x]" % ret) sys.exit() print("相机取流已启动。\n") # 创建保存图像的目录 os.makedirs(g_image_dir, exist_ok=True) print(f"标定图像将保存到: {g_image_dir}/\n") # ===== 交互式拍照循环 ===== try: while len(g_captured_frames) < n_photos: remaining = n_photos - len(g_captured_frames) cmd = input_func(f"[{len(g_captured_frames)}/{n_photos}] 输入 y 并回车拍照 (剩余 {remaining} 张): ") if cmd.strip().lower() == 'y': g_capture_flag = True # 等待回调捕获（每50ms检查一次，最多等5秒） waited = 0 while g_capture_flag and waited < 100: cv2.waitKey(50) waited += 1 if g_capture_flag: print(" 警告: 超时未检测到棋盘格角点，请调整标定板位置后重试") g_capture_flag = False elif cmd.strip().lower() == 'q': print("用户取消操作。") break else: print(" 请输入 y (拍照) 或 q (退出)") except KeyboardInterrupt: print("\n用户中断。") # ===== 停止取流 ===== print("\n停止相机取流...") cv2.destroyAllWindows() ret = cam.MV_CC_StopGrabbing() if ret != 0: print("stop grabbing fail! ret[0x%x]" % ret) ret = cam.MV_CC_CloseDevice() if ret != 0: print("close device fail! ret[0x%x]" % ret) ret = cam.MV_CC_DestroyHandle() if ret != 0: print("destroy handle fail! ret[0x%x]" % ret) MvCamera.MV_CC_Finalize() # ===== 执行标定 ===== if len(g_captured_frames) > 0: run_calibration(distortion_free=use_distortion_free) else: print("\n没有捕获到图像，跳过标定。")