feat: add PnP laser solving and update 0xA5 frame to 4-float protocol
- Integrate PnP rectangle pose estimation + laser world coordinate computation from scripts/calibrate/laser_pnp.py - Publish normalized measurement (laser/RECT) and reference (rect center) via Float32MultiArray - Update gimbal 0xA5 frame to 18-byte format: measure_x, ref_x, measure_y, ref_y - Add camera calibration, laser line params, and RECT_L/W constants - Add corner ordering (TL/BL/BR/TR), SOLVEPNP_IPPE, and pose/laser visualization overlay - Remove standalone vision_example.py (superseded by ROS2 nodes) - Add scripts/calibrate/ with calibration and PnP tools
This commit is contained in:
dev01
@@ -0,0 +1,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()
|
||||
Reference in New Issue
Block a user