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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
FrankCV2048
2025-02-15 21:38:49 +08:00
parent 1603fe9088 910b832ba9
commit 5f00ea6297
14 changed files with 766 additions and 1578 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.idea/.gitignore generated vendored Normal file
View File

@ -0,0 +1,3 @@
# 默认忽略的文件
/shelf/
/workspace.xml

12
.idea/AutoControlSystem.iml generated Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="jdk" jdkName="rob" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="PyDocumentationSettings">
<option name="format" value="PLAIN" />
<option name="myDocStringFormat" value="Plain" />
</component>
</module>

6
.idea/inspectionProfiles/profiles_settings.xml generated Normal file
View File

@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

7
.idea/misc.xml generated Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="Black">
<option name="sdkName" value="rob" />
</component>
<component name="ProjectRootManager" version="2" project-jdk-name="rob" project-jdk-type="Python SDK" />
</project>

8
.idea/modules.xml generated Normal file
View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/AutoControlSystem.iml" filepath="$PROJECT_DIR$/.idea/AutoControlSystem.iml" />
</modules>
</component>
</project>

6
.idea/vcs.xml generated Normal file
View File

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>

62
EMV.py Normal file
View File

@ -0,0 +1,62 @@
import socket
import binascii
# 网络继电器的 IP 和端口
HOST = '192.168.20.18'
PORT = 50000
valve_commands = {
1: {
'open': '00000000000601050000FF00',
'close': '000000000006010500000000',
},
2: {
'open': '00000000000601050001FF00',
'close': '000000000006010500010000',
},
3: {
'open': '00000000000601050002FF00',
'close': '000000000006010500020000',
}
}
# 将十六进制字符串转换为字节数据并发送
def send_command(command):
byte_data = binascii.unhexlify(command)
# 创建套接字并连接到继电器
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
sock.connect((HOST, PORT))
sock.send(byte_data)
response = sock.recv(1024)
return response
# 控制电磁阀打开
def open(grasp, throw, shake):
if grasp:
print("打开电磁阀 1")#抓取
send_command(valve_commands[1]['open'])
if throw:
print("打开电磁阀 2")#扔袋
send_command(valve_commands[2]['open'])
if shake:
print("打开电磁阀 3")#震动
send_command(valve_commands[3]['open'])
# 控制电磁阀关闭
def close(grasp, throw, shake ):
if grasp:
print("关闭电磁阀 1")
send_command(valve_commands[1]['close'])
if throw:
print("关闭电磁阀 2")
send_command(valve_commands[2]['close'])
if shake:
print("关闭电磁阀 3")
send_command(valve_commands[3]['close'])
close(True,True,True)#参数传True和False

8
Trace/handeye_calibration.py
View File

@ -59,7 +59,7 @@ def getPosition(x,y,z,a,b,c,rotation,points):
camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ') #相对目录且分隔符采用os.sep
# robot2base = rotation
# camera2base = robot2base @ camera2robot
target_position = np.dot(camera2robot, target)
target_position_raw = np.dot(camera2robot, target)
corner_points_camera = np.asarray(points)
corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3]
@ -76,8 +76,14 @@ def getPosition(x,y,z,a,b,c,rotation,points):
# 单位化方向向量
short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
delta = -30#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
angle = np.asarray([a,b,c])
noraml = camera2robot[:3, :3]@angle
normal_vector = noraml / np.linalg.norm(noraml)
target_position = target_position_raw[:3] + delta * normal_vector # target_position 沿法向量移动
noraml_base = vec2rpy(noraml,short_edge_direction)
return target_position,noraml_base

3
Vision/camera_coordinate_dete.py
View File

