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,109 @@
|
||||
# 相机标定与激光点解算
|
||||
|
||||
## 文件说明
|
||||
|
||||
| 文件 | 用途 |
|
||||
|------|------|
|
||||
| `calibrate.py` | 相机内参标定，交互式拍照，输出 `calibration_result.json` |
|
||||
| `laser_calibrate.py` | 激光器-相机联合标定，拟合相机坐标系下的激光直线，输出 `laser_result.json` |
|
||||
| `pnp_solve.py` | 矩形检测 + PnP 位姿解算，实时显示相机位姿 |
|
||||
| `laser_pnp.py` | 矩形检测 + PnP 位姿解算 + 激光点世界坐标计算 |
|
||||
|
||||
## 棋盘格规格
|
||||
|
||||
- 方格边长：**15 mm**
|
||||
- 内角点数：**11 列 x 8 行**（12x9 个方格）
|
||||
|
||||
---
|
||||
|
||||
## 工作流程
|
||||
|
||||
### 第一步：相机内参标定
|
||||
|
||||
```bash
|
||||
python calibrate.py
|
||||
```
|
||||
|
||||
1. 输入是否为无畸变镜头（y/n），然后输入拍摄数量（建议 15~20 张）
|
||||
2. 将棋盘格以不同角度/位置放在相机视野中
|
||||
3. 画面显示绿色 **CHESSBOARD DETECTED** 后，输入 `y` 并回车拍照
|
||||
4. 图像自动保存到 `calibration_images/` 目录
|
||||
5. 拍完后自动标定，输出内参矩阵 K 和畸变系数
|
||||
6. 结果保存为 `calibration_result.json`，同时终端打印完整 JSON
|
||||
|
||||
### 第二步（可选）：激光器-相机联合标定
|
||||
|
||||
> 需要开启激光，激光点投射在标定板上
|
||||
|
||||
```bash
|
||||
python laser_calibrate.py
|
||||
```
|
||||
|
||||
1. 自动加载 `calibration_result.json`
|
||||
2. 输入拍摄数量（建议 >= 10 张），标定板以不同姿态摆放
|
||||
3. 画面显示绿色棋盘格角点 + 红色激光点圆圈后，输入 `y` 拍照
|
||||
4. 每张图像：PnP 解算标定板位姿 -> 提取激光点像素 -> 反投影求交 -> 得到相机坐标系 3D 点
|
||||
5. 图像自动保存到 `laser_calib_images/` 目录
|
||||
6. 采集完成后 PCA 拟合激光直线，输出：
|
||||
- `d_cam`：激光方向向量（相机坐标系）
|
||||
- `O_cam`：基准起点（直线与相机 Z=0 平面的交点）
|
||||
7. 结果保存为 `laser_result.json`，同时终端打印完整 JSON
|
||||
|
||||
### 第三步：在线运行
|
||||
|
||||
**仅 PnP 位姿解算：**
|
||||
|
||||
```bash
|
||||
python pnp_solve.py
|
||||
```
|
||||
|
||||
将 `calibrate.py` 输出的 JSON 粘贴到文件顶部的 `CALIB_JSON` 变量中，修改 `RECT_L`/`RECT_W` 为实际矩形尺寸。
|
||||
|
||||
**PnP 位姿 + 激光点世界坐标：**
|
||||
|
||||
```bash
|
||||
python laser_pnp.py
|
||||
```
|
||||
|
||||
将 `calibrate.py` 输出的 JSON 粘贴到 `CALIB_JSON`，将 `laser_calibrate.py` 输出的 JSON 粘贴到 `LASER_JSON`，修改 `RECT_L`/`RECT_W`。
|
||||
|
||||
画面实时显示：
|
||||
- 矩形四个角点标注（TL/BL/BR/TR）
|
||||
- 右手坐标系坐标轴（X 红 / Y 绿 / Z 蓝）
|
||||
- 相机平移向量 T 和旋转欧拉角 R
|
||||
- 激光点世界坐标 (x, y)（仅在 `laser_pnp.py` 中）
|
||||
|
||||
---
|
||||
|
||||
## 内参矩阵缩放
|
||||
|
||||
内参矩阵 K 的单位是像素，与图像分辨率绑定。畸变系数（`dist`）是无量纲的，不随分辨率变化。
|
||||
|
||||
等比例缩放因子 $s = W_{\text{new}} / W_{\text{orig}}$：
|
||||
|
||||
```python
|
||||
K_scaled = K.copy()
|
||||
K_scaled[0, 0] *= s # fx
|
||||
K_scaled[1, 1] *= s # fy
|
||||
K_scaled[0, 2] *= s # cx
|
||||
K_scaled[1, 2] *= s # cy
|
||||
```
|
||||
|
||||
对应的 `pnp_solve.py` 和 `laser_pnp.py` 通过 `IMAGE_SCALE` 常量自动处理缩放。
|
||||
|
||||
---
|
||||
|
||||
## 坐标系约定
|
||||
|
||||
**世界坐标系**（矩形平面，Z=0）：
|
||||
```
|
||||
TL=(0,0) ──── TR=(l,0)
|
||||
│ │
|
||||
BL=(0,w) ──── BR=(l,w)
|
||||
```
|
||||
|
||||
- X 轴沿矩形长边，向右
|
||||
- Y 轴沿矩形宽边，向下
|
||||
- Z 轴由右手定则确定：X x Y = Z，指向观察者
|
||||
|
||||
**相机坐标系**：OpenCV 标准右手系，X 右、Y 下、Z 前（指向场景）。
|
||||
@@ -0,0 +1,504 @@
|
||||
# -- coding: utf-8 --
|
||||
"""
|
||||
交互式相机内参标定程序
|
||||
使用方格标定板（15mm方格边长），实时预览，终端交互拍照，自动标定内参矩阵。
|
||||
"""
|
||||
|
||||
import sys
|
||||
import platform
|
||||
import os
|
||||
from ctypes import *
|
||||
import numpy as np
|
||||
import cv2
|
||||
import json
|
||||
|
||||
# ===== 标定参数配置 =====
|
||||
SQUARE_SIZE = 15.0 # 方格边长 (mm)
|
||||
CHESSBOARD_COLS = 11 # 内角点列数
|
||||
CHESSBOARD_ROWS = 8 # 内角点行数
|
||||
DETECT_INTERVAL = 3 # 每隔多少帧检测一次棋盘格（减少CPU负载）
|
||||
# =======================
|
||||
|
||||
currentsystem = platform.system()
|
||||
print(f"current system: {currentsystem}")
|
||||
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_capture_flag = False
|
||||
g_captured_frames = []
|
||||
g_image_dir = "calibration_images"
|
||||
g_frame_count = 0
|
||||
g_cb_detected = False # 当前帧是否检测到棋盘格
|
||||
g_cb_corners_display = None # 角点坐标（显示图像坐标系）
|
||||
|
||||
stFrame = POINTER(MV_FRAME_OUT)
|
||||
fun_ctype = get_platform_functype()
|
||||
FrameInfoCallBack2 = fun_ctype(None, stFrame, c_void_p, c_bool)
|
||||
|
||||
|
||||
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
|
||||
else:
|
||||
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
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
def image_callback(pstFrame, pUser, bAutoFree):
|
||||
global g_capture_flag, g_captured_frames, g_frame_count
|
||||
global g_cb_detected, g_cb_corners_display
|
||||
|
||||
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:
|
||||
print("HB Decode fail! ret[0x%x]" % ret)
|
||||
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:
|
||||
print("convert pixel fail! ret[0x%x]" % ret)
|
||||
break
|
||||
|
||||
# 构建全分辨率图像
|
||||
if nChannelNum == 1:
|
||||
numpy_image = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape(
|
||||
stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth)
|
||||
display_image = cv2.cvtColor(numpy_image, cv2.COLOR_GRAY2BGR)
|
||||
else:
|
||||
numpy_image = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape(
|
||||
stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth, 3)
|
||||
display_image = cv2.resize(numpy_image, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA)
|
||||
|
||||
# 每隔 DETECT_INTERVAL 帧在显示图像上检测棋盘格（速度快）
|
||||
if g_frame_count % DETECT_INTERVAL == 0:
|
||||
gray_display = cv2.cvtColor(display_image, cv2.COLOR_BGR2GRAY) if nChannelNum == 3 else numpy_image
|
||||
ret_cb, corners_disp = cv2.findChessboardCorners(
|
||||
gray_display, (CHESSBOARD_COLS, CHESSBOARD_ROWS),
|
||||
cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
)
|
||||
g_cb_detected = ret_cb
|
||||
if ret_cb:
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
g_cb_corners_display = cv2.cornerSubPix(
|
||||
gray_display, corners_disp, (5, 5), (-1, -1), criteria)
|
||||
else:
|
||||
g_cb_corners_display = None
|
||||
else:
|
||||
ret_cb = g_cb_detected
|
||||
|
||||
# 在显示图像上绘制检测结果
|
||||
if g_cb_detected and g_cb_corners_display is not None:
|
||||
cv2.drawChessboardCorners(display_image, (CHESSBOARD_COLS, CHESSBOARD_ROWS),
|
||||
g_cb_corners_display, True)
|
||||
|
||||
# 处理拍照指令（保存全分辨率图像，标定时会重新精确检测角点）
|
||||
if g_capture_flag:
|
||||
if g_cb_detected:
|
||||
g_captured_frames.append(numpy_image.copy())
|
||||
idx = len(g_captured_frames)
|
||||
path = os.path.join(g_image_dir, f"calib_{idx:02d}.png")
|
||||
cv2.imwrite(path, numpy_image)
|
||||
print(f" [OK] 已捕获第 {idx} 张图像 -> {path}")
|
||||
g_capture_flag = False
|
||||
# 若未检测到棋盘格，保持标志等待下一帧
|
||||
|
||||
# 绘制状态信息
|
||||
status_text = f"Captured: {len(g_captured_frames)}"
|
||||
if g_cb_detected:
|
||||
cv2.putText(display_image, "CHESSBOARD DETECTED", (10, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
||||
if g_capture_flag and not g_cb_detected:
|
||||
cv2.putText(display_image, "Waiting for chessboard...", (10, 60),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
|
||||
|
||||
cv2.putText(display_image, status_text, (10, display_image.shape[0] - 10),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
|
||||
|
||||
cv2.imshow('Calibration - Live View', display_image)
|
||||
cv2.waitKey(1)
|
||||
|
||||
g_frame_count += 1
|
||||
break
|
||||
|
||||
if bAutoFree == False:
|
||||
if pUser != None:
|
||||
User.MV_CC_FreeImageBuffer(stFrame)
|
||||
else:
|
||||
print("user is null, invalid")
|
||||
|
||||
|
||||
CALL_BACK_FUN = FrameInfoCallBack2(image_callback)
|
||||
|
||||
|
||||
def run_calibration(distortion_free=False):
|
||||
"""使用已捕获的全分辨率图像进行相机内参标定"""
|
||||
if len(g_captured_frames) == 0:
|
||||
print("错误：没有捕获到任何图像！")
|
||||
return
|
||||
|
||||
print("\n正在对全分辨率图像进行亚像素角点检测...")
|
||||
|
||||
objp = np.zeros((CHESSBOARD_COLS * CHESSBOARD_ROWS, 3), np.float32)
|
||||
objp[:, :2] = np.mgrid[0:CHESSBOARD_COLS, 0:CHESSBOARD_ROWS].T.reshape(-1, 2) * SQUARE_SIZE
|
||||
|
||||
objpoints = []
|
||||
imgpoints = []
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
|
||||
for i, img in enumerate(g_captured_frames):
|
||||
if len(img.shape) == 2:
|
||||
gray = img
|
||||
elif img.shape[2] == 1:
|
||||
gray = img[:, :, 0] if hasattr(img, 'shape') and len(img.shape) == 3 else img
|
||||
else:
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
if len(gray.shape) == 3:
|
||||
gray = cv2.cvtColor(gray, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
ret_cb, corners = cv2.findChessboardCorners(
|
||||
gray, (CHESSBOARD_COLS, CHESSBOARD_ROWS),
|
||||
cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
)
|
||||
|
||||
if ret_cb:
|
||||
corners_sub = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
objpoints.append(objp)
|
||||
imgpoints.append(corners_sub)
|
||||
print(f" 图像 {i + 1}/{len(g_captured_frames)}: 角点检测成功")
|
||||
else:
|
||||
print(f" 图像 {i + 1}/{len(g_captured_frames)}: 角点检测失败，跳过")
|
||||
|
||||
if len(objpoints) < 3:
|
||||
print(f"\n错误：有效角点图像不足（需要至少3张，当前{len(objpoints)}张）！")
|
||||
return
|
||||
|
||||
print(f"\n使用 {len(objpoints)} 张有效图像进行标定...")
|
||||
|
||||
img_size = gray.shape[::-1]
|
||||
|
||||
if distortion_free:
|
||||
flags = cv2.CALIB_ZERO_TANGENT_DIST | cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3
|
||||
print("畸变模型: 无畸变（所有畸变系数固定为 0）")
|
||||
else:
|
||||
flags = 0
|
||||
print("畸变模型: 自由估计（k1, k2, p1, p2, k3）")
|
||||
|
||||
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
|
||||
objpoints, imgpoints, img_size, None, None, flags=flags
|
||||
)
|
||||
|
||||
# 计算平均重投影误差
|
||||
mean_error = 0
|
||||
for i in range(len(objpoints)):
|
||||
projected, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
|
||||
error = cv2.norm(imgpoints[i], projected, cv2.NORM_L2) / len(projected)
|
||||
mean_error += error
|
||||
mean_error /= len(objpoints)
|
||||
|
||||
print("\n" + "=" * 55)
|
||||
print(" 相机内参标定结果")
|
||||
print("=" * 55)
|
||||
print(f"\n 标定图像数: {len(objpoints)}")
|
||||
print(f" 图像分辨率: {img_size[0]} x {img_size[1]}")
|
||||
print(f" 方格边长: {SQUARE_SIZE} mm")
|
||||
print(f"\n 内参矩阵 K (pixels):")
|
||||
print(f" [{mtx[0,0]:10.4f} {mtx[0,1]:10.4f} {mtx[0,2]:10.4f}]")
|
||||
print(f" [{mtx[1,0]:10.4f} {mtx[1,1]:10.4f} {mtx[1,2]:10.4f}]")
|
||||
print(f" [{mtx[2,0]:10.4f} {mtx[2,1]:10.4f} {mtx[2,2]:10.4f}]")
|
||||
print(f"\n 畸变系数 (k1, k2, p1, p2, k3):")
|
||||
print(f" {dist.ravel()}")
|
||||
print(f"\n 平均重投影误差: {mean_error:.4f} pixels")
|
||||
print("=" * 55)
|
||||
|
||||
# 保存为 JSON 文件
|
||||
save_path = "calibration_result.json"
|
||||
result = {
|
||||
"mtx": mtx.tolist(),
|
||||
"dist": dist.tolist(),
|
||||
"img_size": list(img_size),
|
||||
"square_size": SQUARE_SIZE,
|
||||
"error": float(mean_error),
|
||||
}
|
||||
with open(save_path, "w") as f:
|
||||
json.dump(result, f, indent=2)
|
||||
print(f"\n结果已保存到: {save_path}")
|
||||
|
||||
# 打印 JSON 内容，方便直接粘贴到 pnp_solve.py
|
||||
print(f"\n--- 复制以下 JSON 到 pnp_solve.py 的 CALIB_JSON 变量中 ---")
|
||||
print(json.dumps(result, indent=2))
|
||||
print(f"----------------------------------------------------------")
|
||||
|
||||
return mtx, dist
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print("=" * 55)
|
||||
print(" 交互式相机内参标定程序")
|
||||
print(f" 棋盘格: {CHESSBOARD_COLS}x{CHESSBOARD_ROWS} 内角点")
|
||||
print(f" 方格边长: {SQUARE_SIZE} mm")
|
||||
print("=" * 55)
|
||||
print()
|
||||
|
||||
# 输入拍照数量
|
||||
while True:
|
||||
try:
|
||||
n_photos = int(input_func("请输入要拍摄的标定图像数量: "))
|
||||
if n_photos >= 3:
|
||||
break
|
||||
print("至少需要3张图像！")
|
||||
except ValueError:
|
||||
print("请输入有效数字！")
|
||||
|
||||
# 是否为无畸变镜头
|
||||
while True:
|
||||
ans = input_func("是否为无畸变镜头？(y/n): ").strip().lower()
|
||||
if ans in ('y', 'n'):
|
||||
break
|
||||
print("请输入 y 或 n")
|
||||
use_distortion_free = (ans == 'y')
|
||||
if use_distortion_free:
|
||||
print("将使用无畸变模型（畸变系数固定为 0）")
|
||||
else:
|
||||
print("将自由估计畸变系数")
|
||||
|
||||
print(f"\n将拍摄 {n_photos} 张图像。")
|
||||
print("操作说明：")
|
||||
print(" 1. 将棋盘格标定板以不同角度/位置放在相机视野中")
|
||||
print(" 2. 看到绿色 'CHESSBOARD DETECTED' 提示后，输入 y 并回车拍照")
|
||||
print(" 3. 重复直到拍完所有图像")
|
||||
print()
|
||||
|
||||
# ===== 初始化相机 =====
|
||||
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_GIGE_DEVICE or mvcc_dev_info.nTLayerType == MV_GENTL_GIGE_DEVICE:
|
||||
print("\ngige device: [%d]" % i)
|
||||
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName if c != 0])
|
||||
print("device model name: %s" % strModeName)
|
||||
nip1 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24)
|
||||
nip2 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16)
|
||||
nip3 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8)
|
||||
nip4 = (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff)
|
||||
print("current ip: %d.%d.%d.%d\n" % (nip1, nip2, nip3, nip4))
|
||||
elif mvcc_dev_info.nTLayerType == MV_USB_DEVICE:
|
||||
print("\nu3v device: [%d]" % i)
|
||||
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName if c != 0])
|
||||
print("device model name: %s" % strModeName)
|
||||
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chSerialNumber if c != 0])
|
||||
print("user serial number: %s" % strSerialNumber)
|
||||
|
||||
nConnectionNum = 0
|
||||
if int(nConnectionNum) >= deviceList.nDeviceNum:
|
||||
print("intput error!")
|
||||
sys.exit()
|
||||
|
||||
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:
|
||||
ret = cam.MV_CC_SetIntValue("GevSCPSPacketSize", nPacketSize)
|
||||
if ret != 0:
|
||||
print("Warning: Set Packet Size fail! ret[0x%x]" % ret)
|
||||
else:
|
||||
print("Warning: Get Packet Size fail! ret[0x%x]" % nPacketSize)
|
||||
|
||||
ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF)
|
||||
if ret != 0:
|
||||
print("set trigger mode fail! ret[0x%x]" % ret)
|
||||
sys.exit()
|
||||
|
||||
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()
|
||||
|
||||
print("相机取流已启动。\n")
|
||||
|
||||
# 创建保存图像的目录
|
||||
os.makedirs(g_image_dir, exist_ok=True)
|
||||
print(f"标定图像将保存到: {g_image_dir}/\n")
|
||||
|
||||
# ===== 交互式拍照循环 =====
|
||||
try:
|
||||
while len(g_captured_frames) < n_photos:
|
||||
remaining = n_photos - len(g_captured_frames)
|
||||
cmd = input_func(f"[{len(g_captured_frames)}/{n_photos}] 输入 y 并回车拍照 (剩余 {remaining} 张): ")
|
||||
|
||||
if cmd.strip().lower() == 'y':
|
||||
g_capture_flag = True
|
||||
# 等待回调捕获（每50ms检查一次，最多等5秒）
|
||||
waited = 0
|
||||
while g_capture_flag and waited < 100:
|
||||
cv2.waitKey(50)
|
||||
waited += 1
|
||||
if g_capture_flag:
|
||||
print(" 警告: 超时未检测到棋盘格角点，请调整标定板位置后重试")
|
||||
g_capture_flag = False
|
||||
elif cmd.strip().lower() == 'q':
|
||||
print("用户取消操作。")
|
||||
break
|
||||
else:
|
||||
print(" 请输入 y (拍照) 或 q (退出)")
|
||||
except KeyboardInterrupt:
|
||||
print("\n用户中断。")
|
||||
|
||||
# ===== 停止取流 =====
|
||||
print("\n停止相机取流...")
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
ret = cam.MV_CC_StopGrabbing()
|
||||
if ret != 0:
|
||||
print("stop grabbing fail! ret[0x%x]" % ret)
|
||||
|
||||
ret = cam.MV_CC_CloseDevice()
|
||||
if ret != 0:
|
||||
print("close device fail! ret[0x%x]" % ret)
|
||||
|
||||
ret = cam.MV_CC_DestroyHandle()
|
||||
if ret != 0:
|
||||
print("destroy handle fail! ret[0x%x]" % ret)
|
||||
|
||||
MvCamera.MV_CC_Finalize()
|
||||
|
||||
# ===== 执行标定 =====
|
||||
if len(g_captured_frames) > 0:
|
||||
run_calibration(distortion_free=use_distortion_free)
|
||||
else:
|
||||
print("\n没有捕获到图像，跳过标定。")
|
||||
@@ -0,0 +1,532 @@
|
||||
# -- coding: utf-8 --
|
||||
"""
|
||||
激光器-相机联合标定程序
|
||||
使用方格标定板 + 激光点，拍摄多组图像，拟合相机坐标系下的激光直线方程。
|
||||
|
||||
流程：
|
||||
1. 加载相机内参 (calibration_result.json)
|
||||
2. 实时预览，棋盘格 + 激光点同时可见时按 y 拍照
|
||||
3. 每张照片：PnP 解算标定板位姿 → 提取激光点像素 → 反投影求交 → 得到相机坐标系 3D 点
|
||||
4. 收集所有 3D 点，PCA 拟合直线
|
||||
5. 输出：方向向量 d_cam + 基准起点 O_cam (与相机 Z=0 平面的交点)
|
||||
"""
|
||||
|
||||
import sys
|
||||
import platform
|
||||
import os
|
||||
from ctypes import *
|
||||
import numpy as np
|
||||
import cv2
|
||||
import json
|
||||
|
||||
# ===== 标定板参数（与 calibrate.py 保持一致） =====
|
||||
SQUARE_SIZE = 15.0 # 方格边长 (mm)
|
||||
CHESSBOARD_COLS = 11 # 内角点列数
|
||||
CHESSBOARD_ROWS = 8 # 内角点行数
|
||||
DETECT_INTERVAL = 3 # 棋盘格检测帧间隔
|
||||
# ================================================
|
||||
|
||||
# ===== 激光点检测参数（蓝紫色激光，HSV 色彩空间） =====
|
||||
LASER_HSV_LOWER = (100, 50, 50) # H, S, V 下限
|
||||
LASER_HSV_UPPER = (160, 255, 255) # H, S, V 上限
|
||||
# ===================================================
|
||||
|
||||
currentsystem = platform.system()
|
||||
print(f"current system: {currentsystem}")
|
||||
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_capture_flag = False
|
||||
g_laser_points_3d = [] # 收集的相机坐标系 3D 激光点
|
||||
g_image_dir = "laser_calib_images"
|
||||
g_frame_count = 0
|
||||
g_cb_detected = False
|
||||
g_cb_corners_display = None
|
||||
g_laser_display = None # 激光点像素坐标（显示用）
|
||||
g_K = None
|
||||
g_dist = None
|
||||
|
||||
stFrame = POINTER(MV_FRAME_OUT)
|
||||
fun_ctype = get_platform_functype()
|
||||
FrameInfoCallBack2 = fun_ctype(None, stFrame, c_void_p, c_bool)
|
||||
|
||||
|
||||
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 detect_laser_spot(bgr):
|
||||
"""检测蓝紫色激光点：HSV 色彩空间过滤，返回亚像素质心坐标，以及二值 mask"""
|
||||
hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
|
||||
mask = cv2.inRange(hsv, LASER_HSV_LOWER, LASER_HSV_UPPER)
|
||||
# 膨胀连接断裂区域
|
||||
mask = cv2.dilate(mask, np.ones((3, 3), np.uint8), iterations=1)
|
||||
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
if not contours:
|
||||
return None, None
|
||||
|
||||
largest = max(contours, key=cv2.contourArea)
|
||||
M = cv2.moments(largest)
|
||||
if M["m00"] == 0:
|
||||
return None, None
|
||||
cx = M["m10"] / M["m00"]
|
||||
cy = M["m01"] / M["m00"]
|
||||
return np.array([cx, cy], dtype=np.float32), mask
|
||||
|
||||
|
||||
def inpaint_laser(gray, laser_mask):
|
||||
"""用 inpainting 填补激光区域，避免激光遮盖棋盘格角点"""
|
||||
if laser_mask is None:
|
||||
return gray
|
||||
# 膨胀 mask 留出安全边距
|
||||
kernel = np.ones((7, 7), np.uint8)
|
||||
mask_dilated = cv2.dilate(laser_mask, kernel, iterations=2)
|
||||
return cv2.inpaint(gray, mask_dilated, 3, cv2.INPAINT_TELEA)
|
||||
|
||||
|
||||
def backproject_to_plane(laser_px, rvec, tvec, K):
|
||||
"""
|
||||
将激光点像素坐标反投影到标定板平面，返回相机坐标系下的 3D 点。
|
||||
|
||||
标定板平面在相机坐标系中: n · (X - t) = 0, 其中 n = R[:, 2]
|
||||
相机射线: P(s) = s * d, 其中 d = K^{-1} * [u, v, 1]^T
|
||||
求交: s = (n · t) / (n · d)
|
||||
"""
|
||||
R, _ = cv2.Rodrigues(rvec)
|
||||
n = R[:, 2] # 标定板法向量（相机坐标系）
|
||||
t = tvec.ravel() # 标定板原点在相机坐标系的位置
|
||||
|
||||
d = np.linalg.inv(K) @ np.array([laser_px[0], laser_px[1], 1.0])
|
||||
d = d / np.linalg.norm(d) # 归一化方向
|
||||
|
||||
nd = np.dot(n, d)
|
||||
if abs(nd) < 1e-8:
|
||||
return None # 射线与平面平行
|
||||
|
||||
s = np.dot(n, t) / nd
|
||||
if s <= 0:
|
||||
return None # 交点在相机后方
|
||||
|
||||
return (s * d).astype(np.float32)
|
||||
|
||||
|
||||
def solve_pnp_for_checkerboard(gray, K, dist):
|
||||
"""检测棋盘格并 PnP 解算标定板位姿"""
|
||||
ret_cb, corners = cv2.findChessboardCorners(
|
||||
gray, (CHESSBOARD_COLS, CHESSBOARD_ROWS),
|
||||
cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
)
|
||||
if not ret_cb:
|
||||
return None, None
|
||||
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
corners_sub = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
|
||||
objp = np.zeros((CHESSBOARD_COLS * CHESSBOARD_ROWS, 3), np.float32)
|
||||
objp[:, :2] = np.mgrid[0:CHESSBOARD_COLS, 0:CHESSBOARD_ROWS].T.reshape(-1, 2) * SQUARE_SIZE
|
||||
|
||||
success, rvec, tvec = cv2.solvePnP(objp, corners_sub, K, dist, flags=cv2.SOLVEPNP_IPPE)
|
||||
if not success:
|
||||
return None, None
|
||||
|
||||
return rvec, tvec
|
||||
|
||||
|
||||
def fit_3d_line(points):
|
||||
"""PCA 拟合空间直线，返回 (direction, centroid)"""
|
||||
pts = np.array(points)
|
||||
centroid = pts.mean(axis=0)
|
||||
centered = pts - centroid
|
||||
_, _, vh = np.linalg.svd(centered)
|
||||
direction = vh[0] # 第一主成分 = 直线方向
|
||||
# 确保方向指向 Z+ 半球
|
||||
if direction[2] < 0:
|
||||
direction = -direction
|
||||
return direction, centroid
|
||||
|
||||
|
||||
def image_callback(pstFrame, pUser, bAutoFree):
|
||||
global g_capture_flag, g_laser_points_3d, g_frame_count
|
||||
global g_cb_detected, g_cb_corners_display, g_laser_display
|
||||
global g_K, g_dist
|
||||
|
||||
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:
|
||||
print("HB Decode fail! ret[0x%x]" % ret)
|
||||
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:
|
||||
print("convert pixel fail! ret[0x%x]" % ret)
|
||||
break
|
||||
|
||||
if nChannelNum == 1:
|
||||
numpy_image = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape(
|
||||
stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth)
|
||||
gray_full = numpy_image
|
||||
display_image = cv2.cvtColor(numpy_image, cv2.COLOR_GRAY2BGR)
|
||||
else:
|
||||
numpy_image = np.frombuffer(DstBuffer, dtype=np.ubyte, count=DstBufferlen).reshape(
|
||||
stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth, 3)
|
||||
gray_full = cv2.cvtColor(numpy_image, cv2.COLOR_BGR2GRAY)
|
||||
display_image = cv2.resize(numpy_image, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA)
|
||||
|
||||
# 激光点检测（彩色图，HSV 色彩空间）
|
||||
if nChannelNum == 3:
|
||||
laser_px, laser_mask = detect_laser_spot(numpy_image)
|
||||
else:
|
||||
laser_px, laser_mask = None, None
|
||||
|
||||
# 用 inpainting 填补激光区域后再做棋盘格检测，避免激光遮盖角点
|
||||
if laser_mask is not None:
|
||||
gray_inpainted = inpaint_laser(gray_full, laser_mask)
|
||||
else:
|
||||
gray_inpainted = gray_full
|
||||
|
||||
if g_frame_count % DETECT_INTERVAL == 0:
|
||||
gray_disp = cv2.resize(gray_inpainted, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA) if nChannelNum == 3 else gray_inpainted
|
||||
ret_cb, corners_disp = cv2.findChessboardCorners(
|
||||
gray_disp, (CHESSBOARD_COLS, CHESSBOARD_ROWS),
|
||||
cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
)
|
||||
g_cb_detected = ret_cb
|
||||
if ret_cb:
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
g_cb_corners_display = cv2.cornerSubPix(gray_disp, corners_disp, (5, 5), (-1, -1), criteria)
|
||||
else:
|
||||
g_cb_corners_display = None
|
||||
|
||||
if laser_px is not None and nChannelNum == 3:
|
||||
g_laser_display = laser_px * 0.5
|
||||
elif laser_px is not None:
|
||||
g_laser_display = laser_px.copy()
|
||||
else:
|
||||
g_laser_display = None
|
||||
|
||||
# 拍照
|
||||
if g_capture_flag:
|
||||
if g_cb_detected and laser_px is not None and g_K is not None:
|
||||
# PnP 使用 inpainting 后的灰度图，避免激光干扰角点提取
|
||||
rvec, tvec = solve_pnp_for_checkerboard(gray_inpainted, g_K, g_dist)
|
||||
if rvec is not None:
|
||||
p3d = backproject_to_plane(laser_px, rvec, tvec, g_K)
|
||||
if p3d is not None:
|
||||
g_laser_points_3d.append(p3d)
|
||||
idx = len(g_laser_points_3d)
|
||||
path = os.path.join(g_image_dir, f"laser_{idx:02d}.png")
|
||||
cv2.imwrite(path, numpy_image)
|
||||
print(f" [OK] 第 {idx} 个激光点: "
|
||||
f"像素({laser_px[0]:.1f}, {laser_px[1]:.1f}) -> "
|
||||
f"3D({p3d[0]:.1f}, {p3d[1]:.1f}, {p3d[2]:.1f}) mm")
|
||||
print(f" 图像已保存: {path}")
|
||||
g_capture_flag = False
|
||||
else:
|
||||
print(" 警告: 激光点反投影失败")
|
||||
g_capture_flag = False
|
||||
else:
|
||||
print(" 警告: PnP 解算失败")
|
||||
g_capture_flag = False
|
||||
else:
|
||||
pass # 等待检测到棋盘格和激光点
|
||||
|
||||
# 绘制
|
||||
if g_cb_detected and g_cb_corners_display is not None:
|
||||
cv2.drawChessboardCorners(display_image, (CHESSBOARD_COLS, CHESSBOARD_ROWS),
|
||||
g_cb_corners_display, True)
|
||||
cv2.putText(display_image, "CHESSBOARD DETECTED", (10, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
||||
if g_laser_display is not None:
|
||||
lx, ly = int(g_laser_display[0]), int(g_laser_display[1])
|
||||
cv2.circle(display_image, (lx, ly), 2, (0, 0, 255), -1)
|
||||
cv2.circle(display_image, (lx, ly), 12, (0, 0, 255), 2)
|
||||
cv2.putText(display_image, "LASER", (lx + 15, ly - 10),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||||
|
||||
status = f"Points: {len(g_laser_points_3d)}"
|
||||
cv2.putText(display_image, status, (10, display_image.shape[0] - 10),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
|
||||
|
||||
cv2.imshow('Laser Calibration', display_image)
|
||||
cv2.waitKey(1)
|
||||
|
||||
g_frame_count += 1
|
||||
break
|
||||
|
||||
if bAutoFree == False:
|
||||
if pUser != None:
|
||||
User.MV_CC_FreeImageBuffer(stFrame)
|
||||
|
||||
|
||||
CALL_BACK_FUN = FrameInfoCallBack2(image_callback)
|
||||
|
||||
|
||||
def run_fitting():
|
||||
"""拟合激光直线并输出结果"""
|
||||
if len(g_laser_points_3d) < 3:
|
||||
print(f"错误：激光点数量不足（需要至少3个，当前{len(g_laser_points_3d)}个）！")
|
||||
return None, None
|
||||
|
||||
direction, centroid = fit_3d_line(g_laser_points_3d)
|
||||
|
||||
# 计算与相机 Z=0 平面的交点作为参考原点
|
||||
if abs(direction[2]) > 1e-8:
|
||||
t_z0 = -centroid[2] / direction[2]
|
||||
origin = centroid + t_z0 * direction
|
||||
else:
|
||||
origin = centroid.copy()
|
||||
print("警告：激光方向近似平行于相机 Z=0 平面，使用质心作为参考原点")
|
||||
|
||||
# 计算拟合残差
|
||||
centered = np.array(g_laser_points_3d) - centroid
|
||||
residuals = np.linalg.norm(centered - np.outer(np.dot(centered, direction), direction), axis=1)
|
||||
rms = np.sqrt(np.mean(residuals ** 2))
|
||||
|
||||
print("\n" + "=" * 55)
|
||||
print(" 激光直线标定结果")
|
||||
print("=" * 55)
|
||||
print(f"\n 采集点数: {len(g_laser_points_3d)}")
|
||||
print(f" RMS 拟合残差: {rms:.3f} mm")
|
||||
print(f"\n 直线参数 (相机坐标系):")
|
||||
print(f" 方向向量 d_cam: [{direction[0]:.6f}, {direction[1]:.6f}, {direction[2]:.6f}]")
|
||||
print(f" 基准起点 O_cam: [{origin[0]:.2f}, {origin[1]:.2f}, {origin[2]:.2f}] mm")
|
||||
print(f" (O_cam 为直线与相机 Z=0 平面的交点)")
|
||||
print("=" * 55)
|
||||
|
||||
# 保存
|
||||
result = {
|
||||
"direction": direction.tolist(),
|
||||
"origin": origin.tolist(),
|
||||
"num_points": len(g_laser_points_3d),
|
||||
"rms_error": float(rms),
|
||||
"points_3d": [p.tolist() for p in g_laser_points_3d],
|
||||
}
|
||||
save_path = "laser_result.json"
|
||||
with open(save_path, "w") as f:
|
||||
json.dump(result, f, indent=2)
|
||||
print(f"\n结果已保存到: {save_path}")
|
||||
|
||||
# 打印可粘贴到 laser_pnp.py 的 JSON
|
||||
paste = {"direction": direction.tolist(), "origin": origin.tolist()}
|
||||
print(f"\n--- 复制以下 JSON 到 laser_pnp.py 的 LASER_JSON 变量中 ---")
|
||||
print(json.dumps(paste, indent=2))
|
||||
print(f"---------------------------------------------------------")
|
||||
|
||||
return direction, origin
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# 加载内参
|
||||
calib_path = "calibration_result.json"
|
||||
try:
|
||||
with open(calib_path, "r") as f:
|
||||
calib_data = json.load(f)
|
||||
g_K = np.array(calib_data["mtx"])
|
||||
g_dist = np.array(calib_data["dist"])
|
||||
img_size = tuple(calib_data["img_size"])
|
||||
print(f"已加载内参: {calib_path}")
|
||||
print(f" 分辨率: {img_size[0]} x {img_size[1]}")
|
||||
except FileNotFoundError:
|
||||
print(f"未找到标定文件: {calib_path}")
|
||||
print("请先运行 calibrate.py 进行相机内参标定")
|
||||
sys.exit(1)
|
||||
|
||||
print("=" * 55)
|
||||
print(" 激光器-相机联合标定程序")
|
||||
print(f" 棋盘格: {CHESSBOARD_COLS}x{CHESSBOARD_ROWS}")
|
||||
print(f" 方格边长: {SQUARE_SIZE} mm")
|
||||
print("=" * 55)
|
||||
print()
|
||||
|
||||
# 输入拍照数量
|
||||
while True:
|
||||
try:
|
||||
n_photos = int(input_func("请输入要拍摄的图像数量 (建议 >= 10): "))
|
||||
if n_photos >= 3:
|
||||
break
|
||||
print("至少需要3张！")
|
||||
except ValueError:
|
||||
print("请输入有效数字！")
|
||||
|
||||
print(f"\n将拍摄 {n_photos} 张图像。")
|
||||
print("操作说明：")
|
||||
print(" 1. 将标定板以不同姿态放在相机视野中")
|
||||
print(" 2. 确保激光点投射在标定板上")
|
||||
print(" 3. 看到绿色棋盘格 + 红色激光点标注后，输入 y 并回车")
|
||||
print()
|
||||
|
||||
# ===== 初始化相机 =====
|
||||
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()
|
||||
|
||||
print("相机取流已启动。\n")
|
||||
|
||||
os.makedirs(g_image_dir, exist_ok=True)
|
||||
print(f"标定图像将保存到: {g_image_dir}/\n")
|
||||
|
||||
# ===== 交互式拍照 =====
|
||||
try:
|
||||
while len(g_laser_points_3d) < n_photos:
|
||||
remaining = n_photos - len(g_laser_points_3d)
|
||||
cmd = input_func(f"[{len(g_laser_points_3d)}/{n_photos}] 输入 y 并回车拍照 (剩余 {remaining}): ")
|
||||
|
||||
if cmd.strip().lower() == 'y':
|
||||
g_capture_flag = True
|
||||
waited = 0
|
||||
while g_capture_flag and waited < 100:
|
||||
cv2.waitKey(50)
|
||||
waited += 1
|
||||
if g_capture_flag:
|
||||
print(" 警告: 超时，请确保棋盘格和激光点同时可见")
|
||||
g_capture_flag = False
|
||||
elif cmd.strip().lower() == 'q':
|
||||
print("用户取消。")
|
||||
break
|
||||
else:
|
||||
print(" 请输入 y (拍照) 或 q (退出)")
|
||||
except KeyboardInterrupt:
|
||||
print("\n用户中断。")
|
||||
|
||||
# ===== 停止取流 =====
|
||||
print("\n停止相机取流...")
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
cam.MV_CC_StopGrabbing()
|
||||
cam.MV_CC_CloseDevice()
|
||||
cam.MV_CC_DestroyHandle()
|
||||
MvCamera.MV_CC_Finalize()
|
||||
|
||||
# ===== 拟合激光直线 =====
|
||||
if len(g_laser_points_3d) > 0:
|
||||
run_fitting()
|
||||
else:
|
||||
print("\n没有采集到激光点，跳过拟合。")
|
||||
@@ -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()
|
||||
@@ -0,0 +1,539 @@
|
||||
# -- 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()
|
||||
@@ -0,0 +1,49 @@
|
||||
|
||||
### 激光点位置解算详细说明
|
||||
|
||||
本方案旨在通过视觉PnP算法解算相机位姿，结合预先标定的“相机-激光器”刚体关系，计算激光点在矩形平面（世界坐标系）上的精确位置。
|
||||
|
||||
#### 1. 前置准备：相机内参标定
|
||||
首先对相机进行内参标定，获取相机内参矩阵 $K$ 和畸变系数。这是后续所有像素坐标转空间坐标的基础。
|
||||
|
||||
#### 2. 第一阶段：激光器与相机的联合标定（离线）
|
||||
由于激光器与相机刚性绑定，激光束在**相机坐标系**下表现为一条固定的空间直线。我们需要通过以下步骤求出这条直线的方程（起点与方向）。
|
||||
|
||||
1. **数据采集**：
|
||||
- 开启激光器，将激光点投射在平面标定板（如棋盘格）上。
|
||||
- 保持相机与激光器相对静止，改变整体姿态（距离、角度），拍摄多组（建议10组以上）包含标定板和激光点的图像。
|
||||
|
||||
2. **解算相机坐标系下的激光点**：
|
||||
- **外参获取**：对每张图像使用PnP算法，根据标定板角点计算相机相对于标定板的外参矩阵 $T_{wc}$（即世界到相机的变换）。
|
||||
- **反投影求交**：
|
||||
- 提取激光点的像素坐标 $(u, v)$。
|
||||
- 利用 $K^{-1}$ 将其转换为归一化射线方向。
|
||||
- **关键步骤**：利用标定板在相机坐标系下的平面方程与该射线求交，计算出激光点在**相机坐标系**下的三维坐标 $P_{c\_i}$。
|
||||
|
||||
3. **拟合激光线方程**：
|
||||
- 收集所有图像对应的相机坐标系下的激光点 $\{P_{c\_1}, P_{c\_2}, ...\}$。
|
||||
- 使用最小二乘法或PCA（主成分分析）拟合这些点，得到相机坐标系下的激光直线方程。
|
||||
- **记录参数**：
|
||||
- **方向向量 $\vec{d}_{cam}$**：拟合直线的单位方向向量。
|
||||
- **基准起点 $O_{cam}$**：选取该直线与相机坐标系 $Z=0$ 平面的交点作为参考原点。
|
||||
|
||||
#### 3. 第二阶段：在线测量与位置解算
|
||||
标定完成后，即可在实际应用中实时计算激光点位置。
|
||||
|
||||
1. **获取当前相机位姿**：
|
||||
- 拍摄包含目标矩形平面的图像。
|
||||
- 根据成像原理：$\text{像素坐标} \approx K \cdot [\text{归一化平面}] (T_{wc} \cdot \text{世界坐标})$。
|
||||
- 已知像素坐标、内参 $K$ 以及矩形平面的特征点世界坐标，利用PnP算法解算出当前的外参矩阵 $T_{wc}$（包含旋转矩阵 $R$ 和平移向量 $t$）。
|
||||
|
||||
2. **坐标变换至世界坐标系**：
|
||||
将标定阶段得到的激光线参数从相机坐标系变换到世界坐标系：
|
||||
- **变换方向向量**（仅受旋转影响）：
|
||||
$$ \vec{d}_{world} = R^T \cdot \vec{d}_{cam} $$
|
||||
- **变换基准起点**（受旋转和平移共同影响，即乘以 $T_{wc}$ 的逆矩阵）：
|
||||
$$ O_{world} = R^T \cdot (O_{cam} - t) $$
|
||||
|
||||
3. **求解世界坐标**：
|
||||
- 构建世界坐标系下的激光射线方程：$L(s) = O_{world} + s \cdot \vec{d}_{world}$。
|
||||
- **平面约束**：已知目标平面在世界坐标系中满足 $Z_{world} = 0$。
|
||||
- **联立求解**：令射线的Z分量等于0，解出参数 $s$，代回方程求得 $X, Y$。
|
||||
- **最终输出**：激光点在矩形平面上的世界坐标 $(x, y, 0)$。
|
||||
Executable
+397
@@ -0,0 +1,397 @@
|
||||
# -- coding: utf-8 --
|
||||
|
||||
import sys
|
||||
import platform
|
||||
import copy
|
||||
import os
|
||||
from ctypes import *
|
||||
import numpy
|
||||
import cv2
|
||||
|
||||
# 配置项
|
||||
image_area = 1080 * 1440 / 4
|
||||
threshold_area = image_area*0.01
|
||||
|
||||
|
||||
currentsystem = platform.system()
|
||||
print(f"current system: {currentsystem}")
|
||||
if currentsystem == 'Windows':
|
||||
sys.path.append(os.getenv('MVCAM_COMMON_RUNENV') + "/Samples/Python/MvImport")
|
||||
else:
|
||||
# Demo 目录: "/opt/xxx/Samples/平台/OpenCV/ZZ/ ") python接口目录： "/opt/xxx/Samples/平台/Python/MvImport"
|
||||
sys.path.append("/opt/MVS/Samples/aarch64/Python/MvImport")
|
||||
print(f"sys.path: {sys.path}")
|
||||
|
||||
|
||||
from MvCameraControl_class import *
|
||||
|
||||
# 兼容Python 2.x和3.x的输入处理
|
||||
if sys.version_info[0] < 3:
|
||||
# Python 2.x
|
||||
input_func = raw_input
|
||||
else:
|
||||
# Python 3.x
|
||||
input_func = input
|
||||
|
||||
stFrame = POINTER(MV_FRAME_OUT)
|
||||
fun_ctype = get_platform_functype()
|
||||
FrameInfoCallBack2 = fun_ctype(None, stFrame, c_void_p, c_bool)
|
||||
|
||||
|
||||
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
|
||||
else:
|
||||
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
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
#执行1次
|
||||
g_SaveImageOnce = False
|
||||
def image_callback2(pstFrame, pUser, bAutoFree):
|
||||
|
||||
start_time = cv2.getTickCount()
|
||||
|
||||
stFrame = cast(pstFrame, POINTER(MV_FRAME_OUT)).contents
|
||||
User = ctypes.cast(pUser, ctypes.py_object).value
|
||||
if stFrame:
|
||||
# print ("get one frame: Width[%d], Height[%d], nFrameNum[%d] len[%d] nLostPacket[%d]" % (stFrame.stFrameInfo.nWidth, stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nFrameNum ,stFrame.stFrameInfo.nFrameLen, stFrame.stFrameInfo.nLostPacket))
|
||||
global g_SaveImageOnce
|
||||
|
||||
while True:
|
||||
|
||||
stDecodeParam = MV_CC_HB_DECODE_PARAM()
|
||||
stConvertParam = MV_CC_PIXEL_CONVERT_PARAM_EX()
|
||||
memset(byref(stConvertParam), 0, sizeof(stConvertParam))
|
||||
|
||||
# 如果是HB图像，需要进行HB解码
|
||||
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:
|
||||
print("HB Decode fail! ret[0x%x]" % ret)
|
||||
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:
|
||||
print ("convert pixel fail! ret[0x%x]" % ret)
|
||||
break
|
||||
|
||||
if nChannelNum == 1:
|
||||
numpy_image = numpy.frombuffer(DstBuffer, dtype=numpy.ubyte, count=DstBufferlen). \
|
||||
reshape(stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth)
|
||||
|
||||
# 执行1次, 只保存1张图
|
||||
if g_SaveImageOnce == False:
|
||||
output_path = "output_mono_image.bmp"
|
||||
cv2.imwrite(output_path, numpy_image)
|
||||
print("save output_mono_image.bmp success")
|
||||
|
||||
g_SaveImageOnce = True
|
||||
else:
|
||||
numpy_image = numpy.frombuffer(DstBuffer, dtype=numpy.ubyte, count=DstBufferlen). \
|
||||
reshape(stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth, 3)
|
||||
#分辨率缩小一半，提高处理速度
|
||||
numpy_image = cv2.resize(numpy_image, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA)
|
||||
# 1. 预处理：灰度图 + 高斯模糊 (去噪)
|
||||
Gray_image = cv2.cvtColor(numpy_image, cv2.COLOR_BGR2GRAY)
|
||||
# 稍微增大模糊核，例如 (5,5)，以平滑更多纹理细节
|
||||
Gauss_image = cv2.GaussianBlur(Gray_image, (5, 5), 1)
|
||||
|
||||
# 2. 二值化替代 Canny (更稳健)
|
||||
# 方法A: Otsu自动阈值 (适合对比度明显的场景)
|
||||
ret, binary = cv2.threshold(Gauss_image, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
|
||||
|
||||
# 方法B: 如果光照不均，建议取消下面注释改用自适应阈值
|
||||
# binary = cv2.adaptiveThreshold(Gauss_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
|
||||
# cv2.THRESH_BINARY, 11, 2)
|
||||
|
||||
# 3. 形态学操作 (关键步骤：连接断裂，去除噪点)
|
||||
kernel = numpy.ones((3,3),numpy.uint8) # 核越大，连接能力越强，但也可能使物体变形
|
||||
dilated_binary = cv2.dilate(binary, kernel, iterations=1) # 膨胀
|
||||
eroded_binary = cv2.erode(dilated_binary, kernel, iterations=1) # 腐蚀 (先膨胀后腐蚀=闭运算，用于填补空洞)
|
||||
|
||||
# 4. 查找轮廓
|
||||
# 注意：二值化图像中，物体应为白色(255)，背景为黑色(0)
|
||||
contours, _ = cv2.findContours(eroded_binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
for cnt in contours:
|
||||
cv2.drawContours(numpy_image, [cnt], -1, (0, 255, 0), 1) # 绘制原始轮廓，调试用
|
||||
# --- 筛选条件 1: 面积过滤 ---
|
||||
# 忽略太小的噪点，根据实际情况调整数值 (例如忽略小于画面 1% 的区域)
|
||||
area = cv2.contourArea(cnt)
|
||||
if area < threshold_area:
|
||||
continue
|
||||
|
||||
# --- 筛选条件 2: 形状近似,必须是四边形且为凸多边形 ---
|
||||
epsilon = 0.02 * cv2.arcLength(cnt, True)
|
||||
approx = cv2.approxPolyDP(cnt, epsilon, True)
|
||||
|
||||
if len(approx) == 4 and cv2.isContourConvex(approx):
|
||||
# 绘制结果
|
||||
cv2.drawContours(numpy_image, [approx], -1, (0, 0, 255), 2)
|
||||
|
||||
# 绘制顶点
|
||||
for point in approx:
|
||||
cv2.circle(numpy_image, tuple(point[0]), 5, (255, 0, 0), -1)
|
||||
|
||||
|
||||
|
||||
# fps计算
|
||||
current_time = cv2.getTickCount()
|
||||
fps = cv2.getTickFrequency() / (current_time - start_time)
|
||||
text = f"FPS: {fps:.2f}"
|
||||
# 在左上角绘制文字
|
||||
cv2.putText(numpy_image, text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
|
||||
|
||||
# 显示结果
|
||||
cv2.imshow('Result', numpy_image)
|
||||
cv2.imshow('Binary', eroded_binary) # 调试时可查看二值化效果
|
||||
# cv2.imshow("ROI", img_ROI)
|
||||
cv2.waitKey(1)
|
||||
# 执行1次, 只保存1张图
|
||||
# if g_SaveImageOnce == False:
|
||||
# output_path = "output_color_image.bmp"
|
||||
# cv2.imwrite(output_path, numpy_image)
|
||||
# print("save output_color_image.bmp success")
|
||||
|
||||
# g_SaveImageOnce = True
|
||||
|
||||
|
||||
#正常退出
|
||||
break
|
||||
|
||||
#非自动释放模式，需要额外进行 手动释放资源
|
||||
if bAutoFree == False :
|
||||
if pUser != None:
|
||||
User.MV_CC_FreeImageBuffer(stFrame)
|
||||
else :
|
||||
print ("user is null, invalid")
|
||||
|
||||
|
||||
CALL_BACK_FUN2 = FrameInfoCallBack2(image_callback2)
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
# ch:初始化SDK | en: initialize SDK
|
||||
MvCamera.MV_CC_Initialize()
|
||||
|
||||
deviceList = MV_CC_DEVICE_INFO_LIST()
|
||||
# ch:选择枚举的传输层协议 (根据需求和系统支持情况选择) | en: Select the Transport Layer Protocol (based on your requirements and system support).
|
||||
tlayerType = (MV_GIGE_DEVICE | MV_USB_DEVICE | MV_GENTL_CAMERALINK_DEVICE
|
||||
| MV_GENTL_CXP_DEVICE | MV_GENTL_XOF_DEVICE)
|
||||
|
||||
# ch:枚举设备 | en:Enum 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 devices!" % 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_GIGE_DEVICE or mvcc_dev_info.nTLayerType == MV_GENTL_GIGE_DEVICE:
|
||||
print ("\ngige device: [%d]" % i)
|
||||
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName if c != 0])
|
||||
print ("device model name: %s" % strModeName)
|
||||
|
||||
nip1 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24)
|
||||
nip2 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16)
|
||||
nip3 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8)
|
||||
nip4 = (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff)
|
||||
print ("current ip: %d.%d.%d.%d\n" % (nip1, nip2, nip3, nip4))
|
||||
elif mvcc_dev_info.nTLayerType == MV_USB_DEVICE:
|
||||
print ("\nu3v device: [%d]" % i)
|
||||
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName if c != 0])
|
||||
print ("device model name: %s" % strModeName)
|
||||
|
||||
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chSerialNumber if c != 0])
|
||||
print ("user serial number: %s" % strSerialNumber)
|
||||
elif mvcc_dev_info.nTLayerType == MV_GENTL_CAMERALINK_DEVICE:
|
||||
print ("\nCML device: [%d]" % i)
|
||||
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stCMLInfo.chModelName if c != 0])
|
||||
print ("device model name: %s" % strModeName)
|
||||
|
||||
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stCMLInfo.chSerialNumber if c != 0])
|
||||
print ("user serial number: %s" % strSerialNumber)
|
||||
elif mvcc_dev_info.nTLayerType == MV_GENTL_CXP_DEVICE:
|
||||
print ("\nCXP device: [%d]" % i)
|
||||
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stCXPInfo.chModelName if c != 0])
|
||||
print ("device model name: %s" % strModeName)
|
||||
|
||||
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stCXPInfo.chSerialNumber if c != 0])
|
||||
print ("user serial number: %s" % strSerialNumber)
|
||||
elif mvcc_dev_info.nTLayerType == MV_GENTL_XOF_DEVICE:
|
||||
print ("\nXoF device: [%d]" % i)
|
||||
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stXoFInfo.chModelName if c != 0])
|
||||
print ("device model name: %s" % strModeName)
|
||||
|
||||
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stXoFInfo.chSerialNumber if c != 0])
|
||||
print ("user serial number: %s" % strSerialNumber)
|
||||
|
||||
# nConnectionNum = input_func("please input_func the number of the device to connect:")
|
||||
nConnectionNum = 0
|
||||
if int(nConnectionNum) >= deviceList.nDeviceNum:
|
||||
print ("intput error!")
|
||||
sys.exit()
|
||||
|
||||
# ch:创建相机实例 | en:Creat Camera Object
|
||||
cam = MvCamera()
|
||||
|
||||
# ch:选择设备并创建句柄 | en:Select device and create handle
|
||||
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()
|
||||
|
||||
# ch:打开设备 | en:Open device
|
||||
ret = cam.MV_CC_OpenDevice(MV_ACCESS_Exclusive, 0)
|
||||
if ret != 0:
|
||||
print ("open device fail! ret[0x%x]" % ret)
|
||||
sys.exit()
|
||||
|
||||
# ch:探测网络最佳包大小(只对GigE相机有效) | en:Detection network optimal package size(It only works for the GigE camera)
|
||||
if stDeviceList.nTLayerType == MV_GIGE_DEVICE or stDeviceList.nTLayerType == MV_GENTL_GIGE_DEVICE:
|
||||
nPacketSize = cam.MV_CC_GetOptimalPacketSize()
|
||||
if int(nPacketSize) > 0:
|
||||
ret = cam.MV_CC_SetIntValue("GevSCPSPacketSize",nPacketSize)
|
||||
if ret != 0:
|
||||
print ("Warning: Set Packet Size fail! ret[0x%x]" % ret)
|
||||
else:
|
||||
print ("Warning: Get Packet Size fail! ret[0x%x]" % nPacketSize)
|
||||
|
||||
# ch:设置触发模式为off | en:Set trigger mode as off
|
||||
ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF)
|
||||
if ret != 0:
|
||||
print ("set trigger mode fail! ret[0x%x]" % ret)
|
||||
sys.exit()
|
||||
|
||||
# ch:注册抓图回调 | en:Register image callback
|
||||
ret = cam.MV_CC_RegisterImageCallBackEx2(CALL_BACK_FUN2, ctypes.py_object(cam), True)
|
||||
if ret != 0:
|
||||
print ("register image callback fail! ret[0x%x]" % ret)
|
||||
sys.exit()
|
||||
|
||||
# ch:开始取流 | en:Start grab image
|
||||
ret = cam.MV_CC_StartGrabbing()
|
||||
if ret != 0:
|
||||
print ("start grabbing fail! ret[0x%x]" % ret)
|
||||
sys.exit()
|
||||
|
||||
print ("press Enter key to stop grabbing.")
|
||||
input_func()
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
# ch:停止取流 | en:Stop grab image
|
||||
ret = cam.MV_CC_StopGrabbing()
|
||||
if ret != 0:
|
||||
print ("stop grabbing fail! ret[0x%x]" % ret)
|
||||
sys.exit()
|
||||
|
||||
# ch:关闭设备 | Close device
|
||||
ret = cam.MV_CC_CloseDevice()
|
||||
if ret != 0:
|
||||
print ("close deivce fail! ret[0x%x]" % ret)
|
||||
sys.exit()
|
||||
|
||||
# ch:销毁句柄 | Destroy handle
|
||||
ret = cam.MV_CC_DestroyHandle()
|
||||
if ret != 0:
|
||||
print ("destroy handle fail! ret[0x%x]" % ret)
|
||||
sys.exit()
|
||||
|
||||
# ch:反初始化SDK | en: finalize SDK
|
||||
MvCamera.MV_CC_Finalize()
|
||||
Reference in New Issue
Block a user