xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

UPDATE Vision 图漾相机点云对齐，异常数据保存

Browse Source
This commit is contained in:
HJW
2024-10-14 16:32:20 +08:00
parent 2af2bff5e8
commit b88e1937ec
3 changed files with 58 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
Vision/camera_coordinate_dete.py
View File

@ -40,10 +40,10 @@ class Detection:
device = 'cpu' device = 'cpu'
if self.cameraType == 'RVC': if self.cameraType == 'RVC':
self.camera_rvc = camera_rvc() self.camera_rvc = camera_rvc()
self.seg_distance_threshold = 0.005 self.seg_distance_threshold = 0.01 # 1厘米
elif self.cameraType == 'Pe': elif self.cameraType == 'Pe':
self.camera_rvc = camera_pe() self.camera_rvc = camera_pe()
self.seg_distance_threshold = 10 self.seg_distance_threshold = 20 # 2厘米
else: else:
print('相机参数错误') print('相机参数错误')
return return
@ -54,22 +54,24 @@ class Detection:
device = 'CPU' device = 'CPU'
if self.cameraType == 'RVC': if self.cameraType == 'RVC':
self.camera_rvc = camera_rvc() self.camera_rvc = camera_rvc()
self.seg_distance_threshold = 0.005 self.seg_distance_threshold = 0.01
elif self.cameraType == 'Pe': elif self.cameraType == 'Pe':
self.camera_rvc = camera_pe() self.camera_rvc = camera_pe()
self.seg_distance_threshold = 10 self.seg_distance_threshold = 20
else: else:
print('相机参数错误') print('相机参数错误')
return return
self.model = yolov8_segment_openvino(model_path, device, conf_thres=0.3, iou_thres=0.3) self.model = yolov8_segment_openvino(model_path, device, conf_thres=0.3, iou_thres=0.3)
def get_position(self, Point_isVision=False, Box_isPoint=True, save_img_point=0, Height_reduce = 30, width_reduce = 30): def get_position(self, Point_isVision=False, Box_isPoint=True, First_Depth =True, Iter_Max_Pixel = 30, save_img_point=0, Height_reduce = 30, width_reduce = 30):
""" """
检测料袋相关信息 检测料袋相关信息
:param Point_isVision: 点云可视化 :param Point_isVision: 点云可视化
:param Box_isPoint: True 返回点云值； False 返回box相机坐标 :param Box_isPoint: True 返回点云值； False 返回box相机坐标
:param save_img_point: 0不保存 ; 1保存原图 ;2保存处理后的图 ; 3保存点云和原图；4 保存点云和处理后的图 :param First_Depth: True 返回料袋中心点深度最小的点云值； False 返回面积最大的料袋中心点云值
:param Iter_Max_Pixel: [int] 点云为NAN时，向该点周围寻找替代值，寻找最大区域（Iter_Max_Pixel×Iter_Max_Pixel）
:param save_img_point: 0不保存 ; 1保存原图 ;2保存处理后的图 ; 3保存点云和原图；4 保存点云和处理后的图; 5 异常数据保存（点云NAN）
:param Height_reduce: 检测框的高内缩像素 :param Height_reduce: 检测框的高内缩像素
:param width_reduce: 检测框的宽内缩像素 :param width_reduce: 检测框的宽内缩像素
:return ret: bool 相机是否正常工作 :return ret: bool 相机是否正常工作
@ -91,16 +93,21 @@ class Detection:
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))]) time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))])
save_img_name = ''.join([save_path, '.png']) save_img_name = ''.join([save_path, '.png'])
save_point_name = ''.join([save_path, '.xyz']) save_point_name = ''.join([save_path, '.xyz'])
if save_img_point == 5:
Abnormal_data_img = img.copy()
if save_img_point==1 or save_img_point==3: if save_img_point==1 or save_img_point==3:
cv2.imwrite(save_img_name, img) cv2.imwrite(save_img_name, img)
if save_img_point==3 or save_img_point==4: if save_img_point==3 or save_img_point==4 or save_img_point==5:
row_list = list(range(1, img.shape[0], 2)) row_list = list(range(1, img.shape[0], 2))
column_list = list(range(1, img.shape[1], 2)) column_list = list(range(1, img.shape[1], 2))
pm_save = pm.copy() pm_save = pm.copy()
pm_save1 = np.delete(pm_save, row_list, axis=0) pm_save1 = np.delete(pm_save, row_list, axis=0)
point_new = np.delete(pm_save1, column_list, axis=1) point_new = np.delete(pm_save1, column_list, axis=1)
point_new = point_new.reshape(-1, 3) point_new = point_new.reshape(-1, 3)
np.savetxt(save_point_name, point_new) if save_img_point==5:
Abnormal_data_point = point_new.copy()
else:
np.savetxt(save_point_name, point_new)
if self.use_openvino_model == False: if self.use_openvino_model == False:
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0) flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
else: else:
@ -211,7 +218,7 @@ class Detection:
select_point = np.array(select_point) select_point = np.array(select_point)
pm_seg = select_point.reshape(-1, 3) pm_seg = select_point.reshape(-1, 3)
pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan
if pm_seg.size<300: if pm_seg.size < 100:
print("分割点云数量较少，无法拟合平面") print("分割点云数量较少，无法拟合平面")
continue continue
@ -240,18 +247,18 @@ class Detection:
box[2][0][1], box[2][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[2][0][1], box[2][0][0]) box[2][0][1], box[2][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[2][0][1], box[2][0][0])
box[3][0][1], box[3][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[3][0][1], box[3][0][0]) box[3][0][1], box[3][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[3][0][1], box[3][0][0])
if Box_isPoint == True: if Box_isPoint == True:
box_point_x1, box_point_y1, box_point_z1 = remove_nan_mean_value(pm, box[0][0][1], box[0][0][0]) box_point_x1, box_point_y1, box_point_z1 = remove_nan_mean_value(pm, box[0][0][1], box[0][0][0], iter_max=Iter_Max_Pixel)
box_point_x2, box_point_y2, box_point_z2 = remove_nan_mean_value(pm, box[1][0][1], box[1][0][0]) box_point_x2, box_point_y2, box_point_z2 = remove_nan_mean_value(pm, box[1][0][1], box[1][0][0], iter_max=Iter_Max_Pixel)
box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0]) box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0], iter_max=Iter_Max_Pixel)
box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0]) box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0], iter_max=Iter_Max_Pixel)
else: else:
x1, y1, z1= uv_to_XY(box[0][0][0], box[0][0][1]) x1, y1, z1 = uv_to_XY(box[0][0][0], box[0][0][1])
x2, y2, z2 = uv_to_XY(box[1][0][0], box[1][0][1]) x2, y2, z2 = uv_to_XY(box[1][0][0], box[1][0][1])
x3, y3, z3 = uv_to_XY(box[2][0][0], box[2][0][1]) x3, y3, z3 = uv_to_XY(box[2][0][0], box[2][0][1])
x4, y4, z4 = uv_to_XY(box[3][0][0], box[3][0][1]) x4, y4, z4 = uv_to_XY(box[3][0][0], box[3][0][1])
x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4) x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4)
y_rotation_center = int((box[0][0][1] + box[1][0][1] + box[2][0][1] + box[3][0][1]) / 4) y_rotation_center = int((box[0][0][1] + box[1][0][1] + box[2][0][1] + box[3][0][1]) / 4)
point_x, point_y, point_z = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center) point_x, point_y, point_z = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center, iter_max=Iter_Max_Pixel)
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点 cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
if np.isnan(point_x): # 点云值为无效值 if np.isnan(point_x): # 点云值为无效值
continue continue
@ -281,9 +288,12 @@ class Detection:
cv2.polylines(img, [box], True, (0, 255, 0), 2) cv2.polylines(img, [box], True, (0, 255, 0), 2)
cv2.polylines(img, [box_outside], True, (226, 12, 89), 2) cv2.polylines(img, [box_outside], True, (226, 12, 89), 2)
_idx = find_position(Depth_Z, RegionalArea, 100,True) _idx = find_position(Depth_Z, RegionalArea, 100, First_Depth)
if _idx == None: if _idx == None:
if save_img_point == 5:
cv2.imwrite(save_img_name, Abnormal_data_img)
np.savetxt(save_point_name, Abnormal_data_point)
return 1, img, None, None, None return 1, img, None, None, None
else: else:
cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点 cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点
@ -307,6 +317,9 @@ class Detection:
if save_img_point == 2 or save_img_point ==4: if save_img_point == 2 or save_img_point ==4:
save_img = cv2.resize(img,(720, 540)) save_img = cv2.resize(img,(720, 540))
cv2.imwrite(save_img_name, save_img) cv2.imwrite(save_img_name, save_img)
if save_img_point == 5:
cv2.imwrite(save_img_name, Abnormal_data_img)
np.savetxt(save_point_name, Abnormal_data_point)
return 1, img, None, None, None return 1, img, None, None, None
else: else:

