更新 Vision/camera_coordinate_dete.py
This commit is contained in:
hjw
@ -1,164 +1,164 @@
|
|||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
# -*- coding: UTF-8 -*-
|
# -*- coding: UTF-8 -*-
|
||||||
'''
|
'''
|
||||||
@Project ：AutoControlSystem-master
|
@Project ：AutoControlSystem-master
|
||||||
@File ：camera_coordinate_dete.py
|
@File ：camera_coordinate_dete.py
|
||||||
@IDE ：PyCharm
|
@IDE ：PyCharm
|
||||||
@Author ：hjw
|
@Author ：hjw
|
||||||
@Date ：2024/8/27 14:24
|
@Date ：2024/8/27 14:24
|
||||||
'''
|
'''
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import cv2
|
import cv2
|
||||||
import open3d as o3d
|
import open3d as o3d
|
||||||
from .tool.CameraRVC import camera
|
from Vision.tool.CameraRVC import camera
|
||||||
from .yolo.yolov8_pt_seg import yolov8_segment
|
from Vision.yolo.yolov8_pt_seg import yolov8_segment
|
||||||
|
|
||||||
|
|
||||||
class Detection:
|
class Detection:
|
||||||
|
|
||||||
def __init__(self, model_path, device):
|
def __init__(self, model_path, device):
|
||||||
self.camera_rvc = camera()
|
self.camera_rvc = camera()
|
||||||
self.model = yolov8_segment()
|
self.model = yolov8_segment()
|
||||||
self.model.load_model(model_path, device)
|
self.model.load_model(model_path, device)
|
||||||
if self.camera_rvc.caminit_isok == True:
|
if self.camera_rvc.caminit_isok == True:
|
||||||
print("RVC X Camera is opened!")
|
print("RVC X Camera is opened!")
|
||||||
else:
|
else:
|
||||||
print("RVC X Camera is not opened!")
|
print("RVC X Camera is not opened!")
|
||||||
|
|
||||||
def get_position(self):
|
def get_position(self):
|
||||||
""
|
""
|
||||||
'''
|
'''
|
||||||
:param api: None
|
:param api: None
|
||||||
:return: ret , img, (x,y,z), (nx, ny, nz)
|
:return: ret , img, (x,y,z), (nx, ny, nz)
|
||||||
'''
|
'''
|
||||||
ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
|
ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
|
||||||
if self.camera_rvc.caminit_isok == True:
|
if self.camera_rvc.caminit_isok == True:
|
||||||
if ret == 1:
|
if ret == 1:
|
||||||
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
|
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
|
||||||
if flag == 1:
|
if flag == 1:
|
||||||
xyz = []
|
xyz = []
|
||||||
nx_ny_nz = []
|
nx_ny_nz = []
|
||||||
for i, item in enumerate(det_cpu):
|
RegionalArea = []
|
||||||
|
Depth_Z = []
|
||||||
# 画box
|
uv = []
|
||||||
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
|
for i, item in enumerate(det_cpu):
|
||||||
label = category_names[int(item[5])]
|
|
||||||
rand_color = (0, 255, 255)
|
# 画box
|
||||||
score = item[4]
|
box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
|
||||||
org = (int((box_x1 + box_x2) / 2), int((box_y1 + box_y2) / 2))
|
label = category_names[int(item[5])]
|
||||||
x_center = int((box_x1 + box_x2) / 2)
|
rand_color = (0, 255, 255)
|
||||||
y_center = int((box_y1 + box_y2) / 2)
|
score = item[4]
|
||||||
text = '{}|{:.2f}'.format(label, score)
|
org = (int((box_x1 + box_x2) / 2), int((box_y1 + box_y2) / 2))
|
||||||
cv2.putText(img, text, org=org, fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8,
|
x_center = int((box_x1 + box_x2) / 2)
|
||||||
color=rand_color,
|
y_center = int((box_y1 + box_y2) / 2)
|
||||||
thickness=2)
|
text = '{}|{:.2f}'.format(label, score)
|
||||||
# 画mask
|
cv2.putText(img, text, org=org, fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8,
|
||||||
# mask = masks[i].cpu().numpy().astype(int)
|
color=rand_color,
|
||||||
mask = masks[i].cpu().data.numpy().astype(int)
|
thickness=2)
|
||||||
# mask = masks[i].numpy().astype(int)
|
# 画mask
|
||||||
bbox_image = img[box_y1:box_y2, box_x1:box_x2]
|
# mask = masks[i].cpu().numpy().astype(int)
|
||||||
h, w = box_y2 - box_y1, box_x2 - box_x1
|
mask = masks[i].cpu().data.numpy().astype(int)
|
||||||
mask_colored = np.zeros((h, w, 3), dtype=np.uint8)
|
# mask = masks[i].numpy().astype(int)
|
||||||
mask_colored[np.where(mask)] = rand_color
|
bbox_image = img[box_y1:box_y2, box_x1:box_x2]
|
||||||
##################################
|
h, w = box_y2 - box_y1, box_x2 - box_x1
|
||||||
imgray = cv2.cvtColor(mask_colored, cv2.COLOR_BGR2GRAY)
|
mask_colored = np.zeros((h, w, 3), dtype=np.uint8)
|
||||||
# cv2.imshow('mask',imgray)
|
mask_colored[np.where(mask)] = rand_color
|
||||||
# cv2.waitKey(1)
|
##################################
|
||||||
# 2、二进制图像
|
imgray = cv2.cvtColor(mask_colored, cv2.COLOR_BGR2GRAY)
|
||||||
ret, binary = cv2.threshold(imgray, 10, 255, 0)
|
# cv2.imshow('mask',imgray)
|
||||||
# 阈值 二进制图像
|
# cv2.waitKey(1)
|
||||||
# cv2.imshow('bin',binary)
|
# 2、二进制图像
|
||||||
# cv2.waitKey(1)
|
ret, binary = cv2.threshold(imgray, 10, 255, 0)
|
||||||
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
|
# 阈值 二进制图像
|
||||||
# all_piont_list = contours_in(contours)
|
# cv2.imshow('bin',binary)
|
||||||
# print(len(all_piont_list))
|
# cv2.waitKey(1)
|
||||||
max_contour = None
|
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
|
||||||
max_perimeter = 0
|
# all_piont_list = contours_in(contours)
|
||||||
for contour in contours: # 排除小分割区域或干扰区域
|
# print(len(all_piont_list))
|
||||||
perimeter = cv2.arcLength(contour, True)
|
max_contour = None
|
||||||
if perimeter > max_perimeter:
|
max_perimeter = 0
|
||||||
max_perimeter = perimeter
|
for contour in contours: # 排除小分割区域或干扰区域
|
||||||
max_contour = contour
|
perimeter = cv2.arcLength(contour, True)
|
||||||
'''
|
if perimeter > max_perimeter:
|
||||||
提取区域范围内的（x, y）
|
max_perimeter = perimeter
|
||||||
'''
|
max_contour = contour
|
||||||
mask_inside = np.zeros(binary.shape, np.uint8)
|
'''
|
||||||
cv2.drawContours(mask_inside, [max_contour], 0, 255, -1)
|
提取区域范围内的（x, y）
|
||||||
pixel_point2 = cv2.findNonZero(mask_inside)
|
'''
|
||||||
# result = np.zeros_like(color_image)
|
mask_inside = np.zeros(binary.shape, np.uint8)
|
||||||
select_point = []
|
cv2.drawContours(mask_inside, [max_contour], 0, 255, -1)
|
||||||
for i in range(pixel_point2.shape[0]):
|
pixel_point2 = cv2.findNonZero(mask_inside)
|
||||||
# k = pixel_point2[i]
|
# result = np.zeros_like(color_image)
|
||||||
select_point.append(pm[pixel_point2[i][0][1], pixel_point2[i][0][0]])
|
select_point = []
|
||||||
select_point = np.array(select_point)
|
for i in range(pixel_point2.shape[0]):
|
||||||
pm_seg = select_point.reshape(-1, 3)
|
select_point.append(pm[pixel_point2[i][0][1]+box_y1, pixel_point2[i][0][0]]+box_x1)
|
||||||
pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan
|
select_point = np.array(select_point)
|
||||||
# cv2.imshow('result', piont_result)
|
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")
|
pcd = o3d.geometry.PointCloud()
|
||||||
plane_model, inliers = pcd.segment_plane(distance_threshold=0.1,
|
pcd.points = o3d.utility.Vector3dVector(pm_seg)
|
||||||
ransac_n=3,
|
# pcd = o3d.io.read_point_cloud("./Data/seg_point.xyz")
|
||||||
num_iterations=100)
|
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
|
||||||
[a, b, c, d] = plane_model
|
ransac_n=5,
|
||||||
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
|
num_iterations=1000)
|
||||||
|
[a, b, c, d] = plane_model
|
||||||
# inlier_cloud = pcd.select_by_index(inliers) # 点云可视化
|
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
|
||||||
# inlier_cloud.paint_uniform_color([1.0, 0, 0])
|
|
||||||
# outlier_cloud = pcd.select_by_index(inliers, invert=True)
|
# inlier_cloud = pcd.select_by_index(inliers) # 点云可视化
|
||||||
# outlier_cloud.paint_uniform_color([0, 1, 0])
|
# inlier_cloud.paint_uniform_color([1.0, 0, 0])
|
||||||
# o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])
|
# 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)
|
rect = cv2.minAreaRect(max_contour)
|
||||||
# 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
|
# cv2.boxPoints可以将轮廓点转换为四个角点坐标
|
||||||
startidx = box.sum(axis=1).argmin()
|
box = cv2.boxPoints(rect)
|
||||||
box = np.roll(box, 4 - startidx, 0)
|
# 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
|
||||||
# 在原图上画出预测的外接矩形
|
startidx = box.sum(axis=1).argmin()
|
||||||
box = box.reshape((-1, 1, 2)).astype(np.int32)
|
box = np.roll(box, 4 - startidx, 0)
|
||||||
box = box + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]
|
# 在原图上画出预测的外接矩形
|
||||||
|
box = box.reshape((-1, 1, 2)).astype(np.int32)
|
||||||
x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4)
|
box = box + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]
|
||||||
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]
|
x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4)
|
||||||
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
|
y_rotation_center = int((box[0][0][1] + box[1][0][1] + box[2][0][1] + box[3][0][1]) / 4)
|
||||||
x1, y1, z1, = pm[x_rotation_center+1, y_rotation_center+1]
|
point_x, point_y, point_z = pm[y_rotation_center, x_rotation_center]
|
||||||
print('x1 y1 z1 :', x1, y1, z1)
|
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
|
||||||
#print('像素坐标（x, y）：', x_rotation_center,y_rotation_center)
|
if np.isnan(point_x): # 点云值为无效值
|
||||||
print('x y z :', point_x, point_y, point_z)
|
continue
|
||||||
# point_x=point_x*1000
|
else:
|
||||||
# point_y=point_y*1000
|
xyz.append([point_x*1000, point_y*1000, point_z*1000])
|
||||||
# point_z=point_z*1000
|
Depth_Z.append(point_z*1000)
|
||||||
print('nx ny nz :', a, b, c)
|
|
||||||
# getPosition(x, y, z, a, b, c)
|
nx_ny_nz.append([a, b, c])
|
||||||
if(point_x == np.nan):
|
RegionalArea.append(cv2.contourArea(max_contour))
|
||||||
continue
|
uv.append([x_rotation_center,y_rotation_center])
|
||||||
else:
|
cv2.polylines(img, [box], True, (0, 255, 0), 2)
|
||||||
xyz.append([x1*1000, y1*1000, z1*1000])
|
|
||||||
nx_ny_nz.append([a, b, c])
|
min_value = min(Depth_Z) # 求深度最大值
|
||||||
cv2.polylines(img, [box], True, (0, 255, 0), 2)
|
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, nx_ny_nz
|
return 1, img, xyz[min_idx], nx_ny_nz[min_idx]
|
||||||
else:
|
else:
|
||||||
return 1, img, None, None
|
return 1, img, None, None
|
||||||
|
|
||||||
else:
|
else:
|
||||||
print("RVC X Camera capture failed!")
|
print("RVC X Camera capture failed!")
|
||||||
return 0, None, None, None
|
return 0, None, None, None
|
||||||
|
|
||||||
else:
|
else:
|
||||||
print("RVC X Camera is not opened!")
|
print("RVC X Camera is not opened!")
|
||||||
return 0, None, None, None
|
return 0, None, None, None
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user