dev01
fd3d6b7ab5
- 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
540 lines
18 KiB
Python
540 lines
18 KiB
Python
# -- 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()
|