36
Vision/camera_coordinate_dete_img.py
View File

@ -46,12 +46,14 @@ class Detection:
pass pass
def get_position(self, Point_isVision=False, Box_isPoint=False, save_img_point=0, seg_distance_threshold = 0.01, Height_reduce = 30, width_reduce = 30): def get_position(self, Point_isVision=False, Box_isPoint=False, First_Depth =True, Iter_Max_Pixel = 30, save_img_point=0, seg_distance_threshold = 0.01, Height_reduce = 30, width_reduce = 30):
""" """
检测料袋相关信息 检测料袋相关信息
:param Point_isVision: 点云可视化 :param Point_isVision: 点云可视化
:param Box_isPoint: True 返回点云值； False 返回box相机坐标 :param Box_isPoint: True 返回点云值； False 返回box相机坐标
:param save_img_point: 0不保存 ; 1保存原图 ;2保存处理后的图 ; 3保存点云和原图；4 保存点云和处理后的图 :param First_Depth: True 返回料袋中心点深度最小的点云值； False 返回面积最大的料袋中心点云值
:param Iter_Max_Pixel: [int] 点云为NAN时，向该点周围寻找替代值，寻找最大区域（Iter_Max_Pixel×Iter_Max_Pixel）
:param save_img_point: 0不保存 ; 1保存原图 ;2保存处理后的图 ; 3保存点云和原图；4 保存点云和处理后的图; 5 异常数据保存（点云NAN）
:param Height_reduce: 检测框的高内缩像素 :param Height_reduce: 检测框的高内缩像素
:param width_reduce: 检测框的宽内缩像素 :param width_reduce: 检测框的宽内缩像素
:return ret: bool 相机是否正常工作 :return ret: bool 相机是否正常工作
@ -73,16 +75,21 @@ class Detection:
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))]) time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))])
save_img_name = ''.join([save_path, '.png']) save_img_name = ''.join([save_path, '.png'])
save_point_name = ''.join([save_path, '.xyz']) save_point_name = ''.join([save_path, '.xyz'])
if save_img_point == 5:
Abnormal_data_img = img.copy()
if save_img_point == 1 or save_img_point == 3: if save_img_point == 1 or save_img_point == 3:
cv2.imwrite(save_img_name, img) cv2.imwrite(save_img_name, img)
if save_img_point == 3 or save_img_point == 4: if save_img_point == 3 or save_img_point == 4 or save_img_point == 5:
row_list = list(range(1, img.shape[0], 2)) row_list = list(range(1, img.shape[0], 2))
column_list = list(range(1, img.shape[1], 2)) column_list = list(range(1, img.shape[1], 2))
pm_save = pm.copy() pm_save = pm.copy()
pm_save1 = np.delete(pm_save, row_list, axis=0) pm_save1 = np.delete(pm_save, row_list, axis=0)
point_new = np.delete(pm_save1, column_list, axis=1) point_new = np.delete(pm_save1, column_list, axis=1)
point_new = point_new.reshape(-1, 3) point_new = point_new.reshape(-1, 3)
np.savetxt(save_point_name, point_new) if save_img_point == 5:
Abnormal_data_point = point_new.copy()
else:
np.savetxt(save_point_name, point_new)
if self.use_openvino_model == False: if self.use_openvino_model == False:
@ -197,7 +204,7 @@ class Detection:
select_point = np.array(select_point) select_point = np.array(select_point)
pm_seg = select_point.reshape(-1, 3) pm_seg = select_point.reshape(-1, 3)
pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan
if pm_seg.size < 300: if pm_seg.size < 100:
print("分割点云数量较少，无法拟合平面") print("分割点云数量较少，无法拟合平面")
continue continue
# cv2.imshow('result', point_result) # cv2.imshow('result', point_result)
@ -226,10 +233,10 @@ class Detection:
box[2][0][1], box[2][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[2][0][1], box[2][0][0]) box[2][0][1], box[2][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[2][0][1], box[2][0][0])
box[3][0][1], box[3][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[3][0][1], box[3][0][0]) box[3][0][1], box[3][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[3][0][1], box[3][0][0])
if Box_isPoint == True: if Box_isPoint == True:
box_point_x1, box_point_y1, box_point_z1 = remove_nan_mean_value(pm, box[0][0][1], box[0][0][0]) box_point_x1, box_point_y1, box_point_z1 = remove_nan_mean_value(pm, box[0][0][1], box[0][0][0], iter_max=First_Depth)
box_point_x2, box_point_y2, box_point_z2 = remove_nan_mean_value(pm, box[1][0][1], box[1][0][0]) box_point_x2, box_point_y2, box_point_z2 = remove_nan_mean_value(pm, box[1][0][1], box[1][0][0], iter_max=First_Depth)
box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0]) box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0], iter_max=First_Depth)
box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0]) box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0], iter_max=First_Depth)
else: else:
x1, y1, z1 = uv_to_XY(self.cameraType, box[0][0][0], box[0][0][1]) x1, y1, z1 = uv_to_XY(self.cameraType, box[0][0][0], box[0][0][1])
x2, y2, z2 = uv_to_XY(self.cameraType, box[1][0][0], box[1][0][1]) x2, y2, z2 = uv_to_XY(self.cameraType, box[1][0][0], box[1][0][1])
@ -237,7 +244,7 @@ class Detection:
x4, y4, z4 = uv_to_XY(self.cameraType, box[3][0][0], box[3][0][1]) x4, y4, z4 = uv_to_XY(self.cameraType, box[3][0][0], box[3][0][1])
x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4) x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4)
y_rotation_center = int((box[0][0][1] + box[1][0][1] + box[2][0][1] + box[3][0][1]) / 4) y_rotation_center = int((box[0][0][1] + box[1][0][1] + box[2][0][1] + box[3][0][1]) / 4)
point_x, point_y, point_z = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center) point_x, point_y, point_z = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center, iter_max=First_Depth)
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点 cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
if np.isnan(point_x): # 点云值为无效值 if np.isnan(point_x): # 点云值为无效值
continue continue
@ -268,9 +275,12 @@ class Detection:
cv2.polylines(img, [box], True, (0, 255, 0), 2) cv2.polylines(img, [box], True, (0, 255, 0), 2)
cv2.polylines(img, [box_outside], True, (226, 12, 89), 2) cv2.polylines(img, [box_outside], True, (226, 12, 89), 2)
_idx = find_position(Depth_Z, RegionalArea, 100, True) _idx = find_position(Depth_Z, RegionalArea, 100, first_depth = Iter_Max_Pixel)
if _idx == None: if _idx == None:
if save_img_point == 5:
cv2.imwrite(save_img_name, Abnormal_data_img)
np.savetxt(save_point_name, Abnormal_data_point)
return 1, img, None, None, None return 1, img, None, None, None
else: else:
cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点 cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点
@ -295,7 +305,9 @@ class Detection:
if save_img_point == 2 or save_img_point == 4: if save_img_point == 2 or save_img_point == 4:
save_img = cv2.resize(img, (720, 540)) save_img = cv2.resize(img, (720, 540))
cv2.imwrite(save_img_name, save_img) cv2.imwrite(save_img_name, save_img)
if save_img_point == 5:
cv2.imwrite(save_img_name, Abnormal_data_img)
np.savetxt(save_point_name, Abnormal_data_point)
return 1, img, None, None, None return 1, img, None, None, None
else: else:

