1

mission-2d-slice-sum.py

@@ -0,0 +1,159 @@
+import open3d as o3d
+import numpy as np
+import math
+import math
+from functools import reduce
+import sys
+import copy
+import random
+
+ratio_factor = 0.9
+neighbor_point_min_num = 8
+
+# pcd = o3d.io.read_point_cloud("data/stockpile.ply")
+pcd = o3d.io.read_point_cloud("data/2box-SG-t-MnQ0zoEQIBX.ply")
+# pcd = o3d.io.read_point_cloud("data/4box-1-SG-t-AsSRO3iA0XT.ply")
+# pcd = o3d.io.read_point_cloud("data/4box-2-SG-t-cp3kSewxMKi.ply")
+# pcd = o3d.io.read_point_cloud("data/4box-3-SG-t-kuuv0moEP0C.ply")
+print(pcd)
+assert (pcd.has_normals())
+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,
+                                         num_iterations=10000)
+
+# 把地面上的点和其他点分开
+[a, b, c, d] = plane_model
+plane_pcd = pcd.select_by_index(inliers)
+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])
+
+# 移动到原点
+plane_pcd = plane_pcd.translate((0,0,d/c))
+stockpile_pcd = stockpile_pcd.translate((0,0,d/c))
+
+# 旋转至与坐标轴对齐
+cos_theta = c / math.sqrt(a**2 + b**2 + c**2)
+sin_theta = math.sqrt((a**2+b**2)/(a**2 + b**2 + c**2))
+u_1 = b / math.sqrt(a**2 + b**2 )
+u_2 = -a / math.sqrt(a**2 + b**2)
+rotation_matrix = np.array([[cos_theta + u_1**2 * (1-cos_theta), u_1*u_2*(1-cos_theta), u_2*sin_theta],
+                            [u_1*u_2*(1-cos_theta), cos_theta + u_2**2*(1- cos_theta), -u_1*sin_theta],
+                            [-u_2*sin_theta, u_1*sin_theta, cos_theta]])
+plane_pcd.rotate(rotation_matrix)
+stockpile_pcd.rotate(rotation_matrix)
+o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
+
+# 用statistical outlier算法去除噪点
+cl, ind = stockpile_pcd.remove_statistical_outlier(nb_neighbors=100,
+                                                    std_ratio=1)
+stockpile_pcd = stockpile_pcd.select_by_index(ind)
+
+# 降采样
+# downpcd = stockpile_pcd.voxel_down_sample(voxel_size=0.1)
+downpcd = stockpile_pcd
+print('有效点云：', np.asarray(downpcd.points))
+
+# 建立三维网格
+bbox = downpcd.get_axis_aligned_bounding_box()
+my_bbox = {
+  'min_bound': (bbox.min_bound[0], bbox.min_bound[1], bbox.min_bound[2]),
+  'max_bound': ((bbox.max_bound[0], bbox.max_bound[1], bbox.max_bound[2] + 0.5)),
+}
+print('my_bbox: ', my_bbox)
+bin_num_1d = 30
+bin_size_x = (my_bbox['max_bound'][0] - my_bbox['min_bound'][0]) / bin_num_1d
+bin_size_y = (my_bbox['max_bound'][1] - my_bbox['min_bound'][1]) / bin_num_1d
+bin_size_z = (my_bbox['max_bound'][2] - my_bbox['min_bound'][2]) / bin_num_1d
+grid = np.zeros((bin_num_1d, bin_num_1d, bin_num_1d))
+print('bin_size: ', bin_size_x, bin_size_y, bin_size_z)
+
+points = np.asarray(downpcd.points)
+for point in points: 
+  bin_idx_x = math.floor((point[0] - my_bbox['min_bound'][0]) / (my_bbox['max_bound'][0] - my_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] - my_bbox['min_bound'][1]) / (my_bbox['max_bound'][1] - my_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
+
+  bin_idx_z = math.floor((point[2] - my_bbox['min_bound'][2]) / (my_bbox['max_bound'][2] - my_bbox['min_bound'][2]) * bin_num_1d)
+  if bin_idx_z <= -1: # 浮点数溢出
+    bin_idx_z = 0
+  if bin_idx_z >= bin_num_1d:
+    bin_idx_z = bin_num_1d - 1
+
+  # print('point', point)
+  # print(grid[bin_idx_x, bin_idx_y, bin_idx_z])
+  grid[bin_idx_x, bin_idx_y, bin_idx_z] += 1
+
+min_point_num_threshold = 1
+assert(grid[0, 0, bin_num_1d - 1] < min_point_num_threshold)
+grid_voxel = np.zeros((bin_num_1d, bin_num_1d, bin_num_1d)) # 0: 初始状态；-1：已加入待处理列表； 1：已处理，无点；2：已处理，有点；
+cellToProcessList = [(0, 0, bin_num_1d - 1)]
+grid_voxel[0, 0, bin_num_1d - 1] = -1
+count1 = 0
+count2 = 0
+
+def processCell():
+  global min_point_num_threshold
+  global grid
+  global grid_voxel
+  global cellToProcessList
+  global count1
+  global count2
+  i = cellToProcessList[0][0]
+  j = cellToProcessList[0][1]
+  k = cellToProcessList[0][2]
+  assert(grid_voxel[i, j, k] == -1)
+  if grid[i, j, k] < min_point_num_threshold:
+    grid_voxel[i, j, k] = 1
+    count1 += 1
+    if i - 1 >= 0 and grid_voxel[i - 1, j, k] == 0:
+      cellToProcessList.append((i - 1, j, k))
+      grid_voxel[i - 1, j, k] = -1
+    if i + 1 <= bin_num_1d - 1 and grid_voxel[i + 1, j, k] == 0:
+      cellToProcessList.append((i + 1, j, k))
+      grid_voxel[i + 1, j, k] = -1
+    if j - 1 >= 0 and grid_voxel[i, j - 1, k] == 0:
+      cellToProcessList.append((i, j - 1, k))
+      grid_voxel[i, j - 1, k] = -1
+    if j + 1 <= bin_num_1d - 1 and grid_voxel[i, j + 1, k] == 0:
+      cellToProcessList.append((i, j + 1, k))
+      grid_voxel[i, j + 1, k] = -1
+    if k - 1 >= 0 and grid_voxel[i, j, k - 1] == 0:
+      cellToProcessList.append((i, j, k - 1))
+      grid_voxel[i, j, k - 1] = -1
+    if k + 1 <= bin_num_1d - 1 and grid_voxel[i, j, k + 1] == 0:
+      cellToProcessList.append((i, j, k + 1))
+      grid_voxel[i, j, k + 1] = -1
+  else:
+    grid_voxel[i, j, k] = 2
+    count2 += 1
+  cellToProcessList.pop(0)
+
+
+while len(cellToProcessList) > 0:
+  processCell()
+
+print('空虚格子数：', count1)
+print('边界格子数：', count2)
+print('内部格子数：', bin_num_1d * bin_num_1d * bin_num_1d - count1 - 0.5 * count2)
+print('物体占比：', (bin_num_1d * bin_num_1d * bin_num_1d - count1 - 0.5 * count2) / (bin_num_1d * bin_num_1d * bin_num_1d))
+print('体积：', (bin_num_1d * bin_num_1d * bin_num_1d - count1 - 0.5 * count2) * bin_size_x * bin_size_y * bin_size_z)
+print('外部体积：', (count1 + 0.5 * count2) * bin_size_x * bin_size_y * bin_size_z)
+print('0, 0, 最高：', grid_voxel[0, 0, bin_num_1d - 1])
+print('0, 0, 最低：', grid_voxel[0, 0, 0])

mission_convex_hull.py

@@ -1,58 +0,0 @@
-import open3d as o3d
-import numpy as np
-import math
-from scipy.spatial import Delaunay
-import math
-from functools import reduce
-import sys
-import copy
-
-pcd = o3d.io.read_point_cloud("data/stockpile.ply")
-# pcd = o3d.io.read_point_cloud("../laser1.ply")
-# print(pcd)  # 输出点云点的个数
-o3d.visualization.draw([pcd])
-print("Paint pointcloud")
-
-# 找出地面所在平面
-plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
-                                         ransac_n=3,
-                                         num_iterations=10000)
-
-# 把地面上的点和其他点分开
-[a, b, c, d] = plane_model
-plane_pcd = pcd.select_by_index(inliers)
-plane_pcd.paint_uniform_color([1.0, 0, 0])
-stockpile_pcd = pcd.select_by_index(inliers, invert=True)
-stockpile_pcd.paint_uniform_color([0, 0, 1.0])
-# o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
-
-# 移动到原点
-plane_pcd = plane_pcd.translate((0,0,d/c))
-stockpile_pcd = stockpile_pcd.translate((0,0,d/c))
-
-# 旋转至与坐标轴对齐
-cos_theta = c / math.sqrt(a**2 + b**2 + c**2)
-sin_theta = math.sqrt((a**2+b**2)/(a**2 + b**2 + c**2))
-u_1 = b / math.sqrt(a**2 + b**2 )
-u_2 = -a / math.sqrt(a**2 + b**2)
-rotation_matrix = np.array([[cos_theta + u_1**2 * (1-cos_theta), u_1*u_2*(1-cos_theta), u_2*sin_theta],
-                            [u_1*u_2*(1-cos_theta), cos_theta + u_2**2*(1- cos_theta), -u_1*sin_theta],
-                            [-u_2*sin_theta, u_1*sin_theta, cos_theta]])
-plane_pcd.rotate(rotation_matrix)
-stockpile_pcd.rotate(rotation_matrix)
-# o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
-
-# 去除噪点
-cl, ind = stockpile_pcd.remove_statistical_outlier(nb_neighbors=50,
-                                                    std_ratio=5.0)
-stockpile_pcd = stockpile_pcd.select_by_index(ind)
-
-o3d.visualization.draw([stockpile_pcd])
- 
-hull, idx = stockpile_pcd.compute_convex_hull()
-hull.paint_uniform_color([1, 0.7, 0])  # 给凸包渲染颜色
-area = hull.get_surface_area()  # 计算表面积
-volume = hull.get_volume()      # 计算体积
-print("表面积为：", area)
-print("体积为：", volume)
-o3d.visualization.draw_geometries([stockpile_pcd, hull])

