1

mission_stock_pile_voxel_true_data.py

@@ -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('做插值……')

readme.md

@@ -95,22 +95,22 @@ viewer.modules.clipping.getAllBoxes().filter(e=>!e.isNew).map(e=>e.box.geometry.
 
 # 结果
 
+## demo
+* 0.0104
+* 0.0983
+
 ## https://uat-laser.4dkankan.com/uat/index.html?m=SG-t-MnQ0zoEQIBX#/
 * （按尺寸标注计算：0.470立方米）
 * stockpile voxel: 0.45 立方米
-* 0.52
 
 ## https://uat-laser.4dkankan.com/uat/index.html?m=SG-t-AsSRO3iA0XT#/
 * （按尺寸标注计算：0.689立方米）
-* stockpile voxel: 0.75立方米
-* 0.79
+* stockpile voxel: 0.74立方米
 
 ## https://uat-laser.4dkankan.com/uat/index.html?m=SG-t-cp3kSewxMKi#/
 * （按尺寸标注计算：0.689立方米）
-* stockpile voxel: 0.70立方米
-* 0.81
+* stockpile voxel: 0.69立方米
 
 ## https://uat-laser.4dkankan.com/uat/index.html?m=SG-t-kuuv0moEP0C#/
 * （按尺寸标注计算：0.689立方米）
-* stockpile voxel: 0.80立方米
-* 0.70
+* stockpile voxel: 0.77立方米