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

上传

Browse Source
This commit is contained in:
cdeyw
2025-08-13 11:42:19 +08:00
parent 92778fa76a
commit b3f417fb3f
10 changed files with 2226 additions and 20702 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.idea/AutoControlSystem.iml generated
View File

@ -2,7 +2,7 @@
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="jdk" jdkName="Python 3.9" jdkType="Python SDK" />
<orderEntry type="jdk" jdkName="rob" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="PyDocumentationSettings">

2
.idea/misc.xml generated
View File

@ -3,5 +3,5 @@
<component name="Black">
<option name="sdkName" value="rob" />
</component>
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9" project-jdk-type="Python SDK" />
<component name="ProjectRootManager" version="2" project-jdk-name="rob" project-jdk-type="Python SDK" />
</project>

2
CU/Feeding.py
View File

@ -35,7 +35,7 @@ class ResetStatus(Enum):
class FeedStatus(IntEnum):
FNone = 0
FNone = 0 #
FStart = 1
FCheck = 2
FMid = 3

4
Seting.ini
View File

@ -1,7 +1,7 @@
[Main]
[Robot_Feed]
ipaddress = 192.168.20.4
ipaddress = 127.0.0.1
port = 502
j1_min = -150
j1_max = +150
@ -47,7 +47,7 @@ photo_v5 = 0.0
photo_w5 = 1.0
linecount = 2
remain_linename = 1
remain_count = 0
remain_count = 999
io_take_addr = 8
io_zip_addr = 11
io_shake_addr = 12

4
Vision/bag_collection.py
View File

@ -31,7 +31,7 @@ class DetectionBag:
model_path = ''.join([os.getcwd(), '/Vision/model/pt/bag_collection.pt'])
self.camera_rvc = camera_pe()
self.imgsz = 640
self.cuda = 'cpu'
self.cuda = '0'
self.conf = 0.40
self.iou = 0.45
self.model = AutoBackend(model_path, device=torch.device(self.cuda))
@ -82,7 +82,7 @@ class DetectionBag:
if Bag==True:
if get_disk_space(path=os.getcwd()) < 15: # 内存小于15G,停止保存数据
save_img_point = 0
print('系统内存不足，无法保存数据')
print('硬盘空间不足，无法保存数据')
else:
save_path = ''.join([os.getcwd(), '/Vision/model/data/',
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))])

509
Vision/camera_coordinate_dete.py
View File

