import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt

# todo 处理前需要点云对齐


# 打印点云
print("Load a ply point cloud, print it, and render it")
pcd = o3d.io.read_point_cloud("../test_data/fragment.ply")
print(pcd)
print(np.asarray(pcd.points))

# 体素下采样
print("Downsample the point cloud with a voxel of 0.05")
downpcd = pcd.voxel_down_sample(voxel_size=0.05)

# 估计法向量
downpcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

"""
o3d.visualization.draw_geometries([downpcd],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024],
                                  point_show_normal=True)
"""

#   打印法向量
print("Print a normal vector of the 0th point")
print(downpcd.normals[0])

#   裁剪一块区域

print("Load a polygon volume and use it to crop the original point cloud")
vol = o3d.visualization.read_selection_polygon_volume(
    "../test_data/Crop/cropped.json")
chair = vol.crop_point_cloud(pcd)
"""
o3d.visualization.draw_geometries([chair],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
"""

# 用点云匹配已有点云 注意 这里的点云是有位置的 不是一个无位置的
dists = pcd.compute_point_cloud_distance(chair)
dists = np.asarray(dists)
ind = np.where(dists > 0.01)[0]
pcd_without_chair = pcd.select_by_index(ind)
"""
o3d.visualization.draw_geometries([pcd_without_chair],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024])
"""

