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

更改去噪参数

Browse Source
This commit is contained in:
liangyuxuan
2025-12-30 13:22:00 +08:00
parent eeedd4bfc6
commit 97c142f6e3
5 changed files with 47 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
vision_detect/configs/launch_configs/medical_sense.json
View File

@@ -25,6 +25,6 @@
"PCA_configs": {
"depth_scale": 1000.0,
"depth_trunc": 3.0,
"voxel_size": 0.001
"voxel_size": 0.005
}
}

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

@@ -176,7 +176,6 @@ class DetectNode(InitBase):
masks = masks[sorted_index]
boxes = boxes[sorted_index]
class_ids = class_ids[sorted_index]
labels = labels[sorted_index]
time3 = time.time()

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

@@ -61,12 +61,14 @@ def calculate_pose_pca(
logging.warning("point_cloud is empty")
return np.eye(4), [0.0, 0.0, 0.0]
center = point_cloud.get_center()
x, y, z = center
if calculate_grab_width:
if np.asarray(point_cloud.points).shape[0] < 4:
logging.warning("点数不足，不能算 OBB")
return np.eye(4), [0.0, 0.0, 0.0]
obb = point_cloud.get_oriented_bounding_box()
x, y, z = obb.center
extent = obb.extent
order = np.argsort(-extent)
@@ -83,9 +85,6 @@ def calculate_pose_pca(
grab_width = grab_width * 1.05
else:
center = point_cloud.get_center()
x, y, z = center
w, v = pca(np.asarray(point_cloud.points))
if w is None or v is None:
@@ -157,25 +156,26 @@ def calculate_pose_icp(
def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: float = 0.005):
"""点云去噪"""
point_cloud = point_cloud.remove_non_finite_points()
while True:
down_pcd = point_cloud.voxel_down_sample(voxel_size=voxel_size)
point_num = len(down_pcd.points)
if point_num <= 1000:
break
voxel_size *= 4
# while True:
# down_pcd = point_cloud.voxel_down_sample(voxel_size=voxel_size)
# point_num = len(down_pcd.points)
# if point_num <= 1000:
# break
# voxel_size *= 4
down_pcd = point_cloud.voxel_down_sample(voxel_size=voxel_size)
# logging.fatal("point_cloud_denoising: point_num={}".format(len(point_cloud.points)))
# logging.fatal("point_cloud_denoising: point_num={}".format(point_num))
# 半径滤波
clean_pcd, _ = down_pcd.remove_radius_outlier(
nb_points=10,
nb_points=int(round(10 * voxel_size / 0.005)),
radius=voxel_size * 10
)
# 统计滤波
clean_pcd, _ = clean_pcd.remove_statistical_outlier(
nb_neighbors=10,
nb_neighbors=int(round(10 * voxel_size / 0.005)),
std_ratio=2.0
)
@@ -189,7 +189,10 @@ def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: floa
# if largest_cluster_ratio < 0.5:
# return None
labels = np.array(clean_pcd.cluster_dbscan(eps=voxel_size * 10, min_points=10))
labels = np.array(clean_pcd.cluster_dbscan(
eps=voxel_size * 10,
min_points=int(round(10 * voxel_size / 0.005)))
)
if len(labels[labels >= 0]) == 0:
return clean_pcd
@@ -216,7 +219,7 @@ def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: floa
# 使用最近簇判断噪音强度
largest_cluster_ratio = len(clean_pcd.points) / point_num
if largest_cluster_ratio < 0.20:
if largest_cluster_ratio < 0.08:
return None
return clean_pcd

2
vision_detect/vision_detect/VisionDetect/utils/draw_tools.py
View File

@@ -29,7 +29,7 @@ def draw_box(
sorted_index = np.lexsort((-y_centers, x_centers))
boxes = boxes[sorted_index]
class_ids = class_ids[sorted_index]
labels = labels[sorted_index]
confidences = confidences[sorted_index]
for i, box in enumerate(boxes):
x_center, y_center, width, height = box[:4]

34
vision_tools/vision_tools/collect_data_node.py
View File

@@ -1,6 +1,7 @@
import os
import json
import threading
import time
import cv2
@@ -70,8 +71,15 @@ class CollectDataNode(Node):
os.makedirs(self.save_path, exist_ok=True)
# left
if self.topic_exists(configs["left"]["color"]) and self.topic_exists(
configs["left"]["depth"]):
left_sign = False
for _ in range(4):
if self.topic_exists(configs["left"]["color"]) and self.topic_exists(
configs["left"]["depth"]):
left_sign = True
break
else:
time.sleep(0.5)
if left_sign:
self.left_sign = True
# Color and Depth
self.sub_left_color = Subscriber(self, Image, configs["left"]["color"])
@@ -88,8 +96,15 @@ class CollectDataNode(Node):
self.get_logger().info("left camera online")
# right
if self.topic_exists(configs["right"]["color"]) and self.topic_exists(
configs["right"]["depth"]):
right_sign = False
for _ in range(4):
if self.topic_exists(configs["right"]["color"]) and self.topic_exists(
configs["right"]["depth"]):
right_sign = True
break
else:
time.sleep(0.5)
if right_sign:
self.right_sign = True
# Color and Depth
self.sub_right_color = Subscriber(self, Image, configs["right"]["color"])
@@ -106,8 +121,15 @@ class CollectDataNode(Node):
self.get_logger().info("right camera online")
# head
if self.topic_exists(configs["head"]["color"]) and self.topic_exists(
configs["head"]["depth"]):
head_sign = False
for _ in range(4):
if self.topic_exists(configs["head"]["color"]) and self.topic_exists(
configs["head"]["depth"]):
head_sign = True
break
else:
time.sleep(0.5)
if head_sign:
self.head_sign = True
# Color and Depth
self.sub_head_color = Subscriber(self, Image, configs["head"]["color"])