@ -64,18 +64,18 @@ class Detection:
return
# 加载openvino-seg
if self.use_openvino_model:
model_path = ''.join([os.getcwd(), '/Vision/model/openvino/one_bag.xml'])
model_path = ''.join([os.getcwd(), './Vision/model/openvino/one_bag.xml'])
device = 'CPU'
self.model_seg = yolov8_segment_openvino(model_path, device, conf_thres=0.6, iou_thres=0.6)
# 加载pt-seg
if self.use_seg_pt_model:
model_path = ''.join([os.getcwd(), '/Vision/model/pt/one_bag.pt'])
model_path = ''.join([os.getcwd(), './Vision/model/pt/one_bag.pt'])
device = 'cpu'
self.model_seg = yolov8_segment()
self.model_seg.load_model(model_path, device)
# 加载pt-pose
if self.use_pose_model:
model_path = ''.join([os.getcwd(), '/Vision/model/pt/one_bag_pose.pt'])
model_path = ''.join([os.getcwd(), './Vision/model/pt/one_bag_pose.pt'])
device = 'cpu'
self.model_pose = yolov8_pose(model_path, device)
@ -103,7 +103,11 @@ class Detection:
:return box_list: list 内缩检测框四顶点，形如[[x1,y1],[],[],[]]
"""
ret, img, pm, _depth_align = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
# ret, img, pm, _depth_align = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
ret = 1
pm1 = np.loadtxt('D:\pychram_rob\AutoControlSystem-git\Vision\model\data\\2024_11_29_10_05_58.xyz', dtype=np.float32)
img = cv2.imread('D:\pychram_rob\AutoControlSystem-git\Vision\model\data\\2024_11_29_10_05_58.png')
pm = pm1.reshape((img.shape[0], img.shape[1], 3))
if self.camera_rvc.caminit_isok == True:
if ret == 1:
if save_img_point != 0:
@ -137,10 +141,10 @@ class Detection:
real_model_pro_isPose = False
if real_model_pro_isPose:
flag, det_cpu, category_names, score_list = self.model_pose.model_inference(img)
flag, det_cpu, category_names, score_list = self.model_pose.model_inference(img)#用关键点检测模型
else:
if self.use_openvino_model == False:
flag, det_cpu, dst_img, masks, category_names = self.model_seg.model_inference(img, 0)
flag, det_cpu, dst_img, masks, category_names = self.model_seg.model_inference(img, 0) #用分割模型
else:
flag, det_cpu, scores, masks, category_names = self.model_seg.segment_objects(img)
if flag == 1:
@ -160,7 +164,7 @@ class Detection:
pcd2.points = o3d.utility.Vector3dVector(pm2)
# o3d.visualization.draw_geometries([pcd2])
for i, item in enumerate(det_cpu):
for i, item in enumerate(det_cpu):#提供检测到的框信息
# 画box
if real_model_pro_isPose:
@ -172,7 +176,7 @@ class Detection:
box_y2 = item[3][1]
pass
else:
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)#找最近的框的1，3角点坐标
if self.use_openvino_model == False:
label = category_names[int(item[5])]
score = item[4]
@ -222,7 +226,7 @@ class Detection:
# box_outside = [[[item[0][0], item[0][1]]],
# [[item[1][0], item[1][1]]],
# [[item[3][0], item[3][1]]],
# [[item[2][0], item[2][1]]]] # 外框
# [[item[2][0], item[2][1]]]]# 外框
box_outside = np.array(box_outside)
box_outside = box_outside.reshape((-1, 1, 2))
# box_outside = np.array(box_outside)
@ -246,13 +250,13 @@ class Detection:
# 阈值 二进制图像
# cv2.imshow('bin',binary)
# cv2.waitKey(1)
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)#检测物体轮廓，在灰度化和二值化之后，contours是轮廓信息
# 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)
perimeter = cv2.arcLength(contour, True)#计算周长
if perimeter > max_perimeter:
max_perimeter = perimeter
max_contour = contour
@ -260,8 +264,7 @@ class Detection:
'''
拟合最小外接矩形，计算矩形中心
'''
rect = cv2.minAreaRect(max_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])
@ -270,23 +273,23 @@ class Detection:
(rect[0][0], rect[0][1]), (rect[1][0], rect[1][1]), rect[2])
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
box_outside = cv2.boxPoints(rect)
box_outside = cv2.boxPoints(rect)#计算顶点坐标
# 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
startidx = box_outside.sum(axis=1).argmin()
box_outside = np.roll(box_outside, 4 - startidx, 0)
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.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_x1, box_y1]]]#我也当他是锚点
'''
提取区域范围内的（x, y）
@ -297,9 +300,9 @@ class Detection:
# 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.append(pm[pixel_point2[i][0][1]+box_y1, pixel_point2[i][0][0]+box_x1])#我为什么要加这个box_y1和box_x1呢？是因为mask取出来不是原图的坐标了，box_y1和box_x1相当于mask在原图的锚点，用来帮助剪切后的形状回到原图的位置
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
if pm_seg.size < 100:
print("分割点云数量较少，无法拟合平面")
@ -309,24 +312,24 @@ class Detection:
'''
拟合平面，计算法向量
'''
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pm_seg)
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
num_iterations=5000)#平面分割，平面拟合，plane_model拟合平面的系数
[a, b, c, d] = plane_model#ax+by+cz+d=0，a,b,c就是法向量
# 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[0][0][1], box[0][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[0][0][1], box[0][0][0])
print(box)
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有没有超过点云范围，pm直接是整个图片的点云，box只是分割模型识别的框
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:
if Box_isPoint == True:#保证box的坐标能被传回来，如果这个点上的没有，就用旁边的均值
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)
@ -338,10 +341,10 @@ class Detection:
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)
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), 2, (255, 255, 255), 3) # 标出中心点
cv2.circle(img, (x_rotation_center, y_rotation_center), 2, (255, 255, 255), 3) # 标出中心点，只是标出来
if np.isnan(point_x): # 点云值为无效值
continue
else:
@ -350,14 +353,14 @@ class Detection:
[[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]])
[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':
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':
@ -366,12 +369,12 @@ class Detection:
if real_model_pro_isPose:
RegionalArea.append(0)
else:
RegionalArea.append(cv2.contourArea(max_contour))
nx_ny_nz.append([a, b, c])
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)
RegionalArea.append(cv2.contourArea(max_contour))#计算面积
nx_ny_nz.append([a, b, c])#法向量
uv.append([x_rotation_center, y_rotation_center])#中心点x,y
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)
@ -415,276 +418,272 @@ class Detection:
print("RVC X Camera is not opened!")
return 0, None, None, None, None
def read_data(self, xyz_path, img_path):
pm1 = np.loadtxt(xyz_path, dtype=np.float32)
img = cv2.imread(img_path)
pm = pm1.reshape((img.shape[0], img.shape[1], 3))
return img, pm
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],[],[],[]]
def save_data(self, img, pm, save_img_point, save_path):
if save_img_point == 0:
return
if not os.path.exists(os.path.dirname(save_path)):
os.makedirs(os.path.dirname(save_path))
save_img_name = save_path + '.png'
save_point_name = save_path + '.xyz'
"""
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:
if save_img_point in (1, 3):
cv2.imwrite(save_img_name, img)
if save_img_point==3 or save_img_point==4 or save_img_point==5:
if save_img_point in (3, 4):
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])]
def model_inference(self, img, Use_Pose_Model_Pro):
real_model_pro_isPose = self.use_pose_model and Use_Pose_Model_Pro
if real_model_pro_isPose:
flag, det_cpu, category_names, score_list = self.model_pose.model_inference(img)
return flag, det_cpu, category_names, score_list, real_model_pro_isPose
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:
if self.use_openvino_model:
flag, det_cpu, scores, masks, category_names = self.model_seg.segment_objects(img)
else:
flag, det_cpu, dst_img, masks, category_names = self.model_seg.model_inference(img, 0)
return flag, det_cpu, category_names, masks, real_model_pro_isPose
def get_box_3d_points(self, pm, box, Box_isPoint=True, Iter_Max_Pixel=30):
"""
输入: box 为 (4, 2) 像素坐标 [[x1,y1], ..., [x4,y4]]
输出: 4个点的3D坐标 [x, y, z]
"""
box = np.array(box).reshape(-1, 2) # 强制为 (4, 2)
pts_3d = []
for pt in box:
# 确保 pt 是 [x, y] 结构
x_img, y_img = int(pt[0]), int(pt[1])
if Box_isPoint:
x3d, y3d, z3d = remove_nan_mean_value(pm, y_img, x_img, iter_max=Iter_Max_Pixel)
else:
x3d, y3d, z3d = uv_to_XY(x_img, y_img)
pts_3d.append([x3d, y3d, z3d])
return pts_3d
def process_mask_and_get_box(self, i,item, masks, pm, box_coords, Height_reduce, width_reduce, real_model_pro_isPose, use_openvino_model):
"""
处理mask，提取区域点云和box（内缩和外框）
返回 box (内缩), box_outside(外框), pm_seg(区域点云)
"""
if real_model_pro_isPose:
# 关键点模型的box四点坐标已经给出
mask = np.zeros(pm.shape[:2], dtype=np.uint8)
if item[0][0] < item[1][0]:
arr = [[item[0][0], item[0][1]], [item[1][0], item[1][1]], [item[3][0], item[3][1]], [item[2][0], item[2][1]]]
else:
arr = [[item[3][0], item[3][1]], [item[2][0], item[2][1]], [item[0][0], item[0][1]], [item[1][0], item[1][1]]]
box = shrink_quadrilateral(arr, Height_reduce)
pts = np.array(box, np.int32)
cv2.fillPoly(mask, [pts], 255)
pm_seg = pm[mask == 255]
box = np.array(box).reshape((-1, 1, 2)).astype(np.int32)
box_outside = np.array(arr).reshape((-1, 1, 2)).astype(np.int32)
else:
# 分割模型
box_x1, box_y1, box_x2, box_y2 = box_coords
if not use_openvino_model:
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
##################################
mask_colored[np.where(mask)] = (0, 255, 255)
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: # 排除小分割区域或干扰区域
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])
if rect[1][0] - width_reduce > 30 and rect[1][1] - Height_reduce > 30:
rect_reduce = (rect[0], (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)
rect_reduce = rect
box_outside = cv2.boxPoints(rect)
startidx = box_outside.sum(axis=1).argmin()
box_outside = np.roll(box_outside, 4 - startidx, 0).astype(np.int32).reshape((-1, 1, 2))
# 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_reduce = np.roll(box_reduce, 4 - startidx, 0).astype(np.int32).reshape((-1, 1, 2))
box_outside += np.array([[[box_x1, box_y1]]] * 4)
box = box_reduce + np.array([[[box_x1, box_y1]]] * 4)
'''
提取区域范围内的（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
select_point.append(pm[pixel_point2[i][0][1] + box_y1, pixel_point2[i][0][0] + box_x1])
pm_seg = np.array(select_point).reshape(-1, 3)
# cv2.imshow('result', point_result)
'''
拟合平面，计算法向量
'''
pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=1), :]
return box, box_outside, pm_seg,max_contour
def fit_plane_and_get_normal(self, pm_seg):
if pm_seg.shape[0] < 100:
print("分割点云数量较少，无法拟合平面")
return None
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])
return [a, b, c]
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]]]
def get_position_test(self, Use_Pose_Model_Pro=False, 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):
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:
if self.camera_rvc.caminit_isok:
print("RVC X Camera is not opened!")
return 0, None, None, None, None
# 这里示例用固定路径，建议修改为参数输入
xyz_path = 'D:/pychram_rob/AutoControlSystem-git/Vision/model/data/2024_11_29_10_05_58.xyz'
img_path = 'D:/pychram_rob/AutoControlSystem-git/Vision/model/data/2024_11_29_10_05_58.png'
img, pm = self.read_data(xyz_path, img_path)
if save_img_point != 0:
free_space = get_disk_space(path=os.getcwd())
if free_space < 15:
print('系统内存不足，无法保存数据')
save_img_point = 0
else:
save_path = os.path.join(os.getcwd(), 'Vision/model/data/',
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime()))
self.save_data(img, pm, save_img_point, save_path)
flag, det_cpu, category_names, extra, real_model_pro_isPose = self.model_inference(img, Use_Pose_Model_Pro)
if flag != 1:
print("模型推理失败")
return 1, img, None, None, None
xyz_list = []
normal_list = []
area_list = []
depth_list = []
uv_list = []
seg_point_list = []
box_list = []
for i, item in enumerate(det_cpu):
if real_model_pro_isPose:
box_coords = None
else:
box_coords = item[0:4].astype(np.int32)
masks = extra if not real_model_pro_isPose else None
box, box_outside, pm_seg,max_contour = self.process_mask_and_get_box(i,
item, masks, pm, box_coords, Height_reduce, width_reduce,
real_model_pro_isPose, self.use_openvino_model)
if pm_seg.shape[0] < 100:
continue
normal = self.fit_plane_and_get_normal(pm_seg)
if normal is None:
continue
# 计算中心点坐标
if real_model_pro_isPose:
x_center = int((item[0][0] + item[1][0] + item[2][0] + item[3][0]) / 4)
y_center = int((item[0][1] + item[1][1] + item[2][1] + item[3][1]) / 4)
else:
x_center = int(np.mean(box[:, 0, 0]))
y_center = int(np.mean(box[:, 0, 1]))
# 确保中心点坐标在范围内
if x_center < Xmin or x_center > Xmax or y_center < Ymin or y_center > Ymax:
continue
# 获取中心点点云坐标
point_x, point_y, point_z = remove_nan_mean_value(pm, y_center, x_center, iter_max=Iter_Max_Pixel)
if np.isnan(point_x):
continue
# 计算面积（如果有轮廓）
if real_model_pro_isPose:
area = 0
else:
area = cv2.contourArea(max_contour) if 'max_contour' in locals() else 0
xyz = [point_x, point_y, point_z]
if self.cameraType == 'RVC':
xyz = [v * 1000 for v in xyz] # 换单位为mm
depth_list.append(point_z * 1000)
else:
depth_list.append(point_z)
xyz_list.append(xyz)
normal_list.append(normal)
area_list.append(area)
uv_list.append([x_center, y_center])
seg_point_list.append(pm_seg)
box = box.reshape(-1,2)
print("box.shape:", box.shape)
print("box example:", box)
box_3d_points = self.get_box_3d_points(pm, box, Box_isPoint)
box_list.append(box_3d_points)
# 画图示例
cv2.polylines(img, [box], True, (0, 255, 0), 2)
cv2.polylines(img, [box_outside], True, (226, 12, 89), 2)
cv2.circle(img, (x_center, y_center), 2, (255, 255, 255), 3)
# 选取最终结果索引
idx = find_position(depth_list, area_list, 100, First_Depth)
if idx is None:
return 1, img, None, None, None
# 标记最终中心点
cv2.circle(img, (uv_list[idx][0], uv_list[idx][1]), 30, (0, 0, 255), 10)
# 点云可视化示例
if Point_isVision:
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(seg_point_list[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])
# 保存图像
if save_img_point in (2, 4):
save_img = cv2.resize(img, (720, 540))
save_path = os.path.join(os.getcwd(), 'Vision/model/data/',
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime()))
cv2.imwrite(save_path + '.png', save_img)
return 1, img, xyz_list[idx], normal_list[idx], box_list[idx]
def get_take_photo_position(self, Height_reduce = 30, width_reduce = 30):
"""

2
Vision/get_position_test.py
View File

@ -13,7 +13,7 @@ import cv2
detection = Detection()
while True:
ret, img, xyz, nx_ny_nz, box = detection.get_position()
ret, img, xyz, nx_ny_nz, box = detection.get_position_test()
if ret==1:
print('xyz点云坐标：', xyz)
print('nx_ny_nz法向量：', nx_ny_nz)

8
Vision/tool/utils.py
View File

@ -144,8 +144,8 @@ def out_bounds_dete(pm_y, pm_x, piont_y, piont_x):
def remove_nan_mean_value(pm, y, x, iter_max=50):
y, x = out_bounds_dete(pm.shape[0], pm.shape[1], y, x)
point_x, point_y, point_z = pm[y, x]
if np.isnan(point_x):
point_x, point_y, point_z = pm[y, x]#得到这个位置的点云的坐标
if np.isnan(point_x):#如果这个位置是nan，找到周围50个像素的范围内的点云，并求平均来代替这个点的坐标
point_x_list = []
point_y_list = []
point_z_list = []
@ -154,7 +154,7 @@ def remove_nan_mean_value(pm, y, x, iter_max=50):
pm_shape_x = pm.shape[1]
remove_nan_isok = False
print('Nan值去除')
while iter_current < iter_max:
while iter_current < iter_max:#这个邻域内不是nan的点就被放到列表中
# 计算开始点
if y - iter_current > 0:
y_start = y - iter_current
@ -180,7 +180,7 @@ def remove_nan_mean_value(pm, y, x, iter_max=50):
point_z_list.append(pm[y_current, x_current][2])
len_point_x = len(point_x_list)
if len_point_x > 0:
if len_point_x > 0:#计算x,y,z的均值
point_x = sum(point_x_list)/len_point_x
point_y = sum(point_y_list)/len_point_x
point_z = sum(point_z_list)/len_point_x

4
Vision/yolo/yolov8_pt_seg.py
View File

@ -332,10 +332,10 @@ class yolov8_segment():
# NMS
det = non_max_suppression(preds, conf_thres=0.4, iou_thres=0.4, nc=len(self.model.CLASSES))[0]
if det.shape[0] != 0:
# bbox还原至原图尺寸
# box还原至原图尺寸
det[:, :4] = scale_boxes(img.shape[2:], det[:, :4], ori_img.shape)
# mask转换成原图尺寸并做裁剪
masks = process_mask(proto[0], det[:, 6:], det[:, :4], img.shape[2:], ori_img.shape[0:2])
masks = process_mask(proto[0], det[:, 6:], det[:, :4], img.shape[2:], ori_img.shape[0:2])#mask尺寸为什么与box尺寸一样
category_names = self.model.CLASSES
# 画图
# result_frame = plot_result(det.cpu().data.numpy(), ori_img, masks, category_names)

22297
log/log.log
View File

File diff suppressed because it is too large Load Diff