447 lines
14 KiB
Python
447 lines
14 KiB
Python
#!/usr/bin/env python
|
||
# -*- coding: UTF-8 -*-
|
||
'''
|
||
@Project ：AutoControlSystem-master
|
||
@File ：utils.py
|
||
@IDE ：PyCharm
|
||
@Author ：hjw
|
||
@Date ：2024/8/29 15:07
|
||
'''
|
||
|
||
import numpy as np
|
||
import cv2
|
||
import psutil
|
||
from psutil._common import bytes2human
|
||
|
||
|
||
def shrink_quadrilateral(points, d):
|
||
"""
|
||
给定4个点围成的四边形，沿着对角线内缩小d个像素
|
||
:param points: 四边形的4个顶点，形状为 (4, 2)
|
||
:param d: 内缩的像素距离
|
||
:return: 缩小后的4个顶点
|
||
"""
|
||
# 将点转换为 numpy 数组
|
||
points = np.array(points, dtype=np.float32)
|
||
|
||
# 计算四边形的中心点
|
||
center = np.mean(points, axis=0)
|
||
|
||
# 计算每个点到中心点的向量
|
||
vectors = points - center
|
||
|
||
# 计算每个向量的长度
|
||
lengths = np.linalg.norm(vectors, axis=1)
|
||
|
||
# 计算缩放比例
|
||
scale = (lengths - d) / lengths
|
||
|
||
# 对每个点进行缩放
|
||
new_points = center + vectors * scale[:, np.newaxis]
|
||
new_points = new_points.astype(np.int32)
|
||
|
||
return new_points
|
||
|
||
|
||
def find_closest_point_index(point_cloud, x1, y1):
|
||
x_coords = point_cloud[:, :, 0]
|
||
y_coords = point_cloud[:, :, 1]
|
||
|
||
# 创建一个掩码，标记非 NaN 的点
|
||
valid_mask = ~(np.isnan(x_coords) & ~np.isnan(y_coords))
|
||
|
||
# 初始化最小距离为一个很大的值
|
||
min_distance = np.inf
|
||
min_index = (None, None)
|
||
|
||
# 遍历所有有效点
|
||
for i in range(point_cloud.shape[0]):
|
||
for j in range(point_cloud.shape[1]):
|
||
if valid_mask[i, j]:
|
||
# 计算当前点到 (x1, y1) 的欧几里得距离
|
||
distance = np.sqrt((x_coords[i, j] - x1) ** 2 + (y_coords[i, j] - y1) ** 2)
|
||
# 如果当前距离小于最小距离，则更新最小距离和索引
|
||
if distance < min_distance:
|
||
min_distance = distance
|
||
min_index = (i, j)
|
||
|
||
return min_index
|
||
|
||
def uv_to_XY(cameraType, u, v):
|
||
"""
|
||
像素坐标转相机坐标
|
||
Args:
|
||
cameraType:
|
||
u:
|
||
v:
|
||
|
||
Returns:
|
||
如本：
|
||
|
||
ExtrinsicMatrix:
|
||
[-0.9916700124740601, -0.003792409785091877, 0.12874870002269745, 0.10222162306308746, -0.003501748666167259, 0.9999907612800598, 0.002483875723555684, -0.08221593499183655, -0.12875692546367645, 0.0020123394206166267, -0.9916741251945496, 0.6480034589767456, 0.0, 0.0, 0.0, 1.0]
|
||
|
||
IntrinsicParameters:
|
||
[2402.101806640625, 0.0, 739.7069091796875,
|
||
0.0, 2401.787353515625, 584.73046875,
|
||
0.0, 0.0, 1.0]
|
||
|
||
distortion:
|
||
[-0.04248141124844551, 0.24386045336723328, -0.38333430886268616, -0.0017840253422036767, 0.0007602088153362274]
|
||
|
||
图漾：
|
||
|
||
depth image format list:
|
||
0 -size[640x480] - desc:DEPTH16_640x480
|
||
1 -size[1280x960] - desc:DEPTH16_1280x960
|
||
2 -size[320x240] - desc:DEPTH16_320x240
|
||
delth calib info:
|
||
calib size :[1280x960]
|
||
calib intr :
|
||
(1048.3614501953125, 0.0, 652.146240234375,
|
||
0.0, 1048.3614501953125, 500.26397705078125,
|
||
0.0, 0.0, 1.0)
|
||
calib extr : (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||
calib distortion : (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||
|
||
"""
|
||
x = None
|
||
y = None
|
||
zc = 1 # 设深度z为1
|
||
if cameraType == 'RVC':
|
||
u0 = 739.70
|
||
v0 = 584.73
|
||
fx = 2402.10
|
||
fy = 2401.78
|
||
x = (u-u0)*zc/fx
|
||
y = (v-v0)*zc/fy
|
||
elif cameraType == 'Pe':
|
||
u0 = 652.14
|
||
v0 = 500.26
|
||
fx = 1048.36
|
||
fy = 1048.36
|
||
|
||
x = (u - u0) * zc / fx
|
||
y = (v - v0) * zc / fy
|
||
return x, y, zc
|
||
|
||
|
||
|
||
def out_bounds_dete(pm_y, pm_x, piont_y, piont_x):
|
||
if piont_y>=pm_y:
|
||
piont_y = pm_y-1
|
||
print('四坐标点超出点云大小')
|
||
if piont_y<0:
|
||
piont_y=0
|
||
print('四坐标点超出点云大小')
|
||
if piont_x>=pm_x:
|
||
piont_x = pm_x-1
|
||
print('四坐标点超出点云大小')
|
||
if piont_x<0:
|
||
piont_x=0
|
||
print('四坐标点超出点云大小')
|
||
return piont_y, piont_x
|
||
|
||
def remove_nan_mean_value(pm, y, x, iter_max=50):
|
||
y, x = out_bounds_dete(pm.shape[0], pm.shape[1], y, x)
|
||
point_x, point_y, point_z = pm[y, x]#得到这个位置的点云的坐标
|
||
if np.isnan(point_x):#如果这个位置是nan，找到周围50个像素的范围内的点云，并求平均来代替这个点的坐标
|
||
point_x_list = []
|
||
point_y_list = []
|
||
point_z_list = []
|
||
iter_current = 1
|
||
pm_shape_y = pm.shape[0]
|
||
pm_shape_x = pm.shape[1]
|
||
remove_nan_isok = False
|
||
print('Nan值去除')
|
||
while iter_current < iter_max:#这个邻域内不是nan的点就被放到列表中
|
||
# 计算开始点
|
||
if y - iter_current > 0:
|
||
y_start = y - iter_current
|
||
else:
|
||
y_start = 0
|
||
|
||
if x - iter_current > 0:
|
||
x_start = x - iter_current
|
||
else:
|
||
x_start = 0
|
||
|
||
for idx_y in range(iter_current*2 + 1):
|
||
y_current = y_start + idx_y
|
||
if y_current > pm_shape_y-1:
|
||
continue
|
||
for idx_x in range(iter_current*2 + 1):
|
||
x_current = x_start + idx_x
|
||
if x_current > pm_shape_x-1:
|
||
continue
|
||
elif np.isnan(pm[y_current, x_current][0]) == False:
|
||
point_x_list.append(pm[y_current, x_current][0])
|
||
point_y_list.append(pm[y_current, x_current][1])
|
||
point_z_list.append(pm[y_current, x_current][2])
|
||
|
||
len_point_x = len(point_x_list)
|
||
if len_point_x > 0:#计算x,y,z的均值
|
||
point_x = sum(point_x_list)/len_point_x
|
||
point_y = sum(point_y_list)/len_point_x
|
||
point_z = sum(point_z_list)/len_point_x
|
||
remove_nan_isok = True
|
||
break
|
||
iter_current += 1
|
||
else:
|
||
remove_nan_isok = True
|
||
if remove_nan_isok == True:
|
||
return point_x, point_y, point_z
|
||
else:
|
||
print(f'在{iter_max}*{iter_max}范围中未找到有效值，所有点云值为无效值')
|
||
return np.nan, np.nan, np.nan
|
||
|
||
def remove_nan(pm, y, x):
|
||
point_x, point_y, point_z = pm[y, x]
|
||
if np.isnan(point_x):
|
||
for i in range(10):
|
||
point_x, point_y, point_z = pm[y+i, x]
|
||
if np.isnan(point_x)==False:
|
||
break
|
||
return point_x, point_y, point_z
|
||
|
||
|
||
def get_disk_space(path='C:'):
|
||
|
||
usage = psutil.disk_usage(path)
|
||
space_free = bytes2human(usage.free)
|
||
# space_total = bytes2human(usage.total)
|
||
# space_used = bytes2human(usage.used)
|
||
# space_free = bytes2human(usage.free)
|
||
# space_used_percent = bytes2human(usage.percent)
|
||
space_free = float(space_free[:-1])
|
||
return space_free
|
||
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]
|
||
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]
|
||
if len(sorted_id)>0:
|
||
return sorted_id[0]
|
||
else:
|
||
return
|
||
|
||
class_names = ['box', 'other']
|
||
|
||
# Create a list of colors for each class where each color is a tuple of 3 integer values
|
||
rng = np.random.default_rng(3)
|
||
colors = rng.uniform(0, 255, size=(len(class_names), 3))
|
||
|
||
|
||
def nms(boxes, scores, iou_threshold):
|
||
# Sort by score
|
||
sorted_indices = np.argsort(scores)[::-1]
|
||
|
||
keep_boxes = []
|
||
while sorted_indices.size > 0:
|
||
# Pick the last box
|
||
box_id = sorted_indices[0]
|
||
keep_boxes.append(box_id)
|
||
|
||
# Compute IoU of the picked box with the rest
|
||
ious = compute_iou(boxes[box_id, :], boxes[sorted_indices[1:], :])
|
||
|
||
# Remove boxes with IoU over the threshold
|
||
keep_indices = np.where(ious < iou_threshold)[0]
|
||
|
||
# print(keep_indices.shape, sorted_indices.shape)
|
||
sorted_indices = sorted_indices[keep_indices + 1]
|
||
|
||
return keep_boxes
|
||
|
||
|
||
def compute_iou(box, boxes):
|
||
# Compute xmin, ymin, xmax, ymax for both boxes
|
||
xmin = np.maximum(box[0], boxes[:, 0])
|
||
ymin = np.maximum(box[1], boxes[:, 1])
|
||
xmax = np.minimum(box[2], boxes[:, 2])
|
||
ymax = np.minimum(box[3], boxes[:, 3])
|
||
|
||
# Compute intersection area
|
||
intersection_area = np.maximum(0, xmax - xmin) * np.maximum(0, ymax - ymin)
|
||
|
||
# Compute union area
|
||
box_area = (box[2] - box[0]) * (box[3] - box[1])
|
||
boxes_area = (boxes[:, 2] - boxes[:, 0]) * (boxes[:, 3] - boxes[:, 1])
|
||
union_area = box_area + boxes_area - intersection_area
|
||
|
||
# Compute IoU
|
||
iou = intersection_area / union_area
|
||
|
||
return iou
|
||
|
||
|
||
def xywh2xyxy(x):
|
||
# Convert bounding box (x, y, w, h) to bounding box (x1, y1, x2, y2)
|
||
y = np.copy(x)
|
||
y[..., 0] = x[..., 0] - x[..., 2] / 2
|
||
y[..., 1] = x[..., 1] - x[..., 3] / 2
|
||
y[..., 2] = x[..., 0] + x[..., 2] / 2
|
||
y[..., 3] = x[..., 1] + x[..., 3] / 2
|
||
return y
|
||
|
||
|
||
def sigmoid(x):
|
||
return 1 / (1 + np.exp(-x))
|
||
|
||
|
||
def draw_detections(image, boxes, scores, class_ids, mask_alpha=0.3, mask_maps=None):
|
||
img_height, img_width = image.shape[:2]
|
||
size = min([img_height, img_width]) * 0.0006
|
||
text_thickness = int(min([img_height, img_width]) * 0.001)
|
||
|
||
mask_img = draw_masks(image, boxes, class_ids, mask_alpha, mask_maps)
|
||
|
||
# Draw bounding boxes and labels of detections
|
||
for box, score, class_id in zip(boxes, scores, class_ids):
|
||
color = colors[class_id]
|
||
|
||
x1, y1, x2, y2 = box.astype(int)
|
||
|
||
# Draw rectangle
|
||
cv2.rectangle(mask_img, (x1, y1), (x2, y2), color, 2)
|
||
|
||
label = class_names[class_id]
|
||
caption = f'{label} {int(score * 100)}%'
|
||
(tw, th), _ = cv2.getTextSize(text=caption, fontFace=cv2.FONT_HERSHEY_SIMPLEX,
|
||
fontScale=size, thickness=text_thickness)
|
||
th = int(th * 1.2)
|
||
|
||
cv2.rectangle(mask_img, (x1, y1),
|
||
(x1 + tw, y1 - th), color, -1)
|
||
|
||
cv2.putText(mask_img, caption, (x1, y1),
|
||
cv2.FONT_HERSHEY_SIMPLEX, size, (255, 255, 255), text_thickness, cv2.LINE_AA)
|
||
|
||
return mask_img
|
||
|
||
|
||
def draw_masks(image, boxes, class_ids, mask_alpha=0.3, mask_maps=None):
|
||
mask_img = image.copy()
|
||
|
||
# Draw bounding boxes and labels of detections
|
||
for i, (box, class_id) in enumerate(zip(boxes, class_ids)):
|
||
color = colors[class_id]
|
||
|
||
x1, y1, x2, y2 = box.astype(int)
|
||
|
||
# Draw fill mask image
|
||
if mask_maps is None:
|
||
cv2.rectangle(mask_img, (x1, y1), (x2, y2), color, -1)
|
||
else:
|
||
crop_mask = mask_maps[i][y1:y2, x1:x2, np.newaxis]
|
||
crop_mask_img = mask_img[y1:y2, x1:x2]
|
||
crop_mask_img = crop_mask_img * (1 - crop_mask) + crop_mask * color
|
||
mask_img[y1:y2, x1:x2] = crop_mask_img
|
||
|
||
return cv2.addWeighted(mask_img, mask_alpha, image, 1 - mask_alpha, 0)
|
||
|
||
|
||
def draw_comparison(img1, img2, name1, name2, fontsize=2.6, text_thickness=3):
|
||
(tw, th), _ = cv2.getTextSize(text=name1, fontFace=cv2.FONT_HERSHEY_DUPLEX,
|
||
fontScale=fontsize, thickness=text_thickness)
|
||
x1 = img1.shape[1] // 3
|
||
y1 = th
|
||
offset = th // 5
|
||
cv2.rectangle(img1, (x1 - offset * 2, y1 + offset),
|
||
(x1 + tw + offset * 2, y1 - th - offset), (0, 115, 255), -1)
|
||
cv2.putText(img1, name1,
|
||
(x1, y1),
|
||
cv2.FONT_HERSHEY_DUPLEX, fontsize,
|
||
(255, 255, 255), text_thickness)
|
||
|
||
(tw, th), _ = cv2.getTextSize(text=name2, fontFace=cv2.FONT_HERSHEY_DUPLEX,
|
||
fontScale=fontsize, thickness=text_thickness)
|
||
x1 = img2.shape[1] // 3
|
||
y1 = th
|
||
offset = th // 5
|
||
cv2.rectangle(img2, (x1 - offset * 2, y1 + offset),
|
||
(x1 + tw + offset * 2, y1 - th - offset), (94, 23, 235), -1)
|
||
|
||
cv2.putText(img2, name2,
|
||
(x1, y1),
|
||
cv2.FONT_HERSHEY_DUPLEX, fontsize,
|
||
(255, 255, 255), text_thickness)
|
||
|
||
combined_img = cv2.hconcat([img1, img2])
|
||
if combined_img.shape[1] > 3840:
|
||
combined_img = cv2.resize(combined_img, (3840, 2160))
|
||
|
||
return combined_img
|
||
|
||
|
||
|
||
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
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|