aabb = chair.get_axis_aligned_bounding_box()
aabb.color = (1, 0, 0)
obb = chair.get_oriented_bounding_box()
obb.color = (0, 1, 0)
#"""
o3d.visualization.draw_geometries([pcd, aabb, obb],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
#"""

"""
hull, _ = chair.compute_convex_hull()
hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)
hull_ls.paint_uniform_color((1, 0, 0))
o3d.visualization.draw_geometries([hull_ls, chair])
"""

#   聚类
"""
with o3d.utility.VerbosityContextManager(
        o3d.utility.VerbosityLevel.Debug) as cm:
    labels = np.array(
        pcd.cluster_dbscan(eps=0.02, min_points=10, print_progress=True))

max_label = labels.max()
print(f"point cloud has {max_label + 1} clusters")
colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1))
colors[labels < 0] = 0
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_geometries([pcd],
                                  zoom=0.455,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])
"""

"""
# 分割
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
                                         ransac_n=3,
                                         num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
outlier_cloud = pcd.select_by_index(inliers, invert=True)



o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud],
                                  zoom=0.8,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])

"""

# 移除不可见点
"""
diameter = np.linalg.norm(
    np.asarray(chair.get_max_bound()) - np.asarray(chair.get_min_bound()))

print("Define parameters used for hidden_point_removal")
camera = [0, 0, diameter]
radius = diameter * 100

print("Get all points that are visible from given view point")
_, pt_map = chair.hidden_point_removal(camera, radius)

print("Visualize result")
chair = chair.select_by_index(pt_map)
"""


import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt

# todo 处理前需要点云对齐


# 打印点云
print("Load a ply point cloud, print it, and render it")
pcd = o3d.io.read_point_cloud("../test_data/fragment.ply")
print(pcd)
print(np.asarray(pcd.points))

# 体素下采样
print("Downsample the point cloud with a voxel of 0.05")
downpcd = pcd.voxel_down_sample(voxel_size=0.05)

# 估计法向量
downpcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

"""
o3d.visualization.draw_geometries([downpcd],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024],
                                  point_show_normal=True)
"""

#   打印法向量
print("Print a normal vector of the 0th point")
print(downpcd.normals[0])

#   裁剪一块区域

print("Load a polygon volume and use it to crop the original point cloud")
vol = o3d.visualization.read_selection_polygon_volume(
    "../test_data/Crop/cropped.json")
chair = vol.crop_point_cloud(pcd)
"""
o3d.visualization.draw_geometries([chair],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
"""

# 用点云匹配已有点云 注意 这里的点云是有位置的 不是一个无位置的
dists = pcd.compute_point_cloud_distance(chair)
dists = np.asarray(dists)
ind = np.where(dists > 0.01)[0]
pcd_without_chair = pcd.select_by_index(ind)
"""
o3d.visualization.draw_geometries([pcd_without_chair],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024])
"""

aabb = chair.get_axis_aligned_bounding_box()
aabb.color = (1, 0, 0)
obb = chair.get_oriented_bounding_box()
obb.color = (0, 1, 0)
#"""
o3d.visualization.draw_geometries([pcd, aabb, obb],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
#"""

"""
hull, _ = chair.compute_convex_hull()
hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)
hull_ls.paint_uniform_color((1, 0, 0))
o3d.visualization.draw_geometries([hull_ls, chair])
"""

#   聚类
"""
with o3d.utility.VerbosityContextManager(
        o3d.utility.VerbosityLevel.Debug) as cm:
    labels = np.array(
        pcd.cluster_dbscan(eps=0.02, min_points=10, print_progress=True))

max_label = labels.max()
print(f"point cloud has {max_label + 1} clusters")
colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1))
colors[labels < 0] = 0
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_geometries([pcd],
                                  zoom=0.455,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])
"""

"""
# 分割
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
                                         ransac_n=3,
                                         num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
outlier_cloud = pcd.select_by_index(inliers, invert=True)



o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud],
                                  zoom=0.8,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])

"""

# 移除不可见点
"""
diameter = np.linalg.norm(
    np.asarray(chair.get_max_bound()) - np.asarray(chair.get_min_bound()))

print("Define parameters used for hidden_point_removal")
camera = [0, 0, diameter]
radius = diameter * 100

print("Get all points that are visible from given view point")
_, pt_map = chair.hidden_point_removal(camera, radius)

print("Visualize result")
chair = chair.select_by_index(pt_map)
"""


# todo 处理前需要点云对齐 # 打印点云 # 体素下采样 # 估计法向量 o3d.visualization.draw_geometries([downpcd],                                   zoom=0.3412,                                   front=[0.4257, -0.2125, -0.8795],                                   lookat=[2.6172, 2.0475, 1.532],                                   up=[-0.0694, -0.9768, 0.2024],                                   point_show_normal=True) #   打印法向量 #   裁剪一块区域 o3d.visualization.draw_geometries([chair],                                   zoom=0.7,                                   front=[0.5439, -0.2333, -0.8060],                                   lookat=[2.4615, 2.1331, 1.338],                                   up=[-0.1781, -0.9708, 0.1608]) # 用点云匹配已有点云 注意 这里的点云是有位置的 不是一个无位置的 o3d.visualization.draw_geometries([pcd_without_chair],                                   zoom=0.3412,                                   front=[0.4257, -0.2125, -0.8795],                                   lookat=[2.6172, 2.0475, 1.532],                                   up=[-0.0694, -0.9768, 0.2024]) #""" #""" hull, _ = chair.compute_convex_hull() hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull) hull_ls.paint_uniform_color((1, 0, 0)) o3d.visualization.draw_geometries([hull_ls, chair]) #   聚类 with o3d.utility.VerbosityContextManager(         o3d.utility.VerbosityLevel.Debug) as cm:     labels = np.array(         pcd.cluster_dbscan(eps=0.02, min_points=10, print_progress=True))  max_label = labels.max() print(f"point cloud has {max_label + 1} clusters") colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1)) colors[labels < 0] = 0 pcd.colors = o3d.utility.Vector3dVector(colors[:, :3]) o3d.visualization.draw_geometries([pcd],                                   zoom=0.455,                                   front=[-0.4999, -0.1659, -0.8499],                                   lookat=[2.1813, 2.0619, 2.0999],                                   up=[0.1204, -0.9852, 0.1215]) # 分割 plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,                                          ransac_n=3,                                          num_iterations=1000) [a, b, c, d] = plane_model print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")  inlier_cloud = pcd.select_by_index(inliers) inlier_cloud.paint_uniform_color([1.0, 0, 0]) outlier_cloud = pcd.select_by_index(inliers, invert=True)    o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud],                                   zoom=0.8,                                   front=[-0.4999, -0.1659, -0.8499],                                   lookat=[2.1813, 2.0619, 2.0999],                                   up=[0.1204, -0.9852, 0.1215]) # 移除不可见点 diameter = np.linalg.norm(     np.asarray(chair.get_max_bound()) - np.asarray(chair.get_min_bound()))  print("Define parameters used for hidden_point_removal") camera = [0, 0, diameter] radius = diameter * 100  print("Get all points that are visible from given view point") _, pt_map = chair.hidden_point_removal(camera, radius)  print("Visualize result") chair = chair.select_by_index(pt_map)