mission_stock_pile_voxel_true_data.py

@@ -6,12 +6,14 @@ from functools import reduce
 import sys
 import copy
 
+# pcd = o3d.io.read_point_cloud("data/stockpile.ply")
 # pcd = o3d.io.read_point_cloud("data/2box-SG-t-MnQ0zoEQIBX.ply")
 # pcd = o3d.io.read_point_cloud("data/4box-1-SG-t-AsSRO3iA0XT.ply")
 # pcd = o3d.io.read_point_cloud("data/4box-2-SG-t-cp3kSewxMKi.ply")
 pcd = o3d.io.read_point_cloud("data/4box-3-SG-t-kuuv0moEP0C.ply")
 print(pcd)
 assert (pcd.has_normals())
+o3d.visualization.draw_geometries([pcd])
 
 axes = o3d.geometry.TriangleMesh.create_coordinate_frame()
 
@@ -45,30 +47,20 @@ plane_pcd.rotate(rotation_matrix)
 stockpile_pcd.rotate(rotation_matrix)
 o3d.visualization.draw_geometries([plane_pcd, stockpile_pcd, axes])
 
-# 去除噪点
-cl, ind = stockpile_pcd.remove_statistical_outlier(nb_neighbors=80,
-                                                    std_ratio=0.1)
+# 用statistical outlier算法去除噪点
+cl, ind = stockpile_pcd.remove_statistical_outlier(nb_neighbors=100,
+                                                    std_ratio=1)
 stockpile_pcd = stockpile_pcd.select_by_index(ind)
