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