|
|
@@ -13,13 +13,13 @@ import copy
|
|
|
pcd = o3d.io.read_point_cloud("data/4box-3-SG-t-kuuv0moEP0C.ply")
|
|
|
print(pcd)
|
|
|
assert (pcd.has_normals())
|
|
|
-o3d.visualization.draw_geometries([pcd])
|
|
|
+# o3d.visualization.draw_geometries([pcd])
|
|
|
|
|
|
axes = o3d.geometry.TriangleMesh.create_coordinate_frame()
|
|
|
|
|
|
# 找出地面所在平面
|
|
|
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
|
|
|
- ransac_n=3,
|
|
|
+ ransac_n=5,
|
|
|
num_iterations=10000)
|
|
|
|
|
|
# 把地面上的点和其他点分开
|
|
|
@@ -29,7 +29,9 @@ plane_pcd.paint_uniform_color([1.0, 0, 0])
|
|
|
stockpile_pcd = pcd.select_by_index(inliers, invert=True)
|
|
|
# stockpile_pcd = pcd
|
|
|
stockpile_pcd.paint_uniform_color([0, 0, 1.0])
|
|
|
-o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
|
|
|
+# o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes], point_show_normal=True)
|
|
|
+o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd])
|
|
|
+# o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
|
|
|
|
|
|
# 移动到原点
|
|
|
plane_pcd = plane_pcd.translate((0,0,d/c))
|
|
|
@@ -51,13 +53,38 @@ o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
|
|
|
cl, ind = stockpile_pcd.remove_statistical_outlier(nb_neighbors=100,
|
|
|
std_ratio=1)
|
|
|
stockpile_pcd = stockpile_pcd.select_by_index(ind)
|
|
|
+o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
|
|
|
+
|
|
|
+# 依据法向量去除噪点
|
|
|
+kd_tree_3d = o3d.geometry.KDTreeFlann(stockpile_pcd)
|
|
|
+noise_list_by_normal = []
|
|
|
+for idx in range (0, len(stockpile_pcd.points)):
|
|
|
+ [k, neighbor_idx_list, _] = kd_tree_3d.search_radius_vector_3d(stockpile_pcd.points[idx], 0.1)
|
|
|
+ if len(neighbor_idx_list) == 0:
|
|
|
+ continue
|
|
|
+ normal_sum = np.zeros((3))
|
|
|
+ count = 0
|
|
|
+ for neib_idx in neighbor_idx_list:
|
|
|
+ normal_sum += np.asarray(stockpile_pcd.normals[neib_idx])
|
|
|
+ count += 1
|
|
|
+ normal_mean = normal_sum / count
|
|
|
+ normal_angle = normal_mean[0] * stockpile_pcd.normals[idx][0] + normal_mean[1] * stockpile_pcd.normals[idx][1] + normal_mean[2] * stockpile_pcd.normals[idx][2]
|
|
|
+ if normal_angle < 0:
|
|
|
+ print(normal_angle)
|
|
|
+ noise_list_by_normal.append(idx)
|
|
|
+noise_by_normal_pcd = stockpile_pcd.select_by_index(noise_list_by_normal)
|
|
|
+noise_by_normal_pcd.paint_uniform_color([0, 1.0, 0])
|
|
|
+stockpile_pcd = stockpile_pcd.select_by_index(noise_list_by_normal, invert=True)
|
|
|
+o3d.visualization.draw_geometries([plane_pcd, noise_by_normal_pcd, axes])
|
|
|
+o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
|
|
|
+
|
|
|
|
|
|
# 降采样
|
|
|
# downpcd = stockpile_pcd.voxel_down_sample(voxel_size=0.1)
|
|
|
downpcd = stockpile_pcd
|
|
|
print('有效点云：', np.asarray(downpcd.points))
|
|
|
|
|
|
-# 建立二维网格
|
|
|
+# 对stockpile建立二维网格
|
|
|
bbox = downpcd.get_axis_aligned_bounding_box()
|
|
|
print('bbox: ', bbox)
|
|
|
bin_num_1d = 250
|
|
|
@@ -88,7 +115,39 @@ for point in points:
|
|
|
grid2d[bin_idx_x, bin_idx_y, 1] += 1
|
|
|
# print('bin after ', bin_idx_x, bin_idx_y, grid2d[bin_idx_x, bin_idx_y, 0], grid2d[bin_idx_x, bin_idx_y, 1])
|
|
|
|
|
|
-# 建立二维kdtree以便查询邻居，对二维网格中没有点的网格做插值
|
|
|
+# 对地面建立二维网格,空的格子就是被物体盖住的。
|
|
|
+grid_ground = np.zeros((bin_num_1d, bin_num_1d))
|
|
|
+for ground_point in np.asarray(plane_pcd.points):
|
|
|
+ if ground_point[0] >= bbox.min_bound[0] and ground_point[0] <= bbox.max_bound[0] and ground_point[1] >= bbox.min_bound[1] and ground_point[1] <= bbox.max_bound[1]:
|
|
|
+ bin_idx_x = math.floor((ground_point[0] - bbox.min_bound[0]) / (bbox.max_bound[0] - bbox.min_bound[0]) * bin_num_1d)
|
|
|
+ if bin_idx_x <= -1: # 浮点数溢出
|
|
|
+ bin_idx_x = 0
|
|
|
+ if bin_idx_x >= bin_num_1d:
|
|
|
+ bin_idx_x = bin_num_1d - 1
|
|
|
+
|
|
|
+ bin_idx_y = math.floor((ground_point[1] - bbox.min_bound[1]) / (bbox.max_bound[1] - bbox.min_bound[1]) * bin_num_1d)
|
|
|
+ if bin_idx_y <= -1: # 浮点数溢出
|
|
|
+ bin_idx_y = 0
|
|
|
+ if bin_idx_y >= bin_num_1d:
|
|
|
+ bin_idx_y = bin_num_1d - 1
|
|
|
+
|
|
|
+ grid_ground[bin_idx_x, bin_idx_y] += 1
|
|
|
+
|
|
|
+for i in range(0, len(stockpile_pcd.points)):
|
|
|
+ point = stockpile_pcd.points[i]
|
|
|
+ bin_idx_x = math.floor((point[0] - bbox.min_bound[0]) / (bbox.max_bound[0] - bbox.min_bound[0]) * bin_num_1d)
|
|
|
+ if bin_idx_x <= -1: # 浮点数溢出
|
|
|
+ bin_idx_x = 0
|
|
|
+ if bin_idx_x >= bin_num_1d:
|
|
|
+ bin_idx_x = bin_num_1d - 1
|
|
|
+
|
|
|
+ bin_idx_y = math.floor((point[1] - bbox.min_bound[1]) / (bbox.max_bound[1] - bbox.min_bound[1]) * bin_num_1d)
|
|
|
+ if bin_idx_y <= -1: # 浮点数溢出
|
|
|
+ bin_idx_y = 0
|
|
|
+ if bin_idx_y >= bin_num_1d:
|
|
|
+ bin_idx_y = bin_num_1d - 1
|
|
|
+
|
|
|
+# 对stockpile建立二维kdtree以便查询邻居，对stockpile二维网格中没有点的网格做插值
|
|
|
downpcd_2d = copy.deepcopy(downpcd)
|
|
|
for point in downpcd_2d.points:
|
|
|
point[2] = 0
|
|
|
@@ -103,6 +162,8 @@ interCount = 0
|
|
|
for i in range(0, bin_num_1d):
|
|
|
for j in range(0, bin_num_1d):
|
|
|
# print('~~~~~~~~~~~~~')
|
|
|
+ if grid_ground[i, j] == 1:
|
|
|
+ continue
|
|
|
|
|
|
if grid2d[i, j, 1] == 0:
|
|
|
# print('做插值……')
|
任一存