10
Vision/tool/CameraPe.py
View File

@ -156,15 +156,15 @@ class camera_pe():
if frame.streamID == PERCIPIO_STREAM_COLOR: if frame.streamID == PERCIPIO_STREAM_COLOR:
img_color = frame img_color = frame
# self.cl.DeviceStreamMapDepthImageToColorCoordinate(self.depth_calib, img_depth.width, img_depth.height, self.cl.DeviceStreamMapDepthImageToColorCoordinate(self.depth_calib, img_depth.width, img_depth.height,
# self.scale_unit, img_depth, self.color_calib, img_color.width, self.scale_unit, img_depth, self.color_calib, img_color.width,
# img_color.height, self.img_registration_depth) img_color.height, self.img_registration_depth)
#
# self.cl.DeviceStreamDepthRender(self.img_registration_depth, self.img_registration_render) # self.cl.DeviceStreamDepthRender(self.img_registration_depth, self.img_registration_render)
# mat_depth_render = self.img_registration_render.as_nparray() # mat_depth_render = self.img_registration_render.as_nparray()
# cv2.imshow('registration', mat_depth_render) # cv2.imshow('registration', mat_depth_render)
self.cl.DeviceStreamMapDepthImageToPoint3D(img_depth, self.depth_calib, self.scale_unit, self.cl.DeviceStreamMapDepthImageToPoint3D(self.img_registration_depth, self.depth_calib, self.scale_unit,
self.pointcloud_data_arr) self.pointcloud_data_arr)
# show p3d arr data # show p3d arr data