195 lines
7.2 KiB
Python
195 lines
7.2 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
# -*- coding: utf-8 -*-
|
||
|
|
'''
|
||
|
|
# @Time : 2024/9/19 16:08
|
||
|
|
# @Author : hjw
|
||
|
|
# @File : CameraPe.py
|
||
|
|
'''
|
||
|
|
from Vision.tool.percipio.win import pcammls
|
||
|
|
from Vision.tool.percipio.win.pcammls import *
|
||
|
|
from Expection import VisionError_Code
|
||
|
|
import cv2
|
||
|
|
|
||
|
|
|
||
|
|
class PythonPercipioDeviceEvent(pcammls.DeviceEvent):
|
||
|
|
Offline = False
|
||
|
|
|
||
|
|
def __init__(self):
|
||
|
|
pcammls.DeviceEvent.__init__(self)
|
||
|
|
|
||
|
|
def run(self, handle, eventID):
|
||
|
|
if eventID==TY_EVENT_DEVICE_OFFLINE:
|
||
|
|
print('=== Event Callback: Device Offline!')
|
||
|
|
self.Offline = True
|
||
|
|
return 0
|
||
|
|
|
||
|
|
def IsOffline(self):
|
||
|
|
return self.Offline
|
||
|
|
|
||
|
|
class camera_pe():
|
||
|
|
|
||
|
|
def __init__(self):
|
||
|
|
super().__init__()
|
||
|
|
self.caminit_isok = False
|
||
|
|
self.cl = PercipioSDK()
|
||
|
|
dev_list = self.cl.ListDevice()
|
||
|
|
for idx in range(len(dev_list)):
|
||
|
|
dev = dev_list[idx]
|
||
|
|
print('{} -- {} \t {}'.format(idx, dev.id, dev.iface.id))
|
||
|
|
if len(dev_list) == 0:
|
||
|
|
print('no device')
|
||
|
|
return
|
||
|
|
if len(dev_list) == 1:
|
||
|
|
selected_idx = 0
|
||
|
|
else:
|
||
|
|
selected_idx = int(input('select a device:'))
|
||
|
|
if selected_idx < 0 or selected_idx >= len(dev_list):
|
||
|
|
return
|
||
|
|
|
||
|
|
sn = dev_list[selected_idx].id
|
||
|
|
# 设备ID
|
||
|
|
self.handle = self.cl.Open(sn)
|
||
|
|
if not self.cl.isValidHandle(self.handle):
|
||
|
|
err = self.cl.TYGetLastErrorCodedescription()
|
||
|
|
print('no device found : ', end='')
|
||
|
|
print(err)
|
||
|
|
return
|
||
|
|
|
||
|
|
self.event = PythonPercipioDeviceEvent()
|
||
|
|
self.cl.DeviceRegiststerCallBackEvent(self.event)
|
||
|
|
|
||
|
|
color_fmt_list = self.cl.DeviceStreamFormatDump(self.handle, PERCIPIO_STREAM_COLOR)
|
||
|
|
if len(color_fmt_list) == 0:
|
||
|
|
print('device has no color stream.')
|
||
|
|
return
|
||
|
|
|
||
|
|
# print('color image format list:')
|
||
|
|
# for idx in range(len(color_fmt_list)): # 查看图像分辨率
|
||
|
|
# fmt = color_fmt_list[idx]
|
||
|
|
# print('\t{} -size[{}x{}]\t-\t desc:{}'.format(idx, cl.Width(fmt), cl.Height(fmt), fmt.getDesc()))
|
||
|
|
self.cl.DeviceStreamFormatConfig(self.handle, PERCIPIO_STREAM_COLOR, color_fmt_list[2])
|
||
|
|
|
||
|
|
depth_fmt_list = self.cl.DeviceStreamFormatDump(self.handle, PERCIPIO_STREAM_DEPTH)
|
||
|
|
if len(depth_fmt_list) == 0:
|
||
|
|
print('device has no depth stream.')
|
||
|
|
return
|
||
|
|
|
||
|
|
# print('depth image format list:') # 查看深度图分辨率
|
||
|
|
# for idx in range(len(depth_fmt_list)):
|
||
|
|
# fmt = depth_fmt_list[idx]
|
||
|
|
# print('\t{} -size[{}x{}]\t-\t desc:{}'.format(idx, cl.Width(fmt), cl.Height(fmt), fmt.getDesc()))
|
||
|
|
self.cl.DeviceStreamFormatConfig(self.handle, PERCIPIO_STREAM_DEPTH, depth_fmt_list[2])
|
||
|
|
|
||
|
|
err = self.cl.DeviceLoadDefaultParameters(self.handle)
|
||
|
|
if err:
|
||
|
|
print('Load default parameters fail: ', end='')
|
||
|
|
print(self.cl.TYGetLastErrorCodedescription())
|
||
|
|
else:
|
||
|
|
print('Load default parameters successful')
|
||
|
|
|
||
|
|
self.scale_unit = self.cl.DeviceReadCalibDepthScaleUnit(self.handle)
|
||
|
|
#print('depth image scale unit :{}'.format(scale_unit))
|
||
|
|
|
||
|
|
self.depth_calib = self.cl.DeviceReadCalibData(self.handle, PERCIPIO_STREAM_DEPTH)
|
||
|
|
self.color_calib = self.cl.DeviceReadCalibData(self.handle, PERCIPIO_STREAM_COLOR)
|
||
|
|
|
||
|
|
self.pointcloud_data_arr = pointcloud_data_list()
|
||
|
|
self.img_registration_depth = image_data()
|
||
|
|
self.img_registration_render = image_data()
|
||
|
|
self.img_parsed_color = image_data()
|
||
|
|
self.img_undistortion_color = image_data()
|
||
|
|
|
||
|
|
err = self.cl.DeviceStreamEnable(self.handle, PERCIPIO_STREAM_COLOR | PERCIPIO_STREAM_DEPTH)
|
||
|
|
if err:
|
||
|
|
print('device stream enable err:{}'.format(err))
|
||
|
|
return
|
||
|
|
|
||
|
|
self.cl.DeviceStreamOn(self.handle)
|
||
|
|
self.caminit_isok = True
|
||
|
|
print(VisionError_Code.CAMERA_SUCCESS)
|
||
|
|
def get_img(self):
|
||
|
|
""
|
||
|
|
'''
|
||
|
|
:param api: None
|
||
|
|
:return: ret ,img
|
||
|
|
'''
|
||
|
|
if self.caminit_isok == False or self.event.IsOffline():
|
||
|
|
return 0, None
|
||
|
|
else:
|
||
|
|
image_list = self.cl.DeviceStreamRead(self.handle, 2000)
|
||
|
|
if len(image_list) == 2:
|
||
|
|
for i in range(len(image_list)):
|
||
|
|
frame = image_list[i]
|
||
|
|
if frame.streamID == PERCIPIO_STREAM_COLOR:
|
||
|
|
img_color = frame
|
||
|
|
self.cl.DeviceStreamImageDecode(frame, self.img_undistortion_color)
|
||
|
|
img = self.img_undistortion_color.as_nparray()
|
||
|
|
return 1, img
|
||
|
|
return 0, None
|
||
|
|
|
||
|
|
def get_point_map(self):
|
||
|
|
""
|
||
|
|
'''
|
||
|
|
:param api: None
|
||
|
|
:return: img
|
||
|
|
'''
|
||
|
|
if self.caminit_isok == False or self.event.IsOffline():
|
||
|
|
return 0, None
|
||
|
|
else:
|
||
|
|
pass
|
||
|
|
|
||
|
|
def get_img_and_point_map(self):
|
||
|
|
""
|
||
|
|
'''
|
||
|
|
:param api: None
|
||
|
|
:return: ret , img, point_map
|
||
|
|
'''
|
||
|
|
if self.caminit_isok == False or self.event.IsOffline():
|
||
|
|
return 0, None, None
|
||
|
|
else:
|
||
|
|
image_list = self.cl.DeviceStreamRead(self.handle, 2000)
|
||
|
|
if len(image_list) == 2:
|
||
|
|
for i in range(len(image_list)):
|
||
|
|
frame = image_list[i]
|
||
|
|
if frame.streamID == PERCIPIO_STREAM_DEPTH:
|
||
|
|
img_depth = frame
|
||
|
|
if frame.streamID == PERCIPIO_STREAM_COLOR:
|
||
|
|
img_color = frame
|
||
|
|
|
||
|
|
self.cl.DeviceStreamMapDepthImageToColorCoordinate(self.depth_calib, img_depth.width, img_depth.height,
|
||
|
|
self.scale_unit, img_depth, self.color_calib, img_color.width,
|
||
|
|
img_color.height, self.img_registration_depth)
|
||
|
|
|
||
|
|
# self.cl.DeviceStreamDepthRender(self.img_registration_depth, self.img_registration_render)
|
||
|
|
# mat_depth_render = self.img_registration_render.as_nparray()
|
||
|
|
# cv2.imshow('registration', mat_depth_render)
|
||
|
|
|
||
|
|
self.cl.DeviceStreamMapDepthImageToPoint3D(self.img_registration_depth, self.depth_calib, self.scale_unit,
|
||
|
|
self.pointcloud_data_arr)
|
||
|
|
|
||
|
|
# show p3d arr data
|
||
|
|
p3d_nparray = self.pointcloud_data_arr.as_nparray()
|
||
|
|
#cv2.imshow('p3d2', p3d_nparray)
|
||
|
|
|
||
|
|
self.cl.DeviceStreamImageDecode(img_color, self.img_parsed_color)
|
||
|
|
self.cl.DeviceStreamDoUndistortion(self.color_calib, self.img_parsed_color, self.img_undistortion_color)
|
||
|
|
mat_undistortion_color = self.img_undistortion_color.as_nparray()
|
||
|
|
return 1, mat_undistortion_color, p3d_nparray
|
||
|
|
else:
|
||
|
|
return 0, None, None
|
||
|
|
|
||
|
|
def release(self):
|
||
|
|
if self.caminit_isok == False:
|
||
|
|
pass
|
||
|
|
else:
|
||
|
|
self.cl.DeviceStreamOff(self.handle)
|
||
|
|
self.cl.Close(self.handle)
|
||
|
|
pass
|
||
|
|
|
||
|
|
|
||
|
|
# my_camera = camera()
|
||
|
|
# while True:
|
||
|
|
# ret, img, p3d_nparray = my_camera.get_img_and_point_map()
|
||
|
|
# cv2.imshow('img', img)
|
||
|
|
# cv2.imshow('3d',p3d_nparray)
|
||
|
|
# cv2.waitKey(1)
|