@ -75,7 +75,7 @@ class Detection:
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 = 60, width_reduce = 100, Xmin =330, Xmax = 1050, Ymin =290 ,Ymax = 690 ):
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: 点云可视化
@ -618,6 +618,7 @@ class Detection:
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):
"""
检测当前拍照点能否检测到料袋

494
Vision/camera_coordinate_dete_planevison.py Normal file
View File

@ -0,0 +1,494 @@
#!/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
from Vision.tool.CameraRVC import camera_rvc
from Vision.tool.CameraPe import camera_pe
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
from Vision.tool.utils import fit_plane_vision
import os
class Detection_plane_vsion:
def __init__(self, use_openvino_model=False, cameraType = 'RVC'):
self.use_openvino_model = use_openvino_model
self.cameraType = cameraType
if self.use_openvino_model == False:
model_path = ''.join([os.getcwd(), '/Vision/model/pt/one_bag.pt'])
device = 'cpu'
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'
self.model = yolov8_segment_openvino(model_path, device, conf_thres=0.6, iou_thres=0.6)
img_path = ''.join([os.getcwd(), '/Vision/model/data/2025_01_16_08_47_02.png'])
point_path = ''.join([os.getcwd(), '/Vision/model/data/2025_01_16_08_47_02.xyz'])
self.img = cv2.imread(img_path)
self.img = cv2.resize(self.img,((int(self.img.shape[1]/2),int(self.img.shape[0]/2))))
self.point = np.loadtxt(point_path).reshape(self.img.shape[0], self.img.shape[1], 3)
self.seg_distance_threshold = 5
pass
def get_position(self, Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,
save_img_point=0, Height_reduce=60, width_reduce=100, Xmin=0, Xmax=1050, Ymin=0, 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() # 拍照，获取图像及
ret = 1
img = self.img
pm = self.point
# self.camera_rvc.caminit_isok = True
if 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('[a, b, c, d]',[a, b, c, d] )
# 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]])
print(box_list)
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:
# 拟合平面展示
plane_vision = fit_plane_vision(box_list, [a, b, c, d])
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, plane_vision])
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 = 1
img = self.img
pm = self.point
find_target = False
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
def release(self):
self.model.clear()

75
Vision/tool/utils.py
View File

@ -151,6 +151,7 @@ def remove_nan(pm, y, x):
break
return point_x, point_y, point_z
def get_disk_space(path='C:'):
usage = psutil.disk_usage(path)
@ -164,19 +165,27 @@ def get_disk_space(path='C:'):
def find_position(Depth_Z, RegionalArea, RegionalArea_Threshold, first_depth=True):
if first_depth == True:
sorted_id = sorted(range(len(Depth_Z)), key=lambda k: Depth_Z[k], reverse=False)
Depth_Z1 = [Depth_Z[i] for i in sorted_id]
RegionalArea1 = [RegionalArea[i] for i in sorted_id]
for i in range(len(Depth_Z1)):
if RegionalArea1[i] > RegionalArea_Threshold:
return sorted_id[i]
# Depth_Z1 = [Depth_Z[i] for i in sorted_id]
# RegionalArea1 = [RegionalArea[i] for i in sorted_id]
# for i in range(len(Depth_Z1)):
# if RegionalArea1[i] > RegionalArea_Threshold:
# return sorted_id[i]
if len(sorted_id)>0:
return sorted_id[0]
else:
return
else:
sorted_id = sorted(range(len(RegionalArea)), key=lambda k: RegionalArea[k], reverse=True)
Depth_Z1 = [Depth_Z[i] for i in sorted_id]
RegionalArea1 = [RegionalArea[i] for i in sorted_id]
for i in range(len(Depth_Z1)):
if RegionalArea1[i] > RegionalArea_Threshold:
return sorted_id[i]
# Depth_Z1 = [Depth_Z[i] for i in sorted_id]
# RegionalArea1 = [RegionalArea[i] for i in sorted_id]
# for i in range(len(Depth_Z1)):
# if RegionalArea1[i] > RegionalArea_Threshold:
# return sorted_id[i]
if len(sorted_id)>0:
return sorted_id[0]
else:
return
class_names = ['box', 'other']
@ -328,6 +337,52 @@ def draw_comparison(img1, img2, name1, name2, fontsize=2.6, text_thickness=3):
def fit_plane_vision(box_list, normal_vector):
plane_x = []
plane_y = []
plane_z = []
print(box_list)
plane_x.append(box_list[0][0][0])
plane_x.append(box_list[0][1][0])
plane_x.append(box_list[0][2][0])
plane_x.append(box_list[0][3][0])
plane_y.append(box_list[0][0][1])
plane_y.append(box_list[0][1][1])
plane_y.append(box_list[0][2][1])
plane_y.append(box_list[0][3][1])
plane_z.append(box_list[0][0][2])
plane_z.append(box_list[0][1][2])
plane_z.append(box_list[0][2][2])
plane_z.append(box_list[0][3][2])
# 定义平面方程的参数
a = normal_vector[0]
b = normal_vector[1]
c = normal_vector[2]
d = normal_vector[3]
# 定义平面的范围
x_range = (int(min(plane_x)), int(max(plane_x)))
y_range = (int(min(plane_y)), int(max(plane_y)))
z_range = (int(min(plane_z)), int(max(plane_z)))
# 生成平面网格
x = np.linspace(x_range[0], x_range[1], 10)
y = np.linspace(y_range[0], y_range[1], 20)
X, Y = np.meshgrid(x, y)
Z = -(a * X + b * Y + d) / c # 根据平面方程计算 Z 坐标
# 确保 Z 坐标在指定范围内
Z = np.clip(Z, z_range[0], z_range[1])
# 创建 TriangleMesh 对象
import open3d as o3d
plane_mesh = o3d.geometry.TriangleMesh()
plane_mesh.vertices = o3d.utility.Vector3dVector(np.vstack((X.ravel(), Y.ravel(), Z.ravel())).T)
plane_mesh.triangles = o3d.utility.Vector3iVector(
np.array([[i, i + 1, i + 100] for i in range(99)] + [[i + 1, i + 101, i + 100] for i in range(99)]))
plane_mesh.paint_uniform_color([1, 0.5, 0.5]) # 设置平面颜色
return plane_mesh

23
get_position_test.py
View File

@ -11,6 +11,7 @@ import cv2
import os
from Vision.camera_coordinate_dete import Detection
from Vision.camera_coordinate_dete_planevison import Detection_plane_vsion
from Trace.handeye_calibration import *
from Vision.tool.utils import get_disk_space
from Vision.tool.utils import remove_nan_mean_value
@ -41,6 +42,23 @@ def detectionPosition_test():
cv2.imshow('img', img)
cv2.waitKey(0)
def Detection_plane_vsion_test():
detection = Detection_plane_vsion()
while True:
ret, img, xyz, nx_ny_nz, box = detection.get_position(Point_isVision=True, save_img_point=1)
if ret==1:
print('xyz点云坐标：', xyz)
print('nx_ny_nz法向量：', nx_ny_nz)
print('矩形框四顶点：', box)
# img = cv2.resize(img,(720, 540))
if xyz!=None:
transformation_matrix = R_matrix(521.534, 0.705, 850.03, 0.0, 90.0, 0.0) # (x, y, z, u, v, w)
target_position, noraml_base = getPosition(xyz[0], xyz[1], xyz[2], nx_ny_nz[0], nx_ny_nz[1], nx_ny_nz[2], transformation_matrix, box)
print("target_position:", target_position)
print("noraml_base", noraml_base)
cv2.imshow('img', img)
cv2.waitKey(0)
def take_photo_position_test():
detection = Detection()
while True:
@ -65,6 +83,7 @@ def detectionPerson_test():
else:
cv2.destroyAllWindows()
print("person", person)
# video_path = ''.join([os.getcwd(), '/Vision/model/data/1.mp4'])
# cam = cv2.VideoCapture(video_path)
@ -77,7 +96,6 @@ def detectionPerson_test():
# cv2.waitKey(1)
# else:
# break
def HIK_Camera_test():
MY_CAMERA = camera_HIK("192.168.1.121", 554, "admin", "zlzk.123")
while True:
@ -86,6 +104,7 @@ def HIK_Camera_test():
img = cv2.resize(frame, (800, 600))
cv2.imshow('img', img)
cv2.waitKey(1)
def detectionBagNum_test():
detectionBagNum = DetectionBagNum()
video_path = ''.join([os.getcwd(), '/Vision/model/data/2.mp4'])
@ -115,4 +134,4 @@ def bag_collection_test():
if __name__ == '__main__':
detectionPosition_test()
Detection_plane_vsion_test()

1597
log/log.log
View File

File diff suppressed because it is too large Load Diff

40
log/log.log.2024-12-14 Normal file
View File

@ -0,0 +1,40 @@
2024-12-14 16:10:42 - INFO - <20><><EFBFBD><EFBFBD>ϵͳ
2024-12-14 16:11:15 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:14:02 - INFO - <20>ƶ<EFBFBD><C6B6><EFBFBD>λ<EFBFBD><CEBB>:<3A><><EFBFBD><EFBFBD>·<EFBFBD><C2B7><EFBFBD><EFBFBD>m0:1.0-m2:0.0-m3:0.0-m4:0.0-m5:0.0-m6:0.0
2024-12-14 16:14:06 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:14:10 - INFO - <20><><EFBFBD><EFBFBD>Ͷ<EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD>:1
2024-12-14 16:14:11 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:14:11 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>״̬
2024-12-14 16:14:12 - INFO - <20><>һ<EFBFBD><D2BB>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:14:12 - INFO - <20><>Ӧ<EFBFBD><D3A6>1:Ͷ<>Ͽ<EFBFBD>ʼ
2024-12-14 16:14:12 - INFO - <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>: 3<><33>1
2024-12-14 16:14:14 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:14:17 - INFO - <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>: 3<><33>0
2024-12-14 16:14:17 - INFO - Ͷ<>Ͽ<EFBFBD>ʼ
2024-12-14 16:14:21 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:21:15 - INFO - <20>˳<EFBFBD>ϵͳ
2024-12-14 16:21:15 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:21:15 - ERROR - model
2024-12-14 16:21:27 - INFO - <20><><EFBFBD><EFBFBD>ϵͳ
2024-12-14 16:21:59 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:37:33 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:37:33 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>״̬
2024-12-14 16:37:34 - INFO - <20><>һ<EFBFBD><D2BB>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:37:34 - INFO - <20><>Ӧ<EFBFBD><D3A6>1:Ͷ<>Ͽ<EFBFBD>ʼ
2024-12-14 16:37:34 - INFO - <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>: 3<><33>1
2024-12-14 16:37:38 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:37:39 - INFO - <20><><EFBFBD><EFBFBD>Ͷ<EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD>:10
2024-12-14 16:37:39 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:37:39 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>״̬
2024-12-14 16:37:39 - INFO - <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>: 3<><33>0
2024-12-14 16:37:39 - INFO - Ͷ<>Ͽ<EFBFBD>ʼ
2024-12-14 16:37:46 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:39:27 - INFO - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:39:27 - INFO - Ͷ<><CDB6>ֹͣ
2024-12-14 16:39:28 - INFO - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:39:28 - INFO - Ͷ<><CDB6>ֹͣ
2024-12-14 16:39:30 - INFO - Ͷ<><CDB6><EFBFBD><EFBFBD>ͣ
2024-12-14 16:39:30 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:39:30 - INFO - Ͷ<><CDB6><EFBFBD><EFBFBD>ͣ
2024-12-14 16:39:38 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:41:47 - INFO - <20>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>