first commit
This commit is contained in:
791
Vision/camera_coordinate_dete.py
Normal file
791
Vision/camera_coordinate_dete.py
Normal file
@ -0,0 +1,791 @@
|
||||
#!/usr/bin/env python
|
||||
# -*- coding: UTF-8 -*-
|
||||
'''
|
||||
@Project ：AutoControlSystem-master
|
||||
@File ：camera_coordinate_dete.py
|
||||
@IDE ：PyCharm
|
||||
@Author ：hjw
|
||||
@Date ：2024/8/27 14:24
|
||||
'''
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
import open3d as o3d
|
||||
import time
|
||||
import os
|
||||
|
||||
from Vision.tool.CameraRVC import camera_rvc
|
||||
from Vision.tool.CameraPe_color2depth import camera_pe as camera_pe_color2depth
|
||||
from Vision.tool.CameraPe_depth2color import camera_pe as camera_pe_depth2color
|
||||
from Vision.yolo.yolov8_pt_seg import yolov8_segment
|
||||
from Vision.yolo.yolov8_openvino import yolov8_segment_openvino
|
||||
from Vision.tool.utils import find_position
|
||||
from Vision.tool.utils import class_names
|
||||
from Vision.tool.utils import get_disk_space
|
||||
from Vision.tool.utils import remove_nan_mean_value
|
||||
from Vision.tool.utils import out_bounds_dete
|
||||
from Vision.tool.utils import uv_to_XY
|
||||
|
||||
|
||||
class Detection:
|
||||
|
||||
def __init__(self, use_openvino_model=False, cameraType = 'Pe', alignmentType = 'color2depth'): # cameraType = 'RVC' or cameraType = 'Pe'
|
||||
"""
|
||||
初始化相机及模型
|
||||
:param use_openvino_model: 选择模型，默认使用openvino
|
||||
:param cameraType: 选择相机 如本相机 'RVC', 图漾相机 'Pe'
|
||||
:param alignmentType: 相机对齐方式 color2depth：彩色图对齐深度图 ；depth2color：深度图对齐彩色图
|
||||
|
||||
"""
|
||||
self.use_openvino_model = use_openvino_model
|
||||
self.cameraType = cameraType
|
||||
self.alignmentType= alignmentType
|
||||
if self.use_openvino_model == False:
|
||||
model_path = ''.join([os.getcwd(), '/Vision/model/pt/one_bag.pt'])
|
||||
device = 'cpu'
|
||||
if self.cameraType == 'RVC':
|
||||
self.camera_rvc = camera_rvc()
|
||||
self.seg_distance_threshold = 10 # 1厘米
|
||||
elif self.cameraType == 'Pe':
|
||||
if self.alignmentType=='color2depth':
|
||||
self.camera_rvc = camera_pe_color2depth()
|
||||
else:
|
||||
self.camera_rvc = camera_pe_depth2color()
|
||||
self.seg_distance_threshold = 15 # 2厘米
|
||||
else:
|
||||
print('相机参数错误')
|
||||
return
|
||||
self.model = yolov8_segment()
|
||||
self.model.load_model(model_path, device)
|
||||
else:
|
||||
model_path = ''.join([os.getcwd(), '/Vision/model/openvino/one_bag.xml'])
|
||||
device = 'CPU'
|
||||
if self.cameraType == 'RVC':
|
||||
self.camera_rvc = camera_rvc()
|
||||
self.seg_distance_threshold = 10
|
||||
elif self.cameraType == 'Pe':
|
||||
if self.alignmentType == 'color2depth':
|
||||
self.camera_rvc = camera_pe_color2depth()
|
||||
else:
|
||||
self.camera_rvc = camera_pe_depth2color()
|
||||
self.seg_distance_threshold = 20
|
||||
else:
|
||||
print('相机参数错误')
|
||||
return
|
||||
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, First_Depth =True, Iter_Max_Pixel = 30, save_img_point=0, Height_reduce = 80, width_reduce = 60, Xmin =160, Xmax = 1050, Ymin =290 ,Ymax = 780):
|
||||
"""
|
||||
检测料袋相关信息
|
||||
:param Point_isVision: 点云可视化
|
||||
:param Box_isPoint: True 返回点云值； False 返回box相机坐标
|
||||
: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 width_reduce: 检测框的宽内缩像素
|
||||
:param Xmin: 限定料袋中心点的范围
|
||||
:param Xmax: 限定料袋中心点的范围
|
||||
:param Ymin: 限定料袋中心点的范围
|
||||
:param Ymax: 限定料袋中心点的范围
|
||||
|
||||
:return ret: bool 相机是否正常工作
|
||||
:return img: ndarray 返回img
|
||||
:return xyz: list 目标中心点云值形如[x,y,z]
|
||||
:return nx_ny_nz: list 拟合平面法向量，形如[a,b,c]
|
||||
:return box_list: list 内缩检测框四顶点，形如[[x1,y1],[],[],[]]
|
||||
|
||||
"""
|
||||
ret, img, pm, _depth_align = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
|
||||
if self.camera_rvc.caminit_isok == True:
|
||||
if ret == 1:
|
||||
if save_img_point != 0:
|
||||
if get_disk_space(path=os.getcwd()) < 15: # 内存小于15G,停止保存数据
|
||||
save_img_point = 0
|
||||
print('系统内存不足，无法保存数据')
|
||||
else:
|
||||
save_path = ''.join([os.getcwd(), '/Vision/model/data/',
|
||||
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))])
|
||||
save_img_name = ''.join([save_path, '.png'])
|
||||
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:
|
||||
cv2.imwrite(save_img_name, img)
|
||||
if save_img_point==3 or save_img_point==4 or save_img_point==5:
|
||||
row_list = list(range(1, img.shape[0], 2))
|
||||
column_list = list(range(1, img.shape[1], 2))
|
||||
pm_save = pm.copy()
|
||||
pm_save1 = np.delete(pm_save, row_list, axis=0)
|
||||
point_new = np.delete(pm_save1, column_list, axis=1)
|
||||
point_new = point_new.reshape(-1, 3)
|
||||
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:
|
||||
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
|
||||
else:
|
||||
flag, det_cpu, scores, masks, category_names = self.model.segment_objects(img)
|
||||
if flag == 1:
|
||||
xyz = []
|
||||
nx_ny_nz = []
|
||||
RegionalArea = []
|
||||
Depth_Z = []
|
||||
uv = []
|
||||
seg_point = []
|
||||
box_list = []
|
||||
if Point_isVision==True:
|
||||
pm2 = pm.copy()
|
||||
pm2 = pm2.reshape(-1, 3)
|
||||
pm2 = pm2[~np.isnan(pm2).all(axis=-1), :]
|
||||
pm2[:, 2] = pm2[:, 2] + 0.25
|
||||
pcd2 = o3d.geometry.PointCloud()
|
||||
pcd2.points = o3d.utility.Vector3dVector(pm2)
|
||||
# o3d.visualization.draw_geometries([pcd2])
|
||||
|
||||
for i, item in enumerate(det_cpu):
|
||||
|
||||
# 画box
|
||||
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
|
||||
if self.use_openvino_model == False:
|
||||
label = category_names[int(item[5])]
|
||||
else:
|
||||
label = class_names[int(item[4])]
|
||||
rand_color = (0, 255, 255)
|
||||
score = item[4]
|
||||
org = (int((box_x1 + box_x2) / 2), int((box_y1 + box_y2) / 2))
|
||||
x_center = int((box_x1 + box_x2) / 2)
|
||||
y_center = int((box_y1 + box_y2) / 2)
|
||||
text = '{}|{:.2f}'.format(label, score)
|
||||
cv2.putText(img, text, org=org, fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8,
|
||||
color=rand_color,
|
||||
thickness=2)
|
||||
# 画mask
|
||||
# mask = masks[i].cpu().numpy().astype(int)
|
||||
if self.use_openvino_model == False:
|
||||
mask = masks[i].cpu().data.numpy().astype(int)
|
||||
else:
|
||||
mask = masks[i].astype(int)
|
||||
mask = mask[box_y1:box_y2, box_x1:box_x2]
|
||||
|
||||
# mask = masks[i].numpy().astype(int)
|
||||
h, w = box_y2 - box_y1, box_x2 - box_x1
|
||||
mask_colored = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
mask_colored[np.where(mask)] = rand_color
|
||||
##################################
|
||||
imgray = cv2.cvtColor(mask_colored, cv2.COLOR_BGR2GRAY)
|
||||
# cv2.imshow('mask',imgray)
|
||||
# cv2.waitKey(1)
|
||||
# 2、二进制图像
|
||||
ret, binary = cv2.threshold(imgray, 10, 255, 0)
|
||||
# 阈值 二进制图像
|
||||
# cv2.imshow('bin',binary)
|
||||
# cv2.waitKey(1)
|
||||
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
|
||||
# all_point_list = contours_in(contours)
|
||||
# print(len(all_point_list))
|
||||
max_contour = None
|
||||
max_perimeter = 0
|
||||
for contour in contours: # 排除小分割区域或干扰区域
|
||||
perimeter = cv2.arcLength(contour, True)
|
||||
if perimeter > max_perimeter:
|
||||
max_perimeter = perimeter
|
||||
max_contour = contour
|
||||
|
||||
'''
|
||||
拟合最小外接矩形，计算矩形中心
|
||||
'''
|
||||
|
||||
rect = cv2.minAreaRect(max_contour)
|
||||
if rect[1][0]-width_reduce > 30 and rect[1][1]-Height_reduce > 30:
|
||||
rect_reduce = (
|
||||
(rect[0][0], rect[0][1]), (rect[1][0] - width_reduce, rect[1][1] - Height_reduce), rect[2])
|
||||
else:
|
||||
rect_reduce = (
|
||||
(rect[0][0], rect[0][1]), (rect[1][0], rect[1][1]), rect[2])
|
||||
|
||||
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
|
||||
box_outside = cv2.boxPoints(rect)
|
||||
# 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
|
||||
startidx = box_outside.sum(axis=1).argmin()
|
||||
box_outside = np.roll(box_outside, 4 - startidx, 0)
|
||||
box_outside = np.intp(box_outside)
|
||||
box_outside = box_outside.reshape((-1, 1, 2)).astype(np.int32)
|
||||
|
||||
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
|
||||
box_reduce = cv2.boxPoints(rect_reduce)
|
||||
startidx = box_reduce.sum(axis=1).argmin()
|
||||
box_reduce = np.roll(box_reduce, 4 - startidx, 0)
|
||||
box_reduce = np.intp(box_reduce)
|
||||
box_reduce = box_reduce.reshape((-1, 1, 2)).astype(np.int32)
|
||||
|
||||
'''
|
||||
提取区域范围内的（x, y）
|
||||
'''
|
||||
mask_inside = np.zeros(binary.shape, np.uint8)
|
||||
cv2.fillPoly(mask_inside, [box_reduce], (255))
|
||||
pixel_point2 = cv2.findNonZero(mask_inside)
|
||||
# result = np.zeros_like(color_image)
|
||||
select_point = []
|
||||
for i in range(pixel_point2.shape[0]):
|
||||
select_point.append(pm[pixel_point2[i][0][1]+box_y1, pixel_point2[i][0][0]+box_x1])
|
||||
select_point = np.array(select_point)
|
||||
pm_seg = select_point.reshape(-1, 3)
|
||||
pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan
|
||||
if pm_seg.size < 100:
|
||||
print("分割点云数量较少，无法拟合平面")
|
||||
continue
|
||||
|
||||
# cv2.imshow('result', point_result)
|
||||
'''
|
||||
拟合平面，计算法向量
|
||||
'''
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(pm_seg)
|
||||
plane_model, inliers = pcd.segment_plane(distance_threshold=self.seg_distance_threshold,
|
||||
ransac_n=5,
|
||||
num_iterations=5000)
|
||||
[a, b, c, d] = plane_model
|
||||
# print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
|
||||
# inlier_cloud = pcd.select_by_index(inliers) # 点云可视化
|
||||
# inlier_cloud.paint_uniform_color([1.0, 0, 0])
|
||||
# outlier_cloud = pcd.select_by_index(inliers, invert=True)
|
||||
# outlier_cloud.paint_uniform_color([0, 1, 0])
|
||||
# o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])
|
||||
|
||||
box_outside = box_outside + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]],[[box_x1, box_y1]]]
|
||||
box = box_reduce + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]
|
||||
|
||||
box[0][0][1], box[0][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[0][0][1], box[0][0][0])
|
||||
box[1][0][1], box[1][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[1][0][1], box[1][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])
|
||||
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], 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], 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], 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], iter_max=Iter_Max_Pixel)
|
||||
else:
|
||||
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])
|
||||
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])
|
||||
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)
|
||||
point_x, point_y, point_z = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center, iter_max=Iter_Max_Pixel)
|
||||
if x_rotation_center<Xmin or x_rotation_center>Xmax or y_rotation_center<Ymin or y_rotation_center>Ymax:
|
||||
continue
|
||||
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
|
||||
if np.isnan(point_x): # 点云值为无效值
|
||||
continue
|
||||
else:
|
||||
if Box_isPoint == True:
|
||||
box_list.append(
|
||||
[[box_point_x1, box_point_y1, box_point_z1],
|
||||
[box_point_x2, box_point_y2, box_point_z2],
|
||||
[box_point_x3, box_point_y3, box_point_z3],
|
||||
[box_point_x4, box_point_y4, box_point_z4]])
|
||||
else:
|
||||
box_list.append([[x1, y1, z1],
|
||||
[x2, y2, z2],
|
||||
[x3, y3, z3],
|
||||
[x4, y4, z4],
|
||||
])
|
||||
if self.cameraType=='RVC':
|
||||
xyz.append([point_x*1000, point_y*1000, point_z*1000])
|
||||
Depth_Z.append(point_z*1000)
|
||||
elif self.cameraType=='Pe':
|
||||
xyz.append([point_x, point_y, point_z])
|
||||
Depth_Z.append(point_z)
|
||||
nx_ny_nz.append([a, b, c])
|
||||
RegionalArea.append(cv2.contourArea(max_contour))
|
||||
uv.append([x_rotation_center, y_rotation_center])
|
||||
seg_point.append(pm_seg)
|
||||
cv2.polylines(img, [box], True, (0, 255, 0), 2)
|
||||
cv2.polylines(img, [box_outside], True, (226, 12, 89), 2)
|
||||
|
||||
_idx = find_position(Depth_Z, RegionalArea, 100, First_Depth)
|
||||
|
||||
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
|
||||
else:
|
||||
cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点
|
||||
|
||||
if Point_isVision==True:
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(seg_point[_idx])
|
||||
plane_model, inliers = pcd.segment_plane(distance_threshold=self.seg_distance_threshold,
|
||||
ransac_n=5,
|
||||
num_iterations=5000)
|
||||
inlier_cloud = pcd.select_by_index(inliers) # 点云可视化
|
||||
inlier_cloud.paint_uniform_color([1.0, 0, 0])
|
||||
outlier_cloud = pcd.select_by_index(inliers, invert=True)
|
||||
outlier_cloud.paint_uniform_color([0, 0, 1])
|
||||
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, pcd2])
|
||||
if save_img_point == 2 or save_img_point ==4:
|
||||
save_img = cv2.resize(img, (720, 540))
|
||||
cv2.imwrite(save_img_name, save_img)
|
||||
return 1, img, xyz[_idx], nx_ny_nz[_idx], box_list[_idx]
|
||||
else:
|
||||
if save_img_point == 2 or save_img_point ==4:
|
||||
save_img = cv2.resize(img,(720, 540))
|
||||
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
|
||||
|
||||
else:
|
||||
print("RVC X Camera capture failed!")
|
||||
return 0, None, None, None, None
|
||||
|
||||
else:
|
||||
print("RVC X Camera is not opened!")
|
||||
return 0, None, None, None, None
|
||||
|
||||
|
||||
def get_position_and_depth(self, Point_isVision=False, Box_isPoint=True, First_Depth =True, Target_pixel_threshold = 200, Iter_Max_Pixel = 30, save_img_point=0, Height_reduce = 30, width_reduce = 30):
|
||||
"""
|
||||
眼在手上，用于料袋顶层抓取，检测料袋相关信息
|
||||
:param Point_isVision: 点云可视化
|
||||
:param Box_isPoint: True 返回点云值； False 返回box相机坐标
|
||||
:param First_Depth: True 返回料袋中心点深度最小的点云值； False 返回面积最大的料袋中心点云值
|
||||
:param Target_pixel_threshold: [int] 设定像素阈值，判断是否可以抓取
|
||||
: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 width_reduce: 检测框的宽内缩像素
|
||||
:return ret: bool 相机是否正常工作
|
||||
:return img: ndarry 返回img
|
||||
:return xyz: list 目标中心点云值形如[x,y,z]
|
||||
:return nx_ny_nz: list 拟合平面法向量，形如[a,b,c]
|
||||
:return box_list: list 内缩检测框四顶点，形如[[x1,y1],[],[],[]]
|
||||
|
||||
"""
|
||||
ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
|
||||
if self.camera_rvc.caminit_isok == True:
|
||||
if ret == 1:
|
||||
if save_img_point != 0:
|
||||
if get_disk_space(path=os.getcwd()) < 15: # 内存小于15G,停止保存数据
|
||||
save_img_point = 0
|
||||
print('系统内存不足，无法保存数据')
|
||||
else:
|
||||
save_path = ''.join([os.getcwd(), '/Vision/model/data/',
|
||||
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))])
|
||||
save_img_name = ''.join([save_path, '.png'])
|
||||
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:
|
||||
cv2.imwrite(save_img_name, img)
|
||||
if save_img_point==3 or save_img_point==4 or save_img_point==5:
|
||||
row_list = list(range(1, img.shape[0], 2))
|
||||
column_list = list(range(1, img.shape[1], 2))
|
||||
pm_save = pm.copy()
|
||||
pm_save1 = np.delete(pm_save, row_list, axis=0)
|
||||
point_new = np.delete(pm_save1, column_list, axis=1)
|
||||
point_new = point_new.reshape(-1, 3)
|
||||
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:
|
||||
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
|
||||
else:
|
||||
flag, det_cpu, scores, masks, category_names = self.model.segment_objects(img)
|
||||
if flag == 1:
|
||||
xyz = []
|
||||
nx_ny_nz = []
|
||||
RegionalArea = []
|
||||
Depth_Z = []
|
||||
uv = []
|
||||
seg_point = []
|
||||
box_list = []
|
||||
target_box_area = 0
|
||||
if Point_isVision==True:
|
||||
pm2 = pm.copy()
|
||||
pm2 = pm2.reshape(-1, 3)
|
||||
pm2 = pm2[~np.isnan(pm2).all(axis=-1), :]
|
||||
pm2[:, 2] = pm2[:, 2] + 0.25
|
||||
pcd2 = o3d.geometry.PointCloud()
|
||||
pcd2.points = o3d.utility.Vector3dVector(pm2)
|
||||
# o3d.visualization.draw_geometries([pcd2])
|
||||
|
||||
for i, item in enumerate(det_cpu):
|
||||
target_box_area = 0
|
||||
# 画box
|
||||
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
|
||||
if self.use_openvino_model == False:
|
||||
label = category_names[int(item[5])]
|
||||
else:
|
||||
label = class_names[int(item[4])]
|
||||
rand_color = (0, 255, 255)
|
||||
score = item[4]
|
||||
org = (int((box_x1 + box_x2) / 2), int((box_y1 + box_y2) / 2))
|
||||
x_center = int((box_x1 + box_x2) / 2)
|
||||
y_center = int((box_y1 + box_y2) / 2)
|
||||
text = '{}|{:.2f}'.format(label, score)
|
||||
cv2.putText(img, text, org=org, fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8,
|
||||
color=rand_color,
|
||||
thickness=2)
|
||||
# 画mask
|
||||
# mask = masks[i].cpu().numpy().astype(int)
|
||||
if self.use_openvino_model == False:
|
||||
mask = masks[i].cpu().data.numpy().astype(int)
|
||||
else:
|
||||
mask = masks[i].astype(int)
|
||||
mask = mask[box_y1:box_y2, box_x1:box_x2]
|
||||
|
||||
# mask = masks[i].numpy().astype(int)
|
||||
h, w = box_y2 - box_y1, box_x2 - box_x1
|
||||
mask_colored = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
mask_colored[np.where(mask)] = rand_color
|
||||
##################################
|
||||
imgray = cv2.cvtColor(mask_colored, cv2.COLOR_BGR2GRAY)
|
||||
# cv2.imshow('mask',imgray)
|
||||
# cv2.waitKey(1)
|
||||
# 2、二进制图像
|
||||
ret, binary = cv2.threshold(imgray, 10, 255, 0)
|
||||
# 阈值 二进制图像
|
||||
# cv2.imshow('bin',binary)
|
||||
# cv2.waitKey(1)
|
||||
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
|
||||
# all_point_list = contours_in(contours)
|
||||
# print(len(all_point_list))
|
||||
max_contour = None
|
||||
max_perimeter = 0
|
||||
for contour in contours: # 排除小分割区域或干扰区域
|
||||
perimeter = cv2.arcLength(contour, True)
|
||||
if perimeter > max_perimeter:
|
||||
max_perimeter = perimeter
|
||||
max_contour = contour
|
||||
|
||||
'''
|
||||
拟合最小外接矩形，计算矩形中心
|
||||
'''
|
||||
|
||||
rect = cv2.minAreaRect(max_contour)
|
||||
if rect[1][0]-width_reduce > 30 and rect[1][1]-Height_reduce > 30:
|
||||
rect_reduce = (
|
||||
(rect[0][0], rect[0][1]), (rect[1][0] - width_reduce, rect[1][1] - Height_reduce), rect[2])
|
||||
else:
|
||||
rect_reduce = (
|
||||
(rect[0][0], rect[0][1]), (rect[1][0], rect[1][1]), rect[2])
|
||||
target_box_area = rect[1][0] * rect[1][1]
|
||||
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
|
||||
box_outside = cv2.boxPoints(rect)
|
||||
# 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
|
||||
startidx = box_outside.sum(axis=1).argmin()
|
||||
box_outside = np.roll(box_outside, 4 - startidx, 0)
|
||||
box_outside = np.intp(box_outside)
|
||||
box_outside = box_outside.reshape((-1, 1, 2)).astype(np.int32)
|
||||
|
||||
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
|
||||
box_reduce = cv2.boxPoints(rect_reduce)
|
||||
startidx = box_reduce.sum(axis=1).argmin()
|
||||
box_reduce = np.roll(box_reduce, 4 - startidx, 0)
|
||||
box_reduce = np.intp(box_reduce)
|
||||
box_reduce = box_reduce.reshape((-1, 1, 2)).astype(np.int32)
|
||||
|
||||
'''
|
||||
提取区域范围内的（x, y）
|
||||
'''
|
||||
mask_inside = np.zeros(binary.shape, np.uint8)
|
||||
cv2.fillPoly(mask_inside, [box_reduce], (255))
|
||||
pixel_point2 = cv2.findNonZero(mask_inside)
|
||||
# result = np.zeros_like(color_image)
|
||||
select_point = []
|
||||
for i in range(pixel_point2.shape[0]):
|
||||
select_point.append(pm[pixel_point2[i][0][1]+box_y1, pixel_point2[i][0][0]+box_x1])
|
||||
select_point = np.array(select_point)
|
||||
pm_seg = select_point.reshape(-1, 3)
|
||||
pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan
|
||||
if pm_seg.size < 100:
|
||||
print("分割点云数量较少，无法拟合平面")
|
||||
continue
|
||||
|
||||
# cv2.imshow('result', point_result)
|
||||
'''
|
||||
拟合平面，计算法向量
|
||||
'''
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(pm_seg)
|
||||
plane_model, inliers = pcd.segment_plane(distance_threshold=self.seg_distance_threshold,
|
||||
ransac_n=5,
|
||||
num_iterations=5000)
|
||||
[a, b, c, d] = plane_model
|
||||
# print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
|
||||
# inlier_cloud = pcd.select_by_index(inliers) # 点云可视化
|
||||
# inlier_cloud.paint_uniform_color([1.0, 0, 0])
|
||||
# outlier_cloud = pcd.select_by_index(inliers, invert=True)
|
||||
# outlier_cloud.paint_uniform_color([0, 1, 0])
|
||||
# o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])
|
||||
|
||||
box_outside = box_outside + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]],[[box_x1, box_y1]]]
|
||||
box = box_reduce + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]
|
||||
|
||||
box[0][0][1], box[0][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[0][0][1], box[0][0][0])
|
||||
box[1][0][1], box[1][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[1][0][1], box[1][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])
|
||||
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], 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], 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], 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], iter_max=Iter_Max_Pixel)
|
||||
else:
|
||||
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])
|
||||
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])
|
||||
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)
|
||||
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) # 标出中心点
|
||||
if np.isnan(point_x): # 点云值为无效值
|
||||
continue
|
||||
else:
|
||||
if Box_isPoint == True:
|
||||
box_list.append(
|
||||
[[box_point_x1, box_point_y1, box_point_z1],
|
||||
[box_point_x2, box_point_y2, box_point_z2],
|
||||
[box_point_x3, box_point_y3, box_point_z3],
|
||||
[box_point_x4, box_point_y4, box_point_z4]])
|
||||
else:
|
||||
box_list.append([[x1, y1, z1],
|
||||
[x2, y2, z2],
|
||||
[x3, y3, z3],
|
||||
[x4, y4, z4],
|
||||
])
|
||||
if target_box_area > img.shape[0]*img.shape[1]*(2/3): # Target_pixel_threshold
|
||||
if self.cameraType == 'RVC':
|
||||
xyz.append([point_x*1000, point_y*1000, point_z*1000])
|
||||
Depth_Z.append(point_z*1000)
|
||||
elif self.cameraType=='Pe':
|
||||
xyz.append([point_x, point_y, point_z])
|
||||
Depth_Z.append(point_z)
|
||||
nx_ny_nz.append([a, b, c])
|
||||
RegionalArea.append(cv2.contourArea(max_contour))
|
||||
uv.append([x_rotation_center, y_rotation_center])
|
||||
seg_point.append(pm_seg)
|
||||
cv2.polylines(img, [box], True, (0, 255, 0), 2)
|
||||
cv2.polylines(img, [box_outside], True, (226, 12, 89), 2)
|
||||
|
||||
_idx = find_position(Depth_Z, RegionalArea, 100, First_Depth)
|
||||
|
||||
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
|
||||
else:
|
||||
cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点
|
||||
|
||||
if Point_isVision==True:
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(seg_point[_idx])
|
||||
plane_model, inliers = pcd.segment_plane(distance_threshold=self.seg_distance_threshold,
|
||||
ransac_n=5,
|
||||
num_iterations=5000)
|
||||
inlier_cloud = pcd.select_by_index(inliers) # 点云可视化
|
||||
inlier_cloud.paint_uniform_color([1.0, 0, 0])
|
||||
outlier_cloud = pcd.select_by_index(inliers, invert=True)
|
||||
outlier_cloud.paint_uniform_color([0, 0, 1])
|
||||
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, pcd2])
|
||||
if save_img_point == 2 or save_img_point ==4:
|
||||
save_img = cv2.resize(img, (720, 540))
|
||||
cv2.imwrite(save_img_name, save_img)
|
||||
return 1, img, xyz[_idx], nx_ny_nz[_idx], box_list[_idx]
|
||||
else:
|
||||
if save_img_point == 2 or save_img_point ==4:
|
||||
save_img = cv2.resize(img,(720, 540))
|
||||
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
|
||||
|
||||
else:
|
||||
print("RVC X Camera capture failed!")
|
||||
return 0, None, None, None, None
|
||||
|
||||
else:
|
||||
print("RVC X Camera is not opened!")
|
||||
return 0, None, None, None, None
|
||||
|
||||
|
||||
def get_take_photo_position(self, Height_reduce = 30, width_reduce = 30):
|
||||
"""
|
||||
检测当前拍照点能否检测到料袋
|
||||
:param Height_reduce:
|
||||
:param width_reduce:
|
||||
:return ret: bool 相机是否正常工作
|
||||
:return img: ndarry 返回img
|
||||
:return find_target: bool 是否有目标
|
||||
:return xyz: list 目标中心点云值，形如[x,y,z]
|
||||
|
||||
"""
|
||||
ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
|
||||
find_target = False
|
||||
if self.camera_rvc.caminit_isok == True:
|
||||
if ret == 1:
|
||||
if self.use_openvino_model == False:
|
||||
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
|
||||
else:
|
||||
flag, det_cpu, scores, masks, category_names = self.model.segment_objects(img)
|
||||
if flag == 1:
|
||||
xyz = []
|
||||
RegionalArea = []
|
||||
Depth_Z = []
|
||||
uv = []
|
||||
for i, item in enumerate(det_cpu):
|
||||
find_target = True
|
||||
# 画box
|
||||
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
|
||||
if self.use_openvino_model == False:
|
||||
label = category_names[int(item[5])]
|
||||
else:
|
||||
label = class_names[int(item[4])]
|
||||
rand_color = (0, 255, 255)
|
||||
score = item[4]
|
||||
org = (int((box_x1 + box_x2) / 2), int((box_y1 + box_y2) / 2))
|
||||
x_center = int((box_x1 + box_x2) / 2)
|
||||
y_center = int((box_y1 + box_y2) / 2)
|
||||
text = '{}|{:.2f}'.format(label, score)
|
||||
cv2.putText(img, text, org=org, fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8,
|
||||
color=rand_color,
|
||||
thickness=2)
|
||||
# 画mask
|
||||
# mask = masks[i].cpu().numpy().astype(int)
|
||||
if self.use_openvino_model == False:
|
||||
mask = masks[i].cpu().data.numpy().astype(int)
|
||||
else:
|
||||
mask = masks[i].astype(int)
|
||||
mask = mask[box_y1:box_y2, box_x1:box_x2]
|
||||
|
||||
# mask = masks[i].numpy().astype(int)
|
||||
h, w = box_y2 - box_y1, box_x2 - box_x1
|
||||
mask_colored = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
mask_colored[np.where(mask)] = rand_color
|
||||
##################################
|
||||
imgray = cv2.cvtColor(mask_colored, cv2.COLOR_BGR2GRAY)
|
||||
# cv2.imshow('mask',imgray)
|
||||
# cv2.waitKey(1)
|
||||
# 2、二进制图像
|
||||
ret, binary = cv2.threshold(imgray, 10, 255, 0)
|
||||
# 阈值 二进制图像
|
||||
# cv2.imshow('bin',binary)
|
||||
# cv2.waitKey(1)
|
||||
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
|
||||
# all_point_list = contours_in(contours)
|
||||
# print(len(all_point_list))
|
||||
max_contour = None
|
||||
max_perimeter = 0
|
||||
for contour in contours: # 排除小分割区域或干扰区域
|
||||
perimeter = cv2.arcLength(contour, True)
|
||||
if perimeter > max_perimeter:
|
||||
max_perimeter = perimeter
|
||||
max_contour = contour
|
||||
|
||||
'''
|
||||
拟合最小外接矩形，计算矩形中心
|
||||
'''
|
||||
|
||||
rect = cv2.minAreaRect(max_contour)
|
||||
if rect[1][0] - width_reduce < 30 or rect[1][1] - Height_reduce < 30:
|
||||
rect_reduce = (
|
||||
(rect[0][0], rect[0][1]), (rect[1][0] - width_reduce, rect[1][1] - Height_reduce),
|
||||
rect[2])
|
||||
else:
|
||||
rect_reduce = (
|
||||
(rect[0][0], rect[0][1]), (rect[1][0], rect[1][1]), rect[2])
|
||||
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
|
||||
box_outside = cv2.boxPoints(rect)
|
||||
# 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
|
||||
startidx = box_outside.sum(axis=1).argmin()
|
||||
box_outside = np.roll(box_outside, 4 - startidx, 0)
|
||||
box_outside = np.intp(box_outside)
|
||||
box_outside = box_outside.reshape((-1, 1, 2)).astype(np.int32)
|
||||
|
||||
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
|
||||
box_reduce = cv2.boxPoints(rect_reduce)
|
||||
startidx = box_reduce.sum(axis=1).argmin()
|
||||
box_reduce = np.roll(box_reduce, 4 - startidx, 0)
|
||||
box_reduce = np.intp(box_reduce)
|
||||
box_reduce = box_reduce.reshape((-1, 1, 2)).astype(np.int32)
|
||||
|
||||
box_outside = box_outside + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]],[[box_x1, box_y1]]]
|
||||
box = box_reduce + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]
|
||||
|
||||
box[0][0][1], box[0][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[0][0][1], box[0][0][0])
|
||||
box[1][0][1], box[1][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[1][0][1], box[1][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])
|
||||
|
||||
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)
|
||||
point_x, point_y, point_z = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center)
|
||||
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
|
||||
if np.isnan(point_x): # 点云值为无效值
|
||||
continue
|
||||
else:
|
||||
if self.cameraType == 'RVC':
|
||||
xyz.append([point_x * 1000, point_y * 1000, point_z * 1000])
|
||||
Depth_Z.append(point_z * 1000)
|
||||
elif self.cameraType == 'Pe':
|
||||
xyz.append([point_x, point_y, point_z])
|
||||
Depth_Z.append(point_z)
|
||||
RegionalArea.append(cv2.contourArea(max_contour))
|
||||
uv.append([x_rotation_center, y_rotation_center])
|
||||
|
||||
cv2.polylines(img, [box], True, (0, 255, 0), 2)
|
||||
cv2.polylines(img, [box_outside], True, (226, 12, 89), 2)
|
||||
|
||||
_idx = find_position(Depth_Z, RegionalArea, 100,True)
|
||||
|
||||
if _idx == None:
|
||||
return 1, img, find_target, None
|
||||
else:
|
||||
cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点
|
||||
return 1, img, find_target, xyz[_idx]
|
||||
else:
|
||||
return 0, None, None
|
||||
else:
|
||||
return 0, None, None
|
||||
|
||||
pass
|
||||
|
||||
def get_center_position(self):
|
||||
""
|
||||
'''
|
||||
:param api: None
|
||||
:return: ret , img, (x,y,z) 图像中心点位置对应的点云数据
|
||||
'''
|
||||
ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
|
||||
if self.camera_rvc.caminit_isok == True:
|
||||
if ret:
|
||||
if pm != 'None':
|
||||
pm_shape_y = pm.shape[0]
|
||||
pm_shape_x = pm.shape[1]
|
||||
center_point = [int(pm_shape_y/2), int(pm_shape_x/2)]
|
||||
point_x, point_y, point_z = remove_nan_mean_value(pm, center_point[0], center_point[1])
|
||||
return img, [point_x, point_y, point_z]
|
||||
else:
|
||||
print('点云值为NAN')
|
||||
return None, None
|
||||
else:
|
||||
return None, None
|
||||
else:
|
||||
return None, None
|
||||
|
||||
def release(self):
|
||||
self.camera_rvc.release()
|
||||
self.model.clear()
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user