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-03 17:33:41 +08:00
parent 5f7f5881fb
commit 3ed1182472
2 changed files with 31 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -457,7 +457,7 @@ class DetectNode(Node):
continue
grab_width = calculate_grav_width(mask, self.k, rmat[2, 3])
rmat[2, 3] = rmat[2, 3] + grab_width * 0.38
rmat[2, 3] = rmat[2, 3] + grab_width * 0.30
rmat = self.hand_eye_mat @ rmat

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

@@ -45,14 +45,14 @@ def calculate_pose_pca(
depth_o3d = o3d.geometry.Image(depth_img_mask.astype(np.uint16))
point_cloud = o3d.geometry.PointCloud.create_from_depth_image(
point_cloud_u = o3d.geometry.PointCloud.create_from_depth_image(
depth=depth_o3d,
intrinsic=intrinsics,
depth_scale=kwargs.get("depth_scale", 1000.0),
depth_trunc=kwargs.get("depth_trunc", 3.0),
)
point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.010))
point_cloud = point_cloud_denoising(point_cloud_u, kwargs.get("voxel_size", 0.010))
if point_cloud is None:
return None
@@ -80,8 +80,8 @@ def calculate_pose_pca(
R = np.column_stack((vx, vy, vz))
rmat = tfs.affines.compose(np.squeeze(np.asarray((x, y, z))), R, [1, 1, 1])
# draw(point_cloud, rmat)
# draw(point_cloud_1, rmat)
draw(point_cloud_u, rmat)
draw(point_cloud, rmat)
return rmat
@@ -127,7 +127,8 @@ def calculate_pose_icp(
def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: float = 0.010):
"""点云去噪"""
point_cloud = point_cloud.remove_non_finite_points()
down_pcd = point_cloud.voxel_down_sample(voxel_size=voxel_size)
down_pcd = point_cloud.voxel_down_sample(voxel_size=voxel_size * 0.5)
point_num = len(down_pcd.points)
# 半径滤波
clean_pcd, _ = down_pcd.remove_radius_outlier(
@@ -177,8 +178,8 @@ def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: floa
clean_pcd = point_cloud_clusters[0][0]
# 使用最近簇判断噪音强度
largest_cluster_ratio = len(clean_pcd.points) / len(points)
if largest_cluster_ratio < 0.2:
largest_cluster_ratio = len(clean_pcd.points) / point_num
if largest_cluster_ratio < 0.20:
return None
return clean_pcd
@@ -220,25 +221,25 @@ def quat2rmat(quat):
rmat = tfs.affines.compose(np.squeeze(np.asarray((x, y, z))), r, [1, 1, 1])
return rmat
# def draw(pcd, mat):
# R = mat[0:3, 0:3]
# point = mat[0:3, 3:4].flatten()
# x, y, z = R[:, 0], R[:, 1], R[:, 2]
#
# points = [
# [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1],
# point, point + x, point + y, point + z
#
# ] # 画点：原点、第一主成分、第二主成分
# lines = [
# [0, 1], [0, 2], [0, 3],
# [4, 5], [4, 6], [4, 7]
# ] # 画出三点之间两两连线
# colors = [
# [1, 0, 0], [0, 1, 0], [0, 0, 1],
# [1, 0, 0], [0, 1, 0], [0, 0, 1]
# ]
# line_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(points), lines=o3d.utility.Vector2iVector(lines))
# line_set.colors = o3d.utility.Vector3dVector(colors)
#
# o3d.visualization.draw_geometries([pcd, line_set])
def draw(pcd, mat):
R = mat[0:3, 0:3]
point = mat[0:3, 3:4].flatten()
x, y, z = R[:, 0], R[:, 1], R[:, 2]
points = [
[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1],
point, point + x, point + y, point + z
] # 画点：原点、第一主成分、第二主成分
lines = [
[0, 1], [0, 2], [0, 3],
[4, 5], [4, 6], [4, 7]
] # 画出三点之间两两连线
colors = [
[1, 0, 0], [0, 1, 0], [0, 0, 1],
[1, 0, 0], [0, 1, 0], [0, 0, 1]
]
line_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(points), lines=o3d.utility.Vector2iVector(lines))
line_set.colors = o3d.utility.Vector3dVector(colors)
o3d.visualization.draw_geometries([pcd, line_set])