dev01
fd3d6b7ab5
- Integrate PnP rectangle pose estimation + laser world coordinate computation from scripts/calibrate/laser_pnp.py - Publish normalized measurement (laser/RECT) and reference (rect center) via Float32MultiArray - Update gimbal 0xA5 frame to 18-byte format: measure_x, ref_x, measure_y, ref_y - Add camera calibration, laser line params, and RECT_L/W constants - Add corner ordering (TL/BL/BR/TR), SOLVEPNP_IPPE, and pose/laser visualization overlay - Remove standalone vision_example.py (superseded by ROS2 nodes) - Add scripts/calibrate/ with calibration and PnP tools
398 lines
18 KiB
Python
Executable File
398 lines
18 KiB
Python
Executable File
# -- 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()
|