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
570 lines
18 KiB
Python
570 lines
18 KiB
Python
# -- 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()
|