first commit
This commit is contained in:
183
Vision/tool/CameraPe_depth2color.py
Normal file
183
Vision/tool/CameraPe_depth2color.py
Normal file
@ -0,0 +1,183 @@
|
||||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
'''
|
||||
# @Time : 2024/9/19 16:08
|
||||
# @Author : hjw
|
||||
# @File : CameraPe.py
|
||||
'''
|
||||
from Vision.tool.tuyang import pcammls
|
||||
from Vision.tool.tuyang.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]) # 深度图大小
|
||||
self.depth_calib_data = self.cl.DeviceReadCalibData(self.handle, PERCIPIO_STREAM_DEPTH)
|
||||
self.color_calib_data = self.cl.DeviceReadCalibData(self.handle, PERCIPIO_STREAM_COLOR)
|
||||
|
||||
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)
|
||||
|
||||
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.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()
|
||||
# self.img_registration_color = image_data()
|
||||
self.img_registration_render = image_data()
|
||||
self.img_registration_depth = image_data()
|
||||
self.img_registration_render = image_data()
|
||||
self.img_parsed_color = image_data()
|
||||
self.img_undistortion_color = image_data()
|
||||
self.img_registration_color = image_data()
|
||||
self.pointcloud_data_arr = pointcloud_data_list()
|
||||
self.caminit_isok = True
|
||||
print(VisionError_Code.CAMERA_SUCCESS)
|
||||
|
||||
|
||||
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, 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, self.scale_unit, 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()
|
||||
self.cl.DeviceStreamMapDepthImageToPoint3D(self.img_registration_depth, self.color_calib_data, self.scale_unit,
|
||||
self.pointcloud_data_arr)
|
||||
p3d_nparray = self.pointcloud_data_arr.as_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()
|
||||
|
||||
# self.cl.DeviceStreamMapRGBImageToDepthCoordinate(self.depth_calib, img_depth, self.scale_unit, self.color_calib,
|
||||
# self.img_undistortion_color, self.img_registration_color)
|
||||
# # 对齐
|
||||
# # mat_depth_render = self.img_registration_render.as_nparray()
|
||||
# self.cl.DeviceStreamDepthRender(img_depth, self.img_registration_render)
|
||||
# mat_depth_render = self.img_registration_render.as_nparray()
|
||||
#
|
||||
# self.cl.DeviceStreamMapDepthImageToPoint3D(img_depth, self.depth_calib_data, self.scale_unit, self.pointcloud_data_arr)
|
||||
# p3d_nparray = self.pointcloud_data_arr.as_nparray()
|
||||
#
|
||||
# mat_registration_color = self.img_registration_color.as_nparray() # 对齐的彩色图
|
||||
|
||||
return 1, mat_undistortion_color, p3d_nparray, mat_depth_render
|
||||
else:
|
||||
return 0, None, 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)
|
||||
Reference in New Issue
Block a user