HiveCoreRD/hivecore_robot_vision
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix

Browse Source
This commit is contained in:
liangyuxuan
2025-11-28 16:22:59 +08:00
parent fac1682871
commit 016e1d2a67
4 changed files with 36 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
vision_detect/setup.py
View File

@@ -17,6 +17,8 @@ setup(
('share/' + package_name + '/configs/launch_configs', glob('configs/launch_configs/*.json')),
('share/' + package_name + '/checkpoints', glob('checkpoints/*.pt')),
('share/' + package_name + '/checkpoints', glob('checkpoints/*.onnx')),
('share/' + package_name + '/checkpoints', glob('checkpoints/*.engine')),
('share/' + package_name + '/pointclouds', glob('pointclouds/*.pcd')),
],
install_requires=['setuptools'],

25
vision_detect/vision_detect/VisionDetect/node/detect_node.py
View File

@@ -410,6 +410,8 @@ class DetectNode(Node):
time3 = time.time()
self.get_logger().info(f"Detect object num: {len(masks)}")
for i, (mask, box) in enumerate(zip(masks, boxes)):
imgs_crop, (x_min, y_min) = crop_imgs_box_xywh([depth_img, mask], box, True)
depth_crop, mask_crop = imgs_crop
@@ -426,17 +428,20 @@ class DetectNode(Node):
self.k[5] - y_min
)
x, y, z, rw, rx, ry, rz = self.calculate_function(
rmat = self.calculate_function(
mask_crop,
depth_crop,
intrinsics,
self.source,
self.hand_eye_mat,
self.configs
)
grab_width = calculate_grav_width(mask, self.k, z)
z = z + grab_width * 0.45
rmat[2, 3] = rmat[2, 3] + grab_width * 0.22
rmat = self.hand_eye_mat @ rmat
x, y, z, rw, rx, ry, rz = rmat2quat(rmat)
if (x, y, z) != (0.0, 0.0, 0.0):
pose = Pose()
@@ -507,15 +512,18 @@ class DetectNode(Node):
self.k[5] - y_min
)
x, y, z, rw, rx, ry, rz = self.calculate_function(
rmat = self.calculate_function(
mask_crop,
depth_crop,
intrinsics,
self.source,
self.hand_eye_mat,
self.configs
)
rmat = self.hand_eye_mat @ rmat
x, y, z, rw, rx, ry, rz = rmat2quat(rmat)
if (x, y, z) != (0.0, 0.0, 0.0):
pose = Pose()
pose.position = Point(x=x, y=y, z=z)
@@ -525,6 +533,7 @@ class DetectNode(Node):
"class_id": int(98),
"class_name": "red",
"pose": pose,
"grab_width": 0.0
}
)
@@ -569,15 +578,16 @@ class DetectNode(Node):
self.k[5] - y_min
)
x, y, z, rw, rx, ry, rz = self.calculate_function(
rmat = self.calculate_function(
mask_crop,
depth_crop,
intrinsics,
self.source,
self.hand_eye_mat,
self.configs
)
x, y, z, rw, rx, ry, rz = rmat2quat(rmat)
self.get_logger().info(f"{x}, {y}, {z}, {rw}, {rx}, {ry}, {rz}")
if (x, y, z) != (0.0, 0.0, 0.0):
@@ -589,6 +599,7 @@ class DetectNode(Node):
"class_id": int(99),
"class_name": 'crossboard',
"pose": pose,
"grab_width": 0.0
}
)

1
vision_detect/vision_detect/VisionDetect/utils/__init__.py
View File

@@ -1,4 +1,3 @@
from .object_icp import *
from .image_tools import *
from .draw_tools import *
from .calculate_tools import *

36
vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py
View File

@@ -111,7 +111,6 @@ def calculate_pose_icp(
depth_img: np.ndarray,
intrinsics,
source,
hand_eye_mat=np.eye(4),
configs = None,
):
if configs is None:
@@ -152,7 +151,7 @@ def calculate_pose_icp(
if len(clean_pcd.points) == 0:
logging.warning("clean_pcd is empty")
return 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
return np.eye(4)
rmat = object_icp(
source,
@@ -162,30 +161,27 @@ def calculate_pose_icp(
icp_max_iter=configs["icp_max_iter"]
)
rmat = hand_eye_mat @ rmat
x, y, z = rmat[0:3, 3:4].flatten()
rw, rx, ry, rz = tfs.quaternions.mat2quat(rmat[0:3, 0:3])
return x, y, z, rw, rx, ry, rz
return rmat
def calculate_grav_width(mask, k, depth):
"""计算重心宽度"""
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if contours:
box = cv2.boxPoints(cv2.minAreaRect(contours[0]))
if np.linalg.norm(box[1] - box[0]) < np.linalg.norm(box[1] - box[2]):
point_diff = box[1] - box[0]
else:
point_diff = box[1] - box[2]
grab_width = depth * np.sqrt(
point_diff[0]**2 / k[0]**2 + point_diff[1]**2 / k[4]**2
)
return grab_width
return 0.0
if not contours:
return 0.0
c = max(contours, key=cv2.contourArea)
box = cv2.boxPoints(cv2.minAreaRect(c))
if np.linalg.norm(box[1] - box[0]) < np.linalg.norm(box[1] - box[2]):
point_diff = box[1] - box[0]
else:
point_diff = box[1] - box[2]
grab_width = depth * np.sqrt(
point_diff[0]**2 / k[0]**2 + point_diff[1]**2 / k[4]**2
)
return grab_width
def rmat2quat(rmat):