sandbox-open3d/03_inOFFpc_getVector.py at main · Qu0rise/sandbox-open3d · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

import open3d as o3d
import numpy as np
import copy
def visualize_planes_interactive(pcd, planes):
   if not hasattr(pcd, 'colors'):
       pcd.colors = o3d.utility.Vector3dVector(np.ones((len(pcd.points), 3)) * 0.5)

   current_plane_idx = 0
   vis = o3d.visualization.VisualizerWithKeyCallback()

   def next_plane(vis):
       nonlocal current_plane_idx
       current_plane_idx = (current_plane_idx + 1) % len(planes)
       update_view()
       return True

   def prev_plane(vis):
       nonlocal current_plane_idx
       current_plane_idx = (current_plane_idx - 1) % len(planes)
       update_view()
       return True

   def update_view():
       plane = planes[current_plane_idx]
       center = plane.center
       normal = plane.R[:, 2]

       view_control = vis.get_view_control()
       camera_params = view_control.convert_to_pinhole_camera_parameters()

       vis.clear_geometries()
       vis.add_geometry(pcd)

       mesh = o3d.geometry.TriangleMesh.create_from_oriented_bounding_box(
           plane, scale=[1, 1, 0.0001])
       mesh.paint_uniform_color([0.8, 0.8, 0.8])
       vis.add_geometry(mesh)

       arrow = o3d.geometry.TriangleMesh.create_arrow(
           cone_radius=0.02, cone_height=0.05,
           cylinder_radius=0.01, cylinder_height=0.1)
       arrow.translate(center)
       R = o3d.geometry.get_rotation_matrix_from_xyz(
           [0, np.arccos(normal[2]), np.arctan2(normal[1], normal[0])])
       arrow.rotate(R)
       arrow.paint_uniform_color([1, 0, 0])
       vis.add_geometry(arrow)

       view_control.convert_from_pinhole_camera_parameters(camera_params)
       vis.poll_events()
       vis.update_renderer()

       d = -np.dot(normal, center)
       print(f"\n現在の平面: {current_plane_idx + 1}")
       print(f"  中心座標: ({center[0]:.3f}, {center[1]:.3f}, {center[2]:.3f})")
       print(f"  平面方程式: {normal[0]:.3f}x + {normal[1]:.3f}y + {normal[2]:.3f}z + {d:.3f} = 0")
       print(f"  法線ベクトル: [{normal[0]:.3f}, {normal[1]:.3f}, {normal[2]:.3f}]")

   vis.create_window()
   vis.register_key_callback(ord("D"), next_plane)
   vis.register_key_callback(ord("A"), prev_plane)
   vis.add_geometry(pcd)

   print("\n操作方法:")
   print("A: 前の平面")
   print("D: 次の平面")

   update_view()
   vis.run()
   vis.destroy_window()

file_path = r"C:\projects\112_open3d\data\test\bed_0516.off"
mesh = o3d.io.read_triangle_mesh(file_path)
pcd = mesh.sample_points_uniformly(number_of_points=100000)
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

planes = pcd.detect_planar_patches(
   normal_variance_threshold_deg=60,
   coplanarity_deg=75,
   outlier_ratio=0.75,
   min_plane_edge_length=0,
   min_num_points=0,
   search_param=o3d.geometry.KDTreeSearchParamKNN(knn=30)
)

print(f"検出された平面の数: {len(planes)}")
visualize_planes_interactive(pcd, planes)