-print('有效点：', stockpile_pcd)
-o3d.visualization.draw_geometries([stockpile_pcd])
 
 # 降采样
 # downpcd = stockpile_pcd.voxel_down_sample(voxel_size=0.1)
 downpcd = stockpile_pcd
 print('有效点云：', np.asarray(downpcd.points))
 
-mean = 0
-meanCount = 0
-for point in np.asarray(downpcd.points):
-  mean += point[2]
-  meanCount += 1
-mean = mean / meanCount
-print('z均值：', mean)
-
 # 建立二维网格
 bbox = downpcd.get_axis_aligned_bounding_box()
 print('bbox: ', bbox)
-bin_num_1d = 200
+bin_num_1d = 250
 bin_size_x = (bbox.max_bound[0] - bbox.min_bound[0]) / bin_num_1d
 bin_size_y = (bbox.max_bound[1] - bbox.min_bound[1]) / bin_num_1d
 grid2d = np.zeros((bin_num_1d, bin_num_1d, 2))
@@ -107,7 +99,6 @@ kd_tree_2d = o3d.geometry.KDTreeFlann(downpcd_2d)
 # 二维网格体积累积
 sum = 0
 interCount = 0
-interSum = 0
 
 for i in range(0, bin_num_1d):
   for j in range(0, bin_num_1d):
