From fd3d6b7ab5e684c61795122f20a49a3aa886c947 Mon Sep 17 00:00:00 2001 From: dev01 Date: Wed, 3 Jun 2026 22:38:51 +0800 Subject: [PATCH] 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 --- docs/云台通信协议指南.md | 24 +- scripts/calibrate/README.md | 109 ++++ scripts/calibrate/calibrate.py | 504 ++++++++++++++++ scripts/calibrate/laser_calibrate.py | 532 ++++++++++++++++ scripts/calibrate/laser_pnp.py | 569 ++++++++++++++++++ scripts/calibrate/pnp_solve.py | 539 +++++++++++++++++ scripts/calibrate/标定与解算流程.md | 49 ++ .../vision_example.py | 0 .../algorithm_pkg/vision_node.py | 285 +++++++-- .../gimbal_control_pkg/gimbal_node.py | 19 +- 10 files changed, 2549 insertions(+), 81 deletions(-) create mode 100644 scripts/calibrate/README.md create mode 100644 scripts/calibrate/calibrate.py create mode 100644 scripts/calibrate/laser_calibrate.py create mode 100644 scripts/calibrate/laser_pnp.py create mode 100644 scripts/calibrate/pnp_solve.py create mode 100644 scripts/calibrate/标定与解算流程.md rename vision_example.py => scripts/vision_example.py (100%) diff --git a/docs/云台通信协议指南.md b/docs/云台通信协议指南.md index 95e6ae3..e854dfa 100644 --- a/docs/云台通信协议指南.md +++ b/docs/云台通信协议指南.md @@ -1,4 +1,4 @@ -# 云台通信协议指南 +# 上位机通信协议指南 ## 串口配置 @@ -15,37 +15,39 @@ | 帧头 | 长度 | 用途 | |---|---|---| -| 0xA5 | 10 | 自动/视觉追踪数据 | -| 0xA6 | 11 | 手动控制数据 | +| 0xA5 | 18 | 自动/视觉追踪数据 | +| 0xA6 | 10 | 手动控制数据 | | 0xA7 | 3 | 命令 | | 0xA8 | 50 | 调试调参数据 | --- -## 1. 自动/视觉追踪数据帧 0xA5 (10 字节) +## 1. 自动/视觉追踪数据帧 0xA5 (18 字节) -**用途**: 视觉识别完成后持续发送跟踪误差。 +**用途**: 视觉识别完成后持续发送目标测量值和参考值。固件侧 PID 内部计算误差 (ref - measure)。 ``` -[SOF:0xA5][error_x:4][error_y:4][checksum:1] +[SOF:0xA5][measure_x:4][ref_x:4][measure_y:4][ref_y:4][checksum:1] ``` | 偏移 | 长度 | 类型 | 含义 | |---|---|---|---| | 0 | 1 | uint8 | 帧头 0xA5 | -| 1 | 4 | float | 归一化 X 轴误差 | -| 5 | 4 | float | 归一化 Y 轴误差 | -| 9 | 1 | uint8 | checksum | +| 1 | 4 | float | X 轴测量值 (视觉检测目标位置) | +| 5 | 4 | float | X 轴参考值 (期望目标位置) | +| 9 | 4 | float | Y 轴测量值 (视觉检测目标位置) | +| 13 | 4 | float | Y 轴参考值 (期望目标位置) | +| 17 | 1 | uint8 | checksum | **Python 写入示例**: ```python import struct -def send_auto_tracking(ser, error_x: float, error_y: float): +def send_auto_tracking(ser, measure_x: float, ref_x: float, measure_y: float, ref_y: float): data = bytearray() data.append(0xA5) - data.extend(struct.pack(' 需要开启激光，激光点投射在标定板上 + +```bash +python laser_calibrate.py +``` + +1. 自动加载 `calibration_result.json` +2. 输入拍摄数量（建议 >= 10 张），标定板以不同姿态摆放 +3. 画面显示绿色棋盘格角点 + 红色激光点圆圈后，输入 `y` 拍照 +4. 每张图像：PnP 解算标定板位姿 -> 提取激光点像素 -> 反投影求交 -> 得到相机坐标系 3D 点 +5. 图像自动保存到 `laser_calib_images/` 目录 +6. 采集完成后 PCA 拟合激光直线，输出： + - `d_cam`：激光方向向量（相机坐标系） + - `O_cam`：基准起点（直线与相机 Z=0 平面的交点） +7. 结果保存为 `laser_result.json`，同时终端打印完整 JSON + +### 第三步：在线运行 + +**仅 PnP 位姿解算：** + +```bash +python pnp_solve.py +``` + +将 `calibrate.py` 输出的 JSON 粘贴到文件顶部的 `CALIB_JSON` 变量中，修改 `RECT_L`/`RECT_W` 为实际矩形尺寸。 + +**PnP 位姿 + 激光点世界坐标：** + +```bash +python laser_pnp.py +``` + +将 `calibrate.py` 输出的 JSON 粘贴到 `CALIB_JSON`，将 `laser_calibrate.py` 输出的 JSON 粘贴到 `LASER_JSON`，修改 `RECT_L`/`RECT_W`。 + +画面实时显示： +- 矩形四个角点标注（TL/BL/BR/TR） +- 右手坐标系坐标轴（X 红 / Y 绿 / Z 蓝） +- 相机平移向量 T 和旋转欧拉角 R +- 激光点世界坐标 (x, y)（仅在 `laser_pnp.py` 中） + +--- + +## 内参矩阵缩放 + +内参矩阵 K 的单位是像素，与图像分辨率绑定。畸变系数（`dist`）是无量纲的，不随分辨率变化。 + +等比例缩放因子 $s = W_{\text{new}} / W_{\text{orig}}$： + +```python +K_scaled = K.copy() +K_scaled[0, 0] *= s # fx +K_scaled[1, 1] *= s # fy +K_scaled[0, 2] *= s # cx +K_scaled[1, 2] *= s # cy +``` + +对应的 `pnp_solve.py` 和 `laser_pnp.py` 通过 `IMAGE_SCALE` 常量自动处理缩放。 + +--- + +## 坐标系约定 + +**世界坐标系**（矩形平面，Z=0）： +``` +TL=(0,0) ──── TR=(l,0) + │ │ +BL=(0,w) ──── BR=(l,w) +``` + +- X 轴沿矩形长边，向右 +- Y 轴沿矩形宽边，向下 +- Z 轴由右手定则确定：X x Y = Z，指向观察者 + +**相机坐标系**：OpenCV 标准右手系，X 右、Y 下、Z 前（指向场景）。 diff --git a/scripts/calibrate/calibrate.py b/scripts/calibrate/calibrate.py new file mode 100644 index 0000000..cb15917 --- /dev/null +++ b/scripts/calibrate/calibrate.py @@ -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没有捕获到图像，跳过标定。") diff --git a/scripts/calibrate/laser_calibrate.py b/scripts/calibrate/laser_calibrate.py new file mode 100644 index 0000000..caca8a5 --- /dev/null +++ b/scripts/calibrate/laser_calibrate.py @@ -0,0 +1,532 @@ +# -- coding: utf-8 -- +""" +激光器-相机联合标定程序 +使用方格标定板 + 激光点，拍摄多组图像，拟合相机坐标系下的激光直线方程。 + +流程： + 1. 加载相机内参 (calibration_result.json) + 2. 实时预览，棋盘格 + 激光点同时可见时按 y 拍照 + 3. 每张照片：PnP 解算标定板位姿 → 提取激光点像素 → 反投影求交 → 得到相机坐标系 3D 点 + 4. 收集所有 3D 点，PCA 拟合直线 + 5. 输出：方向向量 d_cam + 基准起点 O_cam (与相机 Z=0 平面的交点) +""" + +import sys +import platform +import os +from ctypes import * +import numpy as np +import cv2 +import json + +# ===== 标定板参数（与 calibrate.py 保持一致） ===== +SQUARE_SIZE = 15.0 # 方格边长 (mm) +CHESSBOARD_COLS = 11 # 内角点列数 +CHESSBOARD_ROWS = 8 # 内角点行数 +DETECT_INTERVAL = 3 # 棋盘格检测帧间隔 +# ================================================ + +# ===== 激光点检测参数（蓝紫色激光，HSV 色彩空间） ===== +LASER_HSV_LOWER = (100, 50, 50) # H, S, V 下限 +LASER_HSV_UPPER = (160, 255, 255) # H, S, V 上限 +# =================================================== + +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_laser_points_3d = [] # 收集的相机坐标系 3D 激光点 +g_image_dir = "laser_calib_images" +g_frame_count = 0 +g_cb_detected = False +g_cb_corners_display = None +g_laser_display = None # 激光点像素坐标（显示用） +g_K = None +g_dist = 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 + 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 + return False + + +def detect_laser_spot(bgr): + """检测蓝紫色激光点：HSV 色彩空间过滤，返回亚像素质心坐标，以及二值 mask""" + hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(hsv, LASER_HSV_LOWER, LASER_HSV_UPPER) + # 膨胀连接断裂区域 + mask = cv2.dilate(mask, np.ones((3, 3), np.uint8), iterations=1) + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if not contours: + return None, None + + largest = max(contours, key=cv2.contourArea) + M = cv2.moments(largest) + if M["m00"] == 0: + return None, None + cx = M["m10"] / M["m00"] + cy = M["m01"] / M["m00"] + return np.array([cx, cy], dtype=np.float32), mask + + +def inpaint_laser(gray, laser_mask): + """用 inpainting 填补激光区域，避免激光遮盖棋盘格角点""" + if laser_mask is None: + return gray + # 膨胀 mask 留出安全边距 + kernel = np.ones((7, 7), np.uint8) + mask_dilated = cv2.dilate(laser_mask, kernel, iterations=2) + return cv2.inpaint(gray, mask_dilated, 3, cv2.INPAINT_TELEA) + + +def backproject_to_plane(laser_px, rvec, tvec, K): + """ + 将激光点像素坐标反投影到标定板平面，返回相机坐标系下的 3D 点。 + + 标定板平面在相机坐标系中: n · (X - t) = 0, 其中 n = R[:, 2] + 相机射线: P(s) = s * d, 其中 d = K^{-1} * [u, v, 1]^T + 求交: s = (n · t) / (n · d) + """ + R, _ = cv2.Rodrigues(rvec) + n = R[:, 2] # 标定板法向量（相机坐标系） + t = tvec.ravel() # 标定板原点在相机坐标系的位置 + + d = np.linalg.inv(K) @ np.array([laser_px[0], laser_px[1], 1.0]) + d = d / np.linalg.norm(d) # 归一化方向 + + nd = np.dot(n, d) + if abs(nd) < 1e-8: + return None # 射线与平面平行 + + s = np.dot(n, t) / nd + if s <= 0: + return None # 交点在相机后方 + + return (s * d).astype(np.float32) + + +def solve_pnp_for_checkerboard(gray, K, dist): + """检测棋盘格并 PnP 解算标定板位姿""" + ret_cb, corners = cv2.findChessboardCorners( + gray, (CHESSBOARD_COLS, CHESSBOARD_ROWS), + cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE + ) + if not ret_cb: + return None, None + + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + corners_sub = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + + 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 + + success, rvec, tvec = cv2.solvePnP(objp, corners_sub, K, dist, flags=cv2.SOLVEPNP_IPPE) + if not success: + return None, None + + return rvec, tvec + + +def fit_3d_line(points): + """PCA 拟合空间直线，返回 (direction, centroid)""" + pts = np.array(points) + centroid = pts.mean(axis=0) + centered = pts - centroid + _, _, vh = np.linalg.svd(centered) + direction = vh[0] # 第一主成分 = 直线方向 + # 确保方向指向 Z+ 半球 + if direction[2] < 0: + direction = -direction + return direction, centroid + + +def image_callback(pstFrame, pUser, bAutoFree): + global g_capture_flag, g_laser_points_3d, g_frame_count + global g_cb_detected, g_cb_corners_display, g_laser_display + global g_K, g_dist + + 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) + gray_full = numpy_image + 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) + gray_full = cv2.cvtColor(numpy_image, cv2.COLOR_BGR2GRAY) + display_image = cv2.resize(numpy_image, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA) + + # 激光点检测（彩色图，HSV 色彩空间） + if nChannelNum == 3: + laser_px, laser_mask = detect_laser_spot(numpy_image) + else: + laser_px, laser_mask = None, None + + # 用 inpainting 填补激光区域后再做棋盘格检测，避免激光遮盖角点 + if laser_mask is not None: + gray_inpainted = inpaint_laser(gray_full, laser_mask) + else: + gray_inpainted = gray_full + + if g_frame_count % DETECT_INTERVAL == 0: + gray_disp = cv2.resize(gray_inpainted, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA) if nChannelNum == 3 else gray_inpainted + ret_cb, corners_disp = cv2.findChessboardCorners( + gray_disp, (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_disp, corners_disp, (5, 5), (-1, -1), criteria) + else: + g_cb_corners_display = None + + if laser_px is not None and nChannelNum == 3: + g_laser_display = laser_px * 0.5 + elif laser_px is not None: + g_laser_display = laser_px.copy() + else: + g_laser_display = None + + # 拍照 + if g_capture_flag: + if g_cb_detected and laser_px is not None and g_K is not None: + # PnP 使用 inpainting 后的灰度图，避免激光干扰角点提取 + rvec, tvec = solve_pnp_for_checkerboard(gray_inpainted, g_K, g_dist) + if rvec is not None: + p3d = backproject_to_plane(laser_px, rvec, tvec, g_K) + if p3d is not None: + g_laser_points_3d.append(p3d) + idx = len(g_laser_points_3d) + path = os.path.join(g_image_dir, f"laser_{idx:02d}.png") + cv2.imwrite(path, numpy_image) + print(f" [OK] 第 {idx} 个激光点: " + f"像素({laser_px[0]:.1f}, {laser_px[1]:.1f}) -> " + f"3D({p3d[0]:.1f}, {p3d[1]:.1f}, {p3d[2]:.1f}) mm") + print(f" 图像已保存: {path}") + g_capture_flag = False + else: + print(" 警告: 激光点反投影失败") + g_capture_flag = False + else: + print(" 警告: PnP 解算失败") + g_capture_flag = False + else: + pass # 等待检测到棋盘格和激光点 + + # 绘制 + 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) + cv2.putText(display_image, "CHESSBOARD DETECTED", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) + if g_laser_display is not None: + lx, ly = int(g_laser_display[0]), int(g_laser_display[1]) + cv2.circle(display_image, (lx, ly), 2, (0, 0, 255), -1) + cv2.circle(display_image, (lx, ly), 12, (0, 0, 255), 2) + cv2.putText(display_image, "LASER", (lx + 15, ly - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) + + status = f"Points: {len(g_laser_points_3d)}" + cv2.putText(display_image, status, (10, display_image.shape[0] - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + + cv2.imshow('Laser Calibration', display_image) + cv2.waitKey(1) + + g_frame_count += 1 + break + + if bAutoFree == False: + if pUser != None: + User.MV_CC_FreeImageBuffer(stFrame) + + +CALL_BACK_FUN = FrameInfoCallBack2(image_callback) + + +def run_fitting(): + """拟合激光直线并输出结果""" + if len(g_laser_points_3d) < 3: + print(f"错误：激光点数量不足（需要至少3个，当前{len(g_laser_points_3d)}个）！") + return None, None + + direction, centroid = fit_3d_line(g_laser_points_3d) + + # 计算与相机 Z=0 平面的交点作为参考原点 + if abs(direction[2]) > 1e-8: + t_z0 = -centroid[2] / direction[2] + origin = centroid + t_z0 * direction + else: + origin = centroid.copy() + print("警告：激光方向近似平行于相机 Z=0 平面，使用质心作为参考原点") + + # 计算拟合残差 + centered = np.array(g_laser_points_3d) - centroid + residuals = np.linalg.norm(centered - np.outer(np.dot(centered, direction), direction), axis=1) + rms = np.sqrt(np.mean(residuals ** 2)) + + print("\n" + "=" * 55) + print(" 激光直线标定结果") + print("=" * 55) + print(f"\n 采集点数: {len(g_laser_points_3d)}") + print(f" RMS 拟合残差: {rms:.3f} mm") + print(f"\n 直线参数 (相机坐标系):") + print(f" 方向向量 d_cam: [{direction[0]:.6f}, {direction[1]:.6f}, {direction[2]:.6f}]") + print(f" 基准起点 O_cam: [{origin[0]:.2f}, {origin[1]:.2f}, {origin[2]:.2f}] mm") + print(f" (O_cam 为直线与相机 Z=0 平面的交点)") + print("=" * 55) + + # 保存 + result = { + "direction": direction.tolist(), + "origin": origin.tolist(), + "num_points": len(g_laser_points_3d), + "rms_error": float(rms), + "points_3d": [p.tolist() for p in g_laser_points_3d], + } + save_path = "laser_result.json" + with open(save_path, "w") as f: + json.dump(result, f, indent=2) + print(f"\n结果已保存到: {save_path}") + + # 打印可粘贴到 laser_pnp.py 的 JSON + paste = {"direction": direction.tolist(), "origin": origin.tolist()} + print(f"\n--- 复制以下 JSON 到 laser_pnp.py 的 LASER_JSON 变量中 ---") + print(json.dumps(paste, indent=2)) + print(f"---------------------------------------------------------") + + return direction, origin + + +if __name__ == "__main__": + # 加载内参 + calib_path = "calibration_result.json" + try: + with open(calib_path, "r") as f: + calib_data = json.load(f) + g_K = np.array(calib_data["mtx"]) + g_dist = np.array(calib_data["dist"]) + img_size = tuple(calib_data["img_size"]) + print(f"已加载内参: {calib_path}") + print(f" 分辨率: {img_size[0]} x {img_size[1]}") + except FileNotFoundError: + print(f"未找到标定文件: {calib_path}") + print("请先运行 calibrate.py 进行相机内参标定") + sys.exit(1) + + 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("请输入要拍摄的图像数量 (建议 >= 10): ")) + if n_photos >= 3: + break + print("至少需要3张！") + except ValueError: + print("请输入有效数字！") + + print(f"\n将拍摄 {n_photos} 张图像。") + print("操作说明：") + print(" 1. 将标定板以不同姿态放在相机视野中") + print(" 2. 确保激光点投射在标定板上") + print(" 3. 看到绿色棋盘格 + 红色激光点标注后，输入 y 并回车") + 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) + + nConnectionNum = 0 + 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: + cam.MV_CC_SetIntValue("GevSCPSPacketSize", nPacketSize) + + cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF) + + 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_laser_points_3d) < n_photos: + remaining = n_photos - len(g_laser_points_3d) + cmd = input_func(f"[{len(g_laser_points_3d)}/{n_photos}] 输入 y 并回车拍照 (剩余 {remaining}): ") + + if cmd.strip().lower() == 'y': + g_capture_flag = True + 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() + + cam.MV_CC_StopGrabbing() + cam.MV_CC_CloseDevice() + cam.MV_CC_DestroyHandle() + MvCamera.MV_CC_Finalize() + + # ===== 拟合激光直线 ===== + if len(g_laser_points_3d) > 0: + run_fitting() + else: + print("\n没有采集到激光点，跳过拟合。") diff --git a/scripts/calibrate/laser_pnp.py b/scripts/calibrate/laser_pnp.py new file mode 100644 index 0000000..6c6e2c5 --- /dev/null +++ b/scripts/calibrate/laser_pnp.py @@ -0,0 +1,569 @@ +# -- coding: utf-8 -- +""" +PnP 矩形位姿解算 + 激光点世界坐标解算 +从相机画面中检测矩形（4个角点），PnP 解算相机位姿， +同时提取激光点并计算其在世界坐标系（Z=0 平面）上的位置。 +""" + +import sys +import platform +import os +from ctypes import * +import numpy as np +import cv2 +import json + +# ===== 标定结果（从 calibrate.py 生成的 JSON 粘贴到这里） ===== +CALIB_JSON = """ +{ + "mtx": [ + [ + 1247.0565667653996, + 0.0, + 753.8809622002652 + ], + [ + 0.0, + 1256.6168638051222, + 528.9954978706462 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "dist": [ + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "img_size": [ + 1440, + 1080 + ], + "square_size": 15.0, + "error": 0.021690239575344137 +} +""" +# ============================================================= + +# ===== 激光直线参数（从 laser_calibrate.py 生成的 JSON 粘贴到这里） ===== +LASER_JSON = """ +{ + "direction": [ + -0.00989344622939825, + 0.015559548512101173, + 0.9998300075531006 + ], + "origin": [ + -26.1895751953125, + -3.379796028137207, + 0.0 + ] +} +""" +# ================================================================== + +# ===== 矩形物理尺寸 (mm) ===== +RECT_L = 305.0 # 长边 (较长的那条) +RECT_W = 215.0 # 宽边 (较短的那条) +# ============================= + +# ===== 图像缩放因子 ===== +IMAGE_SCALE = 0.5 +# ======================== + +# ===== 图像预处理参数 ===== +BLUR_KERNEL = (5, 5) +MORPH_KERNEL = np.ones((3, 3), np.uint8) +MIN_AREA_RATIO = 0.01 +# =========================== + +currentsystem = platform.system() +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_K = None +g_dist = None +g_d_cam = None # 激光方向向量（相机坐标系） +g_O_cam = None # 激光基准起点（相机坐标系） + + +def parse_calibration(): + data = json.loads(CALIB_JSON) + mtx = np.array(data["mtx"]) + dist = np.array(data["dist"]) + img_size = tuple(data["img_size"]) + print(f"已加载标定结果:") + print(f" 标定分辨率: {img_size[0]} x {img_size[1]}") + print(f" 内参矩阵:\n{mtx}") + return mtx, dist + + +def parse_laser(): + data = json.loads(LASER_JSON) + d = np.array(data["direction"], dtype=np.float64) + o = np.array(data["origin"], dtype=np.float64) + print(f"已加载激光直线参数:") + print(f" 方向向量 d_cam: {d}") + print(f" 基准起点 O_cam: {o}") + return d, o + + +def detect_rectangle(gray, image_area): + h, w = gray.shape[:2] + min_area = image_area * MIN_AREA_RATIO + + blurred = cv2.GaussianBlur(gray, BLUR_KERNEL, 1) + _, binary = cv2.threshold(blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU) + + closed = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, MORPH_KERNEL) + + contours, _ = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + best = None + best_area = 0 + for cnt in contours: + area = cv2.contourArea(cnt) + if area < min_area: + continue + + epsilon = 0.02 * cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, epsilon, True) + + if len(approx) == 4 and cv2.isContourConvex(approx): + if area > best_area: + best_area = area + best = approx.reshape(4, 2) + + return best.astype(np.float32) if best is not None else None + + +def order_rectangle_corners(image_corners): + pts = image_corners.copy() + + edge_info = [] + for i in range(4): + j = (i + 1) % 4 + edge_info.append((i, j, np.linalg.norm(pts[i] - pts[j]))) + + edge_info.sort(key=lambda e: e[2]) + short_a, short_b = edge_info[0], edge_info[1] + + def mid_x(edge): + i, j, _ = edge + return (pts[i][0] + pts[j][0]) / 2.0 + + if mid_x(short_a) < mid_x(short_b): + left_edge, right_edge = short_a, short_b + else: + left_edge, right_edge = short_b, short_a + + li, lj, _ = left_edge + if pts[li][1] < pts[lj][1]: + TL_idx, BL_idx = li, lj + else: + TL_idx, BL_idx = lj, li + + ri, rj, _ = right_edge + if pts[ri][1] < pts[rj][1]: + TR_idx, BR_idx = ri, rj + else: + TR_idx, BR_idx = rj, ri + + ordered = pts[[TL_idx, BL_idx, BR_idx, TR_idx]] + world = np.array([ + [0, 0], + [0, RECT_W], + [RECT_L, RECT_W], + [RECT_L, 0], + ], dtype=np.float32) + + return ordered, world + + +def compute_laser_world_and_px(rvec, tvec, K, dist, d_cam, O_cam): + """ + 将激光直线从相机坐标系变换到世界坐标系，求与 Z=0 平面的交点， + 并投影回图像坐标用于可视化。 + + 步骤： + 1. 从 PnP 结果提取世界转相机的关系: X_cam = R * X_world + t + 逆变换: X_world = R^T * (X_cam - t) + 2. 方向向量（仅受旋转影响）: d_world = R^T * d_cam + 3. 基准起点（受旋转+平移）: O_world = R^T * (O_cam - t) + 4. 射线 L(s) = O_world + s * d_world + 5. 令 Z=0，解 s = -O_world[2] / d_world[2]，代入求 X, Y + + 返回: (x, y) 激光点在 Z=0 平面上的世界坐标，或 None + """ + R, _ = cv2.Rodrigues(rvec) + R_inv = R.T # 即 R^T + t = tvec.ravel() + + d_world = R_inv @ d_cam + O_world = R_inv @ (O_cam - t) + + if abs(d_world[2]) < 1e-10: + return None, None # 激光平行于 Z=0 平面 + + s = -O_world[2] / d_world[2] + P_world = O_world + s * d_world + laser_world = P_world[:2] # (x, y) + + # 投影回图像坐标 + P_world_3d = np.array([[P_world[0], P_world[1], 0]], dtype=np.float32) + px_proj, _ = cv2.projectPoints(P_world_3d, rvec, tvec, K, dist) + laser_px = px_proj.ravel() + + return laser_world, laser_px + + +def draw_results(image, ordered_corners, rvec, tvec, K, dist, + laser_px=None, laser_world=None): + display = image.copy() + h = display.shape[0] + + labels = ["TL", "BL", "BR", "TR"] + colors = [(0, 0, 255), (255, 0, 0), (0, 255, 0), (255, 255, 0)] + + for i, (pt, label) in enumerate(zip(ordered_corners, labels)): + px, py = int(pt[0]), int(pt[1]) + cv2.circle(display, (px, py), 6, colors[i], -1) + cv2.putText(display, f"{label} ({px},{py})", (px + 10, py - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, colors[i], 2) + + for i in range(4): + j = (i + 1) % 4 + p1 = tuple(ordered_corners[i].astype(int)) + p2 = tuple(ordered_corners[j].astype(int)) + cv2.line(display, p1, p2, (255, 255, 255), 2) + + # 绘制激光点 + if laser_px is not None: + lx, ly = int(laser_px[0]), int(laser_px[1]) + cv2.circle(display, (lx, ly), 2, (0, 0, 255), -1) + cv2.circle(display, (lx, ly), 10, (0, 0, 255), 2) + cv2.putText(display, "LASER", (lx + 15, ly - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) + + # 绘制坐标轴 + axis_len = 50 + axis_3d = np.float32([[0, 0, 0], [axis_len, 0, 0], [0, axis_len, 0], [0, 0, axis_len]]) + imgpts, _ = cv2.projectPoints(axis_3d, rvec, tvec, K, dist) + origin = tuple(imgpts[0].ravel().astype(int)) + + cv2.line(display, origin, tuple(imgpts[1].ravel().astype(int)), (0, 0, 255), 3) + cv2.line(display, origin, tuple(imgpts[2].ravel().astype(int)), (0, 255, 0), 3) + cv2.line(display, origin, tuple(imgpts[3].ravel().astype(int)), (255, 0, 0), 3) + + # 位姿文本 + if rvec is not None: + t = tvec.ravel() + r, _ = cv2.Rodrigues(rvec) + sy = np.sqrt(r[0, 0] ** 2 + r[1, 0] ** 2) + if sy > 1e-6: + rx = np.arctan2(r[2, 1], r[2, 2]) + ry = np.arctan2(-r[2, 0], sy) + rz = np.arctan2(r[1, 0], r[0, 0]) + else: + rx = np.arctan2(-r[2, 1], r[1, 1]) + ry = np.arctan2(-r[2, 0], sy) + rz = 0 + + lines = [ + f"T: [{t[0]:.1f}, {t[1]:.1f}, {t[2]:.1f}] mm", + f"R: [{np.degrees(rx):.1f}, {np.degrees(ry):.1f}, {np.degrees(rz):.1f}] deg", + ] + if laser_world is not None: + lines.append(f"Laser: ({laser_world[0]:.1f}, {laser_world[1]:.1f}) mm") + + for i, text in enumerate(lines): + cv2.putText(display, text, (10, h - 10 - 25 * (2 - i)), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2) + + return display + + +def solve_frame(frame, K, dist, d_cam, O_cam): + """处理一帧：检测矩形 → PnP → 解算激光世界坐标 → 投影到图像""" + h, w = frame.shape[:2] + image_area = h * w + + if len(frame.shape) == 3: + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + else: + gray = frame + + corners = detect_rectangle(gray, image_area) + if corners is None: + return None, None, None, None, None + + ordered_corners, world_pts = order_rectangle_corners(corners) + world_pts_3d = np.hstack([world_pts, np.zeros((4, 1), dtype=np.float32)]) + + success, rvec, tvec = cv2.solvePnP( + world_pts_3d, ordered_corners, K, dist, + flags=cv2.SOLVEPNP_IPPE + ) + if not success: + return None, None, ordered_corners, None, None + + # 用标定好的激光直线参数直接解算世界坐标并投影回图像 + laser_world, laser_px = compute_laser_world_and_px(rvec, tvec, K, dist, d_cam, O_cam) + + return rvec, tvec, ordered_corners, laser_px, laser_world + + +# ===== 海康相机回调 ===== +stFrame = POINTER(MV_FRAME_OUT) +fun_ctype = get_platform_functype() +FrameInfoCallBack2 = fun_ctype(None, stFrame, c_void_p, c_bool) + +g_frame = None +g_frame_ready = False + + +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 + 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 + return False + + +def image_callback(pstFrame, pUser, bAutoFree): + global g_frame, g_frame_ready + + 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: + 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: + break + + if nChannelNum == 1: + g_frame = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape( + stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth) + else: + g_frame = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape( + stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth, 3) + + g_frame_ready = True + break + + if bAutoFree == False: + if pUser != None: + User.MV_CC_FreeImageBuffer(stFrame) + + +CALL_BACK_FUN = FrameInfoCallBack2(image_callback) + + +def init_camera(): + 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) + + nConnectionNum = 0 + 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: + cam.MV_CC_SetIntValue("GevSCPSPacketSize", nPacketSize) + + cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF) + + 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() + + return cam + + +def close_camera(cam): + cv2.destroyAllWindows() + cam.MV_CC_StopGrabbing() + cam.MV_CC_CloseDevice() + cam.MV_CC_DestroyHandle() + MvCamera.MV_CC_Finalize() + + +def run_live(): + global g_frame, g_frame_ready, g_K, g_dist, g_d_cam, g_O_cam + + cam = init_camera() + print("相机取流已启动。按 q 退出。\n") + + while not g_frame_ready: + cv2.waitKey(50) + + K_use = g_K.copy() + K_use[0, 0] *= IMAGE_SCALE + K_use[1, 1] *= IMAGE_SCALE + K_use[0, 2] *= IMAGE_SCALE + K_use[1, 2] *= IMAGE_SCALE + + try: + while True: + if g_frame_ready and g_frame is not None: + frame = g_frame.copy() + g_frame_ready = False + + if IMAGE_SCALE != 1.0: + frame = cv2.resize(frame, None, fx=IMAGE_SCALE, fy=IMAGE_SCALE, + interpolation=cv2.INTER_AREA) + + rvec, tvec, ordered, laser_px, laser_world = solve_frame( + frame, K_use, g_dist, g_d_cam, g_O_cam) + + if rvec is not None: + result = draw_results(frame, ordered, rvec, tvec, K_use, g_dist, + laser_px, laser_world) + status = "TRACKING" + else: + result = frame + status = "NO RECTANGLE" + + cv2.putText(result, status, (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, + (0, 255, 0) if rvec is not None else (0, 0, 255), 2) + + cv2.imshow("Laser PnP Solver", result) + + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + break + except KeyboardInterrupt: + print("\n用户中断。") + + close_camera(cam) + + +if __name__ == "__main__": + if RECT_W > RECT_L: + RECT_L, RECT_W = RECT_W, RECT_L + print(f"矩形尺寸: 长={RECT_L} mm, 宽={RECT_W} mm") + + g_K, g_dist = parse_calibration() + g_d_cam, g_O_cam = parse_laser() + + run_live() diff --git a/scripts/calibrate/pnp_solve.py b/scripts/calibrate/pnp_solve.py new file mode 100644 index 0000000..01f62a4 --- /dev/null +++ b/scripts/calibrate/pnp_solve.py @@ -0,0 +1,539 @@ +# -- coding: utf-8 -- +""" +PnP 矩形位姿解算 +从相机画面中检测矩形（4个角点），利用标定好的内参矩阵，通过 PnP 解算矩形在相机坐标系下的位姿。 +""" + +import sys +import platform +import os +from ctypes import * +import numpy as np +import cv2 +import json + +# ===== 标定结果（从 calibrate.py 生成的 JSON 粘贴到这里） ===== +CALIB_JSON = """ +{ + "mtx": [ + [ + 1247.0565667653996, + 0.0, + 753.8809622002652 + ], + [ + 0.0, + 1256.6168638051222, + 528.9954978706462 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "dist": [ + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "img_size": [ + 1440, + 1080 + ], + "square_size": 15.0, + "error": 0.021690239575344137 +} +""" +# ============================================================= + +# ===== 矩形物理尺寸 (mm) ===== +RECT_L = 305.0 # 长边 (较长的那条) +RECT_W = 215.0 # 宽边 (较短的那条) +# ============================= + +# ===== 图像缩放因子 ===== +IMAGE_SCALE = 0.5 # 使用图像相对标定原图的缩放比，K 矩阵自动等比缩放 +# ======================== + +# ===== 图像预处理参数 ===== +BLUR_KERNEL = (5, 5) +MORPH_KERNEL = np.ones((3, 3), np.uint8) +MIN_AREA_RATIO = 0.01 # 矩形最小面积占画面比例 +# =========================== + +currentsystem = platform.system() +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_K = None # 内参矩阵 +g_dist = None # 畸变系数 + + +def parse_calibration(): + """从 CALIB_JSON 常量解析标定结果""" + data = json.loads(CALIB_JSON) + mtx = np.array(data["mtx"]) + dist = np.array(data["dist"]) + img_size = tuple(data["img_size"]) + print(f"已加载标定结果:") + print(f" 标定分辨率: {img_size[0]} x {img_size[1]}") + print(f" 内参矩阵:\n{mtx}") + print(f" 畸变系数: {dist.ravel()}") + return mtx, dist + + +def detect_rectangle(gray, image_area): + """ + 检测图像中的矩形，返回四个角点。 + 复用 run.py 的预处理 + 轮廓检测流程。 + 返回: corners (4,2) 或 None + """ + h, w = gray.shape[:2] + min_area = image_area * MIN_AREA_RATIO + + blurred = cv2.GaussianBlur(gray, BLUR_KERNEL, 1) + _, binary = cv2.threshold(blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU) + + closed = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, MORPH_KERNEL) + + contours, _ = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + best = None + best_area = 0 + for cnt in contours: + area = cv2.contourArea(cnt) + if area < min_area: + continue + + epsilon = 0.02 * cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, epsilon, True) + + if len(approx) == 4 and cv2.isContourConvex(approx): + if area > best_area: + best_area = area + best = approx.reshape(4, 2) + + return best.astype(np.float32) if best is not None else None + + +def order_rectangle_corners(image_corners): + """ + 将检测到的 4 个图像角点排序，利用长宽约束识别各角点身份。 + + 逻辑： + 1. 计算 4 条边的长度，找出 2 条短边 + 2. 比较两条短边中点的 x 坐标，x 较小者为左短边，另一条为右短边 + 3. 左短边上 y 较小的端点为 TL，较大的为 BL + 4. 右短边上 y 较小的端点为 TR，较大的为 BR + 5. 按逆时针顺序返回: [TL, BL, BR, TR] + + 返回: (ordered_corners, world_points) + ordered_corners: [TL, BL, BR, TR] 图像坐标 + world_points: [TL, BL, BR, TR] 世界坐标 + """ + pts = image_corners.copy() + + # 1. 计算 4 条边（approxPolyDP 返回的点是首尾相连的轮廓顺序） + edge_info = [] # [(i, j, length), ...] + for i in range(4): + j = (i + 1) % 4 + edge_info.append((i, j, np.linalg.norm(pts[i] - pts[j]))) + + # 2. 按边长排序，找出两条短边 + edge_info.sort(key=lambda e: e[2]) + short_a, short_b = edge_info[0], edge_info[1] # 两条短边 + + # 3. 比较中点 x 坐标，确定左短边和右短边 + def mid_x(edge): + i, j, _ = edge + return (pts[i][0] + pts[j][0]) / 2.0 + + if mid_x(short_a) < mid_x(short_b): + left_edge = short_a + right_edge = short_b + else: + left_edge = short_b + right_edge = short_a + + # 4. 左短边上 y 较小的是 TL，较大的是 BL + li, lj, _ = left_edge + if pts[li][1] < pts[lj][1]: + TL_idx, BL_idx = li, lj + else: + TL_idx, BL_idx = lj, li + + # 5. 右短边上 y 较小的是 TR，较大的是 BR + ri, rj, _ = right_edge + if pts[ri][1] < pts[rj][1]: + TR_idx, BR_idx = ri, rj + else: + TR_idx, BR_idx = rj, ri + + # 逆时针: TL -> BL -> BR -> TR + ordered = pts[[TL_idx, BL_idx, BR_idx, TR_idx]] + + # 世界坐标，与角点顺序一一对应 + world = np.array([ + [0, 0], # TL + [0, RECT_W], # BL + [RECT_L, RECT_W], # BR + [RECT_L, 0], # TR + ], dtype=np.float32) + + return ordered, world + + +def draw_results(image, ordered_corners, rvec, tvec, K, dist): + """在图像上绘制角点标注和位姿信息""" + display = image.copy() + h, w = display.shape[:2] + + labels = ["TL", "BL", "BR", "TR"] + colors = [(0, 0, 255), (255, 0, 0), (0, 255, 0), (255, 255, 0)] + + for i, (pt, label) in enumerate(zip(ordered_corners, labels)): + px, py = int(pt[0]), int(pt[1]) + cv2.circle(display, (px, py), 6, colors[i], -1) + cv2.putText(display, f"{label} ({px},{py})", (px + 10, py - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, colors[i], 2) + + # 绘制矩形边 + for i in range(4): + j = (i + 1) % 4 + p1 = tuple(ordered_corners[i].astype(int)) + p2 = tuple(ordered_corners[j].astype(int)) + cv2.line(display, p1, p2, (255, 255, 255), 2) + + # 绘制坐标轴（投影到图像） + axis_len = 50 # mm + axis_3d = np.float32([[0, 0, 0], [axis_len, 0, 0], [0, axis_len, 0], [0, 0, axis_len]]) + imgpts, _ = cv2.projectPoints(axis_3d, rvec, tvec, K, dist) + origin = tuple(imgpts[0].ravel().astype(int)) + + cv2.line(display, origin, tuple(imgpts[1].ravel().astype(int)), (0, 0, 255), 3) # X: red + cv2.line(display, origin, tuple(imgpts[2].ravel().astype(int)), (0, 255, 0), 3) # Y: green + cv2.line(display, origin, tuple(imgpts[3].ravel().astype(int)), (255, 0, 0), 3) # Z: blue + + # 位姿文本 + if rvec is not None: + t = tvec.ravel() + r, _ = cv2.Rodrigues(rvec) + # 欧拉角 (ZYX) + sy = np.sqrt(r[0, 0] ** 2 + r[1, 0] ** 2) + if sy > 1e-6: + rx = np.arctan2(r[2, 1], r[2, 2]) + ry = np.arctan2(-r[2, 0], sy) + rz = np.arctan2(r[1, 0], r[0, 0]) + else: + rx = np.arctan2(-r[2, 1], r[1, 1]) + ry = np.arctan2(-r[2, 0], sy) + rz = 0 + + lines = [ + f"T: [{t[0]:.1f}, {t[1]:.1f}, {t[2]:.1f}] mm", + f"R: [{np.degrees(rx):.1f}, {np.degrees(ry):.1f}, {np.degrees(rz):.1f}] deg", + ] + for i, text in enumerate(lines): + cv2.putText(display, text, (10, h - 10 - 25 * (1 - i)), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2) + + return display + + +def solve_pnp(image, K, dist, debug=False): + """ + 从单张图像中检测矩形并解算位姿。 + 返回: (rvec, tvec, ordered_corners) 或 (None, None, None) + """ + h, w = image.shape[:2] + image_area = h * w + + if len(image.shape) == 3: + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + else: + gray = image + + corners = detect_rectangle(gray, image_area) + if corners is None: + return None, None, None + + ordered_corners, world_pts = order_rectangle_corners(corners) + + # solvePnP 要求世界坐标为 Nx3（矩形在 Z=0 平面上） + world_pts_3d = np.hstack([world_pts, np.zeros((4, 1), dtype=np.float32)]) + + success, rvec, tvec = cv2.solvePnP( + world_pts_3d, ordered_corners, K, dist, + flags=cv2.SOLVEPNP_IPPE + ) + + if not success: + return None, None, None + + if debug: + print(f" 平移向量 (mm): {tvec.ravel()}") + print(f" 旋转向量: {rvec.ravel()}") + + return rvec, tvec, ordered_corners + + +# ===== 海康相机相关函数 ===== +stFrame = POINTER(MV_FRAME_OUT) +fun_ctype = get_platform_functype() +FrameInfoCallBack2 = fun_ctype(None, stFrame, c_void_p, c_bool) + +g_frame = None +g_frame_ready = False + + +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 + 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 + return False + + +def image_callback(pstFrame, pUser, bAutoFree): + global g_frame, g_frame_ready + + 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: + 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: + break + + if nChannelNum == 1: + g_frame = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape( + stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth) + else: + g_frame = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape( + stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth, 3) + + g_frame_ready = True + break + + if bAutoFree == False: + if pUser != None: + User.MV_CC_FreeImageBuffer(stFrame) + + +CALL_BACK_FUN = FrameInfoCallBack2(image_callback) + + +def init_camera(): + """初始化海康相机，返回 cam 对象""" + 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_USB_DEVICE: + strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName if c != 0]) + print(" [%d] %s" % (i, strModeName)) + elif mvcc_dev_info.nTLayerType == MV_GIGE_DEVICE or mvcc_dev_info.nTLayerType == MV_GENTL_GIGE_DEVICE: + strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName if c != 0]) + print(" [%d] %s" % (i, strModeName)) + + nConnectionNum = 0 + 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: + cam.MV_CC_SetIntValue("GevSCPSPacketSize", nPacketSize) + + cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF) + + 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() + + return cam + + +def close_camera(cam): + cv2.destroyAllWindows() + cam.MV_CC_StopGrabbing() + cam.MV_CC_CloseDevice() + cam.MV_CC_DestroyHandle() + MvCamera.MV_CC_Finalize() + + +def run_live(): + """实时相机模式：持续检测矩形并解算位姿""" + global g_frame, g_frame_ready, g_K, g_dist + + cam = init_camera() + print("相机取流已启动。按 q 退出。\n") + + # 等待第一帧 + while not g_frame_ready: + cv2.waitKey(50) + + # 根据缩放因子调整内参矩阵 + K_use = g_K.copy() + K_use[0, 0] *= IMAGE_SCALE # fx + K_use[1, 1] *= IMAGE_SCALE # fy + K_use[0, 2] *= IMAGE_SCALE # cx + K_use[1, 2] *= IMAGE_SCALE # cy + + try: + while True: + if g_frame_ready and g_frame is not None: + frame = g_frame.copy() + g_frame_ready = False + + if IMAGE_SCALE != 1.0: + frame = cv2.resize(frame, None, fx=IMAGE_SCALE, fy=IMAGE_SCALE, + interpolation=cv2.INTER_AREA) + + rvec, tvec, ordered = solve_pnp(frame, K_use, g_dist) + + if rvec is not None: + result = draw_results(frame, ordered, rvec, tvec, K_use, g_dist) + status = "TRACKING" + else: + result = frame + status = "NO RECTANGLE" + + cv2.putText(result, status, (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, + (0, 255, 0) if rvec is not None else (0, 0, 255), 2) + + cv2.imshow("PnP Solver", result) + + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + break + except KeyboardInterrupt: + print("\n用户中断。") + + close_camera(cam) + + +if __name__ == "__main__": + if RECT_W > RECT_L: + RECT_L, RECT_W = RECT_W, RECT_L + print(f"矩形尺寸: 长={RECT_L} mm, 宽={RECT_W} mm") + + g_K, g_dist = parse_calibration() + + run_live() diff --git a/scripts/calibrate/标定与解算流程.md b/scripts/calibrate/标定与解算流程.md new file mode 100644 index 0000000..703ecc7 --- /dev/null +++ b/scripts/calibrate/标定与解算流程.md @@ -0,0 +1,49 @@ + +### 激光点位置解算详细说明 + +本方案旨在通过视觉PnP算法解算相机位姿，结合预先标定的“相机-激光器”刚体关系，计算激光点在矩形平面（世界坐标系）上的精确位置。 + +#### 1. 前置准备：相机内参标定 +首先对相机进行内参标定，获取相机内参矩阵 $K$ 和畸变系数。这是后续所有像素坐标转空间坐标的基础。 + +#### 2. 第一阶段：激光器与相机的联合标定（离线） +由于激光器与相机刚性绑定，激光束在**相机坐标系**下表现为一条固定的空间直线。我们需要通过以下步骤求出这条直线的方程（起点与方向）。 + +1. **数据采集**： + - 开启激光器，将激光点投射在平面标定板（如棋盘格）上。 + - 保持相机与激光器相对静止，改变整体姿态（距离、角度），拍摄多组（建议10组以上）包含标定板和激光点的图像。 + +2. **解算相机坐标系下的激光点**： + - **外参获取**：对每张图像使用PnP算法，根据标定板角点计算相机相对于标定板的外参矩阵 $T_{wc}$（即世界到相机的变换）。 + - **反投影求交**： + - 提取激光点的像素坐标 $(u, v)$。 + - 利用 $K^{-1}$ 将其转换为归一化射线方向。 + - **关键步骤**：利用标定板在相机坐标系下的平面方程与该射线求交，计算出激光点在**相机坐标系**下的三维坐标 $P_{c\_i}$。 + +3. **拟合激光线方程**： + - 收集所有图像对应的相机坐标系下的激光点 $\{P_{c\_1}, P_{c\_2}, ...\}$。 + - 使用最小二乘法或PCA（主成分分析）拟合这些点，得到相机坐标系下的激光直线方程。 + - **记录参数**： + - **方向向量 $\vec{d}_{cam}$**：拟合直线的单位方向向量。 + - **基准起点 $O_{cam}$**：选取该直线与相机坐标系 $Z=0$ 平面的交点作为参考原点。 + +#### 3. 第二阶段：在线测量与位置解算 +标定完成后，即可在实际应用中实时计算激光点位置。 + +1. **获取当前相机位姿**： + - 拍摄包含目标矩形平面的图像。 + - 根据成像原理：$\text{像素坐标} \approx K \cdot [\text{归一化平面}] (T_{wc} \cdot \text{世界坐标})$。 + - 已知像素坐标、内参 $K$ 以及矩形平面的特征点世界坐标，利用PnP算法解算出当前的外参矩阵 $T_{wc}$（包含旋转矩阵 $R$ 和平移向量 $t$）。 + +2. **坐标变换至世界坐标系**： + 将标定阶段得到的激光线参数从相机坐标系变换到世界坐标系： + - **变换方向向量**（仅受旋转影响）： + $$ \vec{d}_{world} = R^T \cdot \vec{d}_{cam} $$ + - **变换基准起点**（受旋转和平移共同影响，即乘以 $T_{wc}$ 的逆矩阵）： + $$ O_{world} = R^T \cdot (O_{cam} - t) $$ + +3. **求解世界坐标**： + - 构建世界坐标系下的激光射线方程：$L(s) = O_{world} + s \cdot \vec{d}_{world}$。 + - **平面约束**：已知目标平面在世界坐标系中满足 $Z_{world} = 0$。 + - **联立求解**：令射线的Z分量等于0，解出参数 $s$，代回方程求得 $X, Y$。 + - **最终输出**：激光点在矩形平面上的世界坐标 $(x, y, 0)$。 diff --git a/vision_example.py b/scripts/vision_example.py similarity index 100% rename from vision_example.py rename to scripts/vision_example.py diff --git a/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py b/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py index 996beef..0488b30 100755 --- a/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py +++ b/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py @@ -5,12 +5,36 @@ import cv2 import rclpy from rclpy.node import Node from sensor_msgs.msg import Image -from geometry_msgs.msg import Point +from std_msgs.msg import Float32MultiArray from cv_bridge import CvBridge -# 配置项（与 vision_example.py 保持一致） +# Camera calibration (from calibrate.py) +K = np.array([ + [1247.0565667653996, 0.0, 753.8809622002652], + [0.0, 1256.6168638051222, 528.9954978706462], + [0.0, 0.0, 1.0] +]) +DIST = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]]) + +# Laser line parameters (from laser_calibrate.py) +D_CAM = np.array([-0.00989344622939825, 0.015559548512101173, 0.9998300075531006]) +O_CAM = np.array([-26.1895751953125, -3.379796028137207, 0.0]) + +# Rectangle physical dimensions (mm) +RECT_L = 305.0 +RECT_W = 215.0 +if RECT_W > RECT_L: + RECT_L, RECT_W = RECT_W, RECT_L + +# Image preprocessing +IMAGE_SCALE = 0.5 IMAGE_AREA = 1080 * 1440 / 4 -THRESHOLD_AREA = IMAGE_AREA * 0.01 +MIN_AREA_RATIO = 0.01 +THRESHOLD_AREA = IMAGE_AREA * MIN_AREA_RATIO + +# Reference values (center of rectangle, normalized) +REF_X = (RECT_L / 2.0) / RECT_L # 0.5 +REF_Y = (RECT_W / 2.0) / RECT_W # 0.5 class VisionNode(Node): @@ -18,6 +42,13 @@ class VisionNode(Node): super().__init__('vision_node') self._bridge = CvBridge() + # Scale intrinsics to match IMAGE_SCALE + self._K_scaled = K.copy() + self._K_scaled[0, 0] *= IMAGE_SCALE + self._K_scaled[1, 1] *= IMAGE_SCALE + self._K_scaled[0, 2] *= IMAGE_SCALE + self._K_scaled[1, 2] *= IMAGE_SCALE + self._subscription = self.create_subscription( Image, '/camera/image_raw', self._image_callback, 10 ) @@ -28,7 +59,7 @@ class VisionNode(Node): Image, '/vision/binary_image', 10 ) self._target_publisher = self.create_publisher( - Point, '/vision/target', 10 + Float32MultiArray, '/vision/target', 10 ) # GPU acceleration setup @@ -70,6 +101,93 @@ class VisionNode(Node): self._gaussian_filter.apply(gpu_gray, self._gpu_blurred) return self._gpu_blurred.download() + def _detect_rectangle(self, blurred): + """Otsu threshold + morphology close + contour fitting. + + Returns ordered corners (4, 2) float32 or None. + """ + _, binary = cv2.threshold( + blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU) + kernel = np.ones((3, 3), np.uint8) + closed = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel) + + contours, _ = cv2.findContours( + closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + best = None + best_area = 0 + for cnt in contours: + area = cv2.contourArea(cnt) + if area < THRESHOLD_AREA: + continue + epsilon = 0.02 * cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, epsilon, True) + if len(approx) == 4 and cv2.isContourConvex(approx): + if area > best_area: + best_area = area + best = approx.reshape(4, 2) + + return best.astype(np.float32) if best is not None else None, closed + + def _order_corners(self, image_corners): + """Order corners as TL, BL, BR, TR and build world points.""" + pts = image_corners.copy() + + edge_info = [] + for i in range(4): + j = (i + 1) % 4 + edge_info.append((i, j, np.linalg.norm(pts[i] - pts[j]))) + + edge_info.sort(key=lambda e: e[2]) + short_a, short_b = edge_info[0], edge_info[1] + + def mid_x(edge): + i, j, _ = edge + return (pts[i][0] + pts[j][0]) / 2.0 + + if mid_x(short_a) < mid_x(short_b): + left_edge, right_edge = short_a, short_b + else: + left_edge, right_edge = short_b, short_a + + li, lj, _ = left_edge + if pts[li][1] < pts[lj][1]: + TL_idx, BL_idx = li, lj + else: + TL_idx, BL_idx = lj, li + + ri, rj, _ = right_edge + if pts[ri][1] < pts[rj][1]: + TR_idx, BR_idx = ri, rj + else: + TR_idx, BR_idx = rj, ri + + ordered = pts[[TL_idx, BL_idx, BR_idx, TR_idx]] + world = np.array([ + [0, 0], + [0, RECT_W], + [RECT_L, RECT_W], + [RECT_L, 0], + ], dtype=np.float32) + + return ordered, world + + def _compute_laser_world(self, rvec, tvec): + """Compute laser point world coordinates on Z=0 plane.""" + R, _ = cv2.Rodrigues(rvec) + R_inv = R.T + t = tvec.ravel() + + d_world = R_inv @ D_CAM + O_world = R_inv @ (O_CAM - t) + + if abs(d_world[2]) < 1e-10: + return None + + s = -O_world[2] / d_world[2] + P_world = O_world + s * d_world + return P_world[:2] + def _image_callback(self, msg): try: encoding = msg.encoding @@ -81,87 +199,126 @@ class VisionNode(Node): gauss_image = self._preprocess(cv_image) - # Otsu 二值化 - _, binary = cv2.threshold( - gauss_image, 0, 255, - cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU) + corners, binary = self._detect_rectangle(gauss_image) + if corners is None: + # Publish binary image even when no rectangle found + binary_color = cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR) + binary_msg = self._bridge.cv2_to_imgmsg( + binary_color, encoding='bgr8') + binary_msg.header.stamp = msg.header.stamp + binary_msg.header.frame_id = msg.header.frame_id + self._binary_publisher.publish(binary_msg) - # 形态学闭运算 - kernel = np.ones((3, 3), np.uint8) - dilated = cv2.dilate(binary, kernel, iterations=1) - eroded = cv2.erode(dilated, kernel, iterations=1) + # Publish result image with FPS overlay + result_image = cv_image.copy() + fps = cv2.getTickFrequency() / max( + cv2.getTickCount() - self.start_time, 1) + cv2.putText(result_image, f'FPS: {fps:.2f}', (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + self.start_time = cv2.getTickCount() + result_msg = self._bridge.cv2_to_imgmsg( + result_image, encoding='bgr8') + result_msg.header.stamp = msg.header.stamp + result_msg.header.frame_id = msg.header.frame_id + self._result_publisher.publish(result_msg) + return - # 查找轮廓 - contours, _ = cv2.findContours( - eroded, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE + ordered_corners, world_pts = self._order_corners(corners) + world_pts_3d = np.hstack( + [world_pts, np.zeros((4, 1), dtype=np.float32)]) + + success, rvec, tvec = cv2.solvePnP( + world_pts_3d, ordered_corners, self._K_scaled, DIST, + flags=cv2.SOLVEPNP_IPPE ) result_image = cv_image.copy() - best_target = None - max_area = 0 - for cnt in contours: - # 绘制原始轮廓，调试用 - cnt_scaled = (cnt / 0.5).astype(np.int32) - cv2.drawContours(result_image, [cnt_scaled], -1, (0, 255, 0), 1) + scale_up = 1.0 / IMAGE_SCALE - # 面积过滤 - area = cv2.contourArea(cnt) - if area < THRESHOLD_AREA: - continue + if success: + laser_world = self._compute_laser_world(rvec, tvec) - # 形状近似：四边形 + 凸多边形 - epsilon = 0.02 * cv2.arcLength(cnt, True) - approx = cv2.approxPolyDP(cnt, epsilon, True) + # Draw ordered corners (scaled up to original resolution) + corners_scaled = (ordered_corners * scale_up).astype(np.int32) + labels = ["TL", "BL", "BR", "TR"] + colors = [(0, 0, 255), (255, 0, 0), (0, 255, 0), (255, 255, 0)] + for i, (pt, label) in enumerate(zip(corners_scaled, labels)): + cv2.circle(result_image, tuple(pt), 6, colors[i], -1) + cv2.putText(result_image, f"{label}", (pt[0] + 10, pt[1] - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, colors[i], 2) - if len(approx) == 4 and cv2.isContourConvex(approx): - # 坐标映射回原始分辨率 - approx_scaled = (approx / 0.5).astype(np.int32) - cv2.drawContours(result_image, [approx_scaled], -1, - (0, 0, 255), 2) - for point in approx_scaled: - cv2.circle(result_image, tuple(point[0]), 5, - (255, 0, 0), -1) + for i in range(4): + j = (i + 1) % 4 + cv2.line(result_image, tuple(corners_scaled[i]), + tuple(corners_scaled[j]), (255, 255, 255), 2) - if area > max_area: - max_area = area - best_target = approx + # Draw axis + axis_len = 50 + axis_3d = np.float32( + [[0, 0, 0], [axis_len, 0, 0], [0, axis_len, 0], + [0, 0, axis_len]]) + imgpts, _ = cv2.projectPoints( + axis_3d, rvec, tvec, self._K_scaled, DIST) + origin = tuple( + (imgpts[0].ravel() * scale_up).astype(int)) + cv2.line(result_image, origin, + tuple((imgpts[1].ravel() * scale_up).astype(int)), + (0, 0, 255), 3) + cv2.line(result_image, origin, + tuple((imgpts[2].ravel() * scale_up).astype(int)), + (0, 255, 0), 3) + cv2.line(result_image, origin, + tuple((imgpts[3].ravel() * scale_up).astype(int)), + (255, 0, 0), 3) - if best_target is not None: - moments = cv2.moments(best_target) - if moments['m00'] > 0: - cx = moments['m10'] / moments['m00'] - cy = moments['m01'] / moments['m00'] - h, w = gauss_image.shape[:2] - error_x = (cx - w / 2) / (w / 2) - error_y = (cy - h / 2) / (h / 2) + # Compute and publish normalized measurement + reference + if laser_world is not None: + measure_x = float(laser_world[0] / RECT_L) + measure_y = float(laser_world[1] / RECT_W) - target_msg = Point() - target_msg.x = error_x - target_msg.y = -error_y - target_msg.z = 0.0 + target_msg = Float32MultiArray() + target_msg.data = [ + measure_x, REF_X, measure_y, REF_Y] self._target_publisher.publish(target_msg) - # FPS 叠加 - fps = cv2.getTickFrequency() / (cv2.getTickCount() - self.start_time) - cv2.putText( - result_image, f'FPS: {fps:.2f}', - (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2 - ) + # Draw laser point (project to image) + P_world_3d = np.array( + [[laser_world[0], laser_world[1], 0]], + dtype=np.float32) + px_proj, _ = cv2.projectPoints( + P_world_3d, rvec, tvec, self._K_scaled, DIST) + lx = int(px_proj.ravel()[0] * scale_up) + ly = int(px_proj.ravel()[1] * scale_up) + cv2.circle(result_image, (lx, ly), 10, (0, 0, 255), 2) + cv2.circle(result_image, (lx, ly), 2, (0, 0, 255), -1) + else: + # PnP failed — still draw detected corners + corners_scaled = (ordered_corners * scale_up).astype(np.int32) + for pt in corners_scaled: + cv2.circle(result_image, tuple(pt), 6, (0, 165, 255), -1) + for i in range(4): + j = (i + 1) % 4 + cv2.line(result_image, tuple(corners_scaled[i]), + tuple(corners_scaled[j]), (0, 165, 255), 2) + + # FPS overlay + fps = cv2.getTickFrequency() / max( + cv2.getTickCount() - self.start_time, 1) + cv2.putText(result_image, f'FPS: {fps:.2f}', (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) self.start_time = cv2.getTickCount() - # 发布结果图像（带轮廓标注） + # Publish result image result_msg = self._bridge.cv2_to_imgmsg( - result_image, encoding='bgr8' - ) + result_image, encoding='bgr8') result_msg.header.stamp = msg.header.stamp result_msg.header.frame_id = msg.header.frame_id self._result_publisher.publish(result_msg) - # 发布二值化图像（调试用） - binary_color = cv2.cvtColor(eroded, cv2.COLOR_GRAY2BGR) + # Publish binary image + binary_color = cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR) binary_msg = self._bridge.cv2_to_imgmsg( - binary_color, encoding='bgr8' - ) + binary_color, encoding='bgr8') binary_msg.header.stamp = msg.header.stamp binary_msg.header.frame_id = msg.header.frame_id self._binary_publisher.publish(binary_msg) diff --git a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py index 1bf7225..f625027 100644 --- a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py +++ b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py @@ -5,8 +5,8 @@ import json import serial import rclpy from rclpy.node import Node -from geometry_msgs.msg import Twist, Point -from std_msgs.msg import String +from geometry_msgs.msg import Twist +from std_msgs.msg import Float32MultiArray, String CMD_AUTO = 0x01 CMD_MANUAL = 0x02 @@ -42,7 +42,7 @@ class GimbalNode(Node): Twist, '/cmd_gimbal', self._cmd_gimbal_callback, 10 ) self._target_sub = self.create_subscription( - Point, '/vision/target', self._target_callback, 10 + Float32MultiArray, '/vision/target', self._target_callback, 10 ) self._pid_config_sub = self.create_subscription( String, '/pid_config', self._pid_config_callback, 10 @@ -80,15 +80,22 @@ class GimbalNode(Node): data.append(_checksum(data)) self._write_frame(data) - def _target_callback(self, msg: Point): + def _target_callback(self, msg: Float32MultiArray): if self._current_mode != 'auto': return + if len(msg.data) < 4: + self.get_logger().error( + f'Target data requires 4 values, got {len(msg.data)}') + return + measure_x, ref_x, measure_y, ref_y = msg.data[:4] self.get_logger().debug( - f'Auto tracking: error_x={msg.x:.4f}, error_y={msg.y:.4f}' + f'Auto tracking: measure=({measure_x:.4f},{measure_y:.4f}) ' + f'ref=({ref_x:.4f},{ref_y:.4f})' ) data = bytearray() data.append(0xA5) - data.extend(struct.pack('