189 lines
8.7 KiB
Python
189 lines
8.7 KiB
Python
#!/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 = './pt_model/best.pt'
|
||
device = 'cpu'
|
||
img_path = './pt_model/test0824.png'
|
||
point_path = './pt_model/test0824.xyz'
|
||
self.model = yolov8_segment()
|
||
self.model.load_model(model_path, device)
|
||
self.img = cv2.imread(img_path)
|
||
self.point = np.loadtxt(point_path).reshape((1080, 1440, 3))
|
||
|
||
|
||
|
||
def get_position(self, Point_isVision=True):
|
||
""
|
||
'''
|
||
:param api: None
|
||
:return: ret , img, (x,y,z), (nx, ny, nz)
|
||
'''
|
||
#ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
|
||
ret = 1
|
||
img = self.img
|
||
pm = self.point
|
||
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 = []
|
||
seg_point = []
|
||
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)
|
||
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(0)
|
||
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 t in range(pixel_point2.shape[0]):
|
||
select_point.append(pm[pixel_point2[t][0][1] + box_y1, pixel_point2[t][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)
|
||
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
|
||
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])
|
||
|
||
'''
|
||
拟合最小外接矩形，计算矩形中心
|
||
'''
|
||
|
||
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])
|
||
seg_point.append(pm_seg)
|
||
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]), 30, (0, 0, 255), 20) # 标出中心点
|
||
|
||
if Point_isVision == True:
|
||
pcd = o3d.geometry.PointCloud()
|
||
pcd.points = o3d.utility.Vector3dVector(seg_point[min_idx])
|
||
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
|
||
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, 1, 0])
|
||
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, pcd2])
|
||
|
||
return 1, img, xyz[min_idx], nx_ny_nz[min_idx]
|
||
else:
|
||
return 1, img, None, None
|
||
|
||
def release(self):
|
||
self.model.clear()
|
||
|
||
|
||
|