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])