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
2024-09-06 10:57:46 +08:00
parent 1918520cc9 3d4389399a
commit 4ac05c232b
3 changed files with 195 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files

328
Vision/camera_coordinate_dete.py
View File

@ -1,164 +1,164 @@
#!/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
from .tool.CameraRVC import camera
from .yolo.yolov8_pt_seg import yolov8_segment
class Detection:
def __init__(self, model_path, device):
self.camera_rvc = camera()
self.model = yolov8_segment()
self.model.load_model(model_path, device)
if self.camera_rvc.caminit_isok == True:
print("RVC X Camera is opened!")
else:
print("RVC X Camera is not opened!")
def get_position(self):
""
'''
:param api: None
:return: ret , img, (x,y,z), (nx, ny, nz)
'''
ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
if self.camera_rvc.caminit_isok == True:
if ret == 1:
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
if flag == 1:
xyz = []
nx_ny_nz = []
for i, item in enumerate(det_cpu):
# 画box
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
label = category_names[int(item[5])]
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)
mask = masks[i].cpu().data.numpy().astype(int)
# mask = masks[i].numpy().astype(int)
bbox_image = img[box_y1:box_y2, box_x1:box_x2]
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_piont_list = contours_in(contours)
# print(len(all_piont_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
'''
提取区域范围内的（x, y）
'''
mask_inside = np.zeros(binary.shape, np.uint8)
cv2.drawContours(mask_inside, [max_contour], 0, 255, -1)
pixel_point2 = cv2.findNonZero(mask_inside)
# result = np.zeros_like(color_image)
select_point = []
for i in range(pixel_point2.shape[0]):
# k = pixel_point2[i]
select_point.append(pm[pixel_point2[i][0][1], pixel_point2[i][0][0]])
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
# cv2.imshow('result', piont_result)
'''
拟合平面，计算法向量
'''
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pm_seg)
# pcd = o3d.io.read_point_cloud("./Data/seg_point.xyz")
plane_model, inliers = pcd.segment_plane(distance_threshold=0.1,
ransac_n=3,
num_iterations=100)
[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])
'''
拟合最小外接矩形，计算矩形中心
'''
rect = cv2.minAreaRect(max_contour)
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
box = cv2.boxPoints(rect)
# 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
startidx = box.sum(axis=1).argmin()
box = np.roll(box, 4 - startidx, 0)
# 在原图上画出预测的外接矩形
box = box.reshape((-1, 1, 2)).astype(np.int32)
box = box + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]
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 =pm[x_rotation_center, y_rotation_center]
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
x1, y1, z1, = pm[x_rotation_center+1, y_rotation_center+1]
print('x1 y1 z1 :', x1, y1, z1)
#print('像素坐标（x, y）：', x_rotation_center,y_rotation_center)
print('x y z :', point_x, point_y, point_z)
# point_x=point_x*1000
# point_y=point_y*1000
# point_z=point_z*1000
print('nx ny nz :', a, b, c)
# getPosition(x, y, z, a, b, c)
if(point_x == np.nan):
continue
else:
xyz.append([x1*1000, y1*1000, z1*1000])
nx_ny_nz.append([a, b, c])
cv2.polylines(img, [box], True, (0, 255, 0), 2)
return 1, img, xyz, nx_ny_nz
else:
return 1, img, None, None
else:
print("RVC X Camera capture failed!")
return 0, None, None, None
else:
print("RVC X Camera is not opened!")
return 0, None, None, None
#!/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
from Vision.tool.CameraRVC import camera
from Vision.yolo.yolov8_pt_seg import yolov8_segment
class Detection:
def __init__(self, model_path, device):
self.camera_rvc = camera()
self.model = yolov8_segment()
self.model.load_model(model_path, device)
if self.camera_rvc.caminit_isok == True:
print("RVC X Camera is opened!")
else:
print("RVC X Camera is not opened!")
def get_position(self):
""
'''
:param api: None
:return: ret , img, (x,y,z), (nx, ny, nz)
'''
ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
if self.camera_rvc.caminit_isok == True:
if ret == 1:
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
if flag == 1:
xyz = []
nx_ny_nz = []
RegionalArea = []
Depth_Z = []
uv = []
for i, item in enumerate(det_cpu):
# 画box
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
label = category_names[int(item[5])]
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)
mask = masks[i].cpu().data.numpy().astype(int)
# mask = masks[i].numpy().astype(int)
bbox_image = img[box_y1:box_y2, box_x1:box_x2]
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_piont_list = contours_in(contours)
# print(len(all_piont_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
'''
提取区域范围内的（x, y）
'''
mask_inside = np.zeros(binary.shape, np.uint8)
cv2.drawContours(mask_inside, [max_contour], 0, 255, -1)
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
# cv2.imshow('result', piont_result)
'''
拟合平面，计算法向量
'''
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pm_seg)
# pcd = o3d.io.read_point_cloud("./Data/seg_point.xyz")
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
ransac_n=5,
num_iterations=1000)
[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])
'''
拟合最小外接矩形，计算矩形中心
'''
rect = cv2.minAreaRect(max_contour)
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
box = cv2.boxPoints(rect)
# 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
startidx = box.sum(axis=1).argmin()
box = np.roll(box, 4 - startidx, 0)
# 在原图上画出预测的外接矩形
box = box.reshape((-1, 1, 2)).astype(np.int32)
box = box + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]
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 = 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:
xyz.append([point_x*1000, point_y*1000, point_z*1000])
Depth_Z.append(point_z*1000)
nx_ny_nz.append([a, b, c])
RegionalArea.append(cv2.contourArea(max_contour))
uv.append([x_rotation_center,y_rotation_center])
cv2.polylines(img, [box], True, (0, 255, 0), 2)
min_value = min(Depth_Z) # 求深度最大值
min_idx = Depth_Z.index(min_value) # 求最大值对应索引
cv2.circle(img, (uv[min_idx][0], uv[min_idx][1]), 20, (255, 0, 0), 5) # 标出中心点
return 1, img, xyz[min_idx], nx_ny_nz[min_idx]
else:
return 1, img, None, None
else:
print("RVC X Camera capture failed!")
return 0, None, None, None
else:
print("RVC X Camera is not opened!")
return 0, None, None, None

23
Vision/camera_coordinate_dete_test.py Normal file
View File

@ -0,0 +1,23 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
# @Time : 2024/9/5 14:57
# @Author : hjw
# @File : camera_coordinate_dete_test.py.py
'''
from camera_coordinate_dete import Detection
import cv2
model_path = './pt_model/last-0903.pt'
device = 'cpu'
detection = Detection(model_path, device)
while True:
ret, img, xyz, nx_ny_nz = detection.get_position()
if ret==1:
print('xyz点云坐标：', xyz)
print('nx_ny_nz法向量：', nx_ny_nz)
img = cv2.resize(img,(720, 540))
cv2.imshow('img', img)
cv2.waitKey(1)

10
requirements
View File

@ -1,7 +1,7 @@
colorthief=0.2.1
darkdetect=0.8.0
numpy=2.0.1
pillow=10.4.
pillow=10.4
pip=24.0
PyQt5=5.15.11
PyQt5-Frameless-Window=0.3.9
@ -15,4 +15,10 @@ scipy=1.13.1
setuptools=69.5.1
shiboken6=6.7.2
wheel=0.43.0
python=3.9.19
python=3.9.19
open3d=0.18.0
opencv-python=4.7.0
PyRVC=1.10.0
torch=1.13.1
torchvision=0.14.1
ultralytics=8.2.86