feat: add PnP laser solving and update 0xA5 frame to 4-float protocol
- Integrate PnP rectangle pose estimation + laser world coordinate computation from scripts/calibrate/laser_pnp.py - Publish normalized measurement (laser/RECT) and reference (rect center) via Float32MultiArray - Update gimbal 0xA5 frame to 18-byte format: measure_x, ref_x, measure_y, ref_y - Add camera calibration, laser line params, and RECT_L/W constants - Add corner ordering (TL/BL/BR/TR), SOLVEPNP_IPPE, and pose/laser visualization overlay - Remove standalone vision_example.py (superseded by ROS2 nodes) - Add scripts/calibrate/ with calibration and PnP tools
This commit is contained in:
dev01
@@ -0,0 +1,504 @@
|
||||
# -- 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 = 3 # 每隔多少帧检测一次棋盘格（减少CPU负载）
|
||||
# =======================
|
||||
|
||||
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没有捕获到图像，跳过标定。")
|
||||
Reference in New Issue
Block a user