165 lines
8.2 KiB
Python
165 lines
8.2 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 .tool.CameraRVC import camera
|
||
from .yolo.yolov8_pt_seg import yolov8_segment
|
||
|
||
|
||
class Detection_Position:
|
||
|
||
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
|
||
|
||
|
||
|