# -- 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()