xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

UPDATE Vision box：点云与相机坐标

Browse Source
This commit is contained in:
HJW
2024-10-09 17:34:39 +08:00
parent 1feb28b68d
commit 25bb85dca1
3 changed files with 89 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
Vision/camera_coordinate_dete.py
View File

@ -20,6 +20,7 @@ from Vision.tool.utils import class_names
from Vision.tool.utils import get_disk_space
from Vision.tool.utils import remove_nan_mean_value
from Vision.tool.utils import out_bounds_dete
from Vision.tool.utils import uv_to_XY
import time
import os
@ -244,7 +245,10 @@ class Detection:
box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0])
box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0])
else:
pass
x1, y1, z1= uv_to_XY(box[0][0][0], box[0][0][1])
x2, y2, z2 = uv_to_XY(box[1][0][0], box[1][0][1])
x3, y3, z3 = uv_to_XY(box[2][0][0], box[2][0][1])
x4, y4, z4 = uv_to_XY(box[3][0][0], box[3][0][1])
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 = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center)
@ -252,7 +256,6 @@ class Detection:
if np.isnan(point_x): # 点云值为无效值
continue
else:
if Box_isPoint == True:
box_list.append(
[[box_point_x1, box_point_y1, box_point_z1],
@ -260,7 +263,11 @@ class Detection:
[box_point_x3, box_point_y3, box_point_z3],
[box_point_x4, box_point_y4, box_point_z4]])
else:
box_list.append(box)
box_list.append([[x1, y1, z1],
[x2, y2, z2],
[x3, y3, z3],
[x4, y4, z4],
])
if self.cameraType=='RVC':
xyz.append([point_x*1000, point_y*1000, point_z*1000])
Depth_Z.append(point_z*1000)

22
Vision/camera_coordinate_dete_img.py
View File

@ -21,6 +21,7 @@ from Vision.tool.utils import class_names
from Vision.tool.utils import get_disk_space
from Vision.tool.utils import remove_nan_mean_value
from Vision.tool.utils import out_bounds_dete
from Vision.tool.utils import uv_to_XY
import os
class Detection:
@ -45,7 +46,7 @@ class Detection:
pass
def get_position(self, Point_isVision=False, Box_isPoint=True, save_img_point=0, seg_distance_threshold = 0.01, Height_reduce = 30, width_reduce = 30):
def get_position(self, Point_isVision=False, Box_isPoint=False, save_img_point=0, seg_distance_threshold = 0.01, Height_reduce = 30, width_reduce = 30):
"""
检测料袋相关信息
:param Point_isVision: 点云可视化
@ -209,8 +210,8 @@ class Detection:
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")
# 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)
@ -230,7 +231,10 @@ class Detection:
box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0])
box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0])
else:
pass
x1, y1, z1 = uv_to_XY(self.cameraType, box[0][0][0], box[0][0][1])
x2, y2, z2 = uv_to_XY(self.cameraType, box[1][0][0], box[1][0][1])
x3, y3, z3 = uv_to_XY(self.cameraType, box[2][0][0], box[2][0][1])
x4, y4, z4 = uv_to_XY(self.cameraType, box[3][0][0], box[3][0][1])
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 = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center)
@ -245,8 +249,18 @@ class Detection:
[box_point_x3, box_point_y3, box_point_z3],
[box_point_x4, box_point_y4, box_point_z4]])
else:
box_list.append(box)
box_list.append([[x1, y1, z1],
[x2, y2, z2],
[x3, y3, z3],
[x4, y4, z4],
])
if self.cameraType == 'RVC':
xyz.append([point_x * 1000, point_y * 1000, point_z * 1000])
Depth_Z.append(point_z * 1000)
elif self.cameraType == 'Pe':
xyz.append([point_x, point_y, point_z])
Depth_Z.append(point_z)
#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])

60
Vision/tool/utils.py
View File

@ -14,6 +14,66 @@ import psutil
from psutil._common import bytes2human
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