@@ -116,7 +107,7 @@ for i in range(0, bin_num_1d):
     if grid2d[i, j, 1] == 0:
       # print('做插值……')
       interCount += 1
-      [k, neighbor_idx_list, _] = kd_tree_2d.search_knn_vector_3d([bin_size_x * (i + 0.5) + bbox.min_bound[0], bin_size_y * (j + 0.5) + bbox.min_bound[1], 0], 100)
+      [k, neighbor_idx_list, _] = kd_tree_2d.search_knn_vector_3d([bin_size_x * (i + 0.5) + bbox.min_bound[0], bin_size_y * (j + 0.5) + bbox.min_bound[1], 0], 50)
       
       # 针对物体有垂直的侧面的情况，拿k近邻中z值最小的
       neighbor_pcd = downpcd.select_by_index(neighbor_idx_list)
@@ -125,10 +116,9 @@ for i in range(0, bin_num_1d):
       for pos in neighbor_pcd_pos:
         if pos[2] < z_min:
           z_min = pos[2]
-      
+
       grid2d[i, j, 0] = z_min
       grid2d[i, j, 1] = 1
-      interSum += z_min
 
     if(grid2d[i, j, 1]):
       sum += (grid2d[i, j, 0] / grid2d[i, j, 1] * bin_size_x * bin_size_y)
@@ -139,9 +129,7 @@ for i in range(0, bin_num_1d):
     # print('sum: ', sum)
     # print('~~~~~~~~~~~~~')
 
-print('插值均值：', interSum / interCount)
-
 print('体积：', sum)
-print('插值次数：', interCount)
+# print('插值次数：', interCount)
 
 exit(0)

readme.md

@@ -97,16 +97,20 @@ viewer.modules.clipping.getAllBoxes().filter(e=>!e.isNew).map(e=>e.box.geometry.
 
 ## https://uat-laser.4dkankan.com/uat/index.html?m=SG-t-MnQ0zoEQIBX#/
 * （按尺寸标注计算：0.470立方米）
-* stockpile voxel: 0.43 立方米
+* 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
 
 ## https://uat-laser.4dkankan.com/uat/index.html?m=SG-t-cp3kSewxMKi#/
 * （按尺寸标注计算：0.689立方米）
 * stockpile voxel: 0.70立方米
+* 0.81
 
 ## https://uat-laser.4dkankan.com/uat/index.html?m=SG-t-kuuv0moEP0C#/
-*（按尺寸标注计算：0.689立方米）
-* stockpile voxel: 0.84立方米
+* （按尺寸标注计算：0.689立方米）
+* stockpile voxel: 0.80立方米
+* 0.70