import open3d as o3d
from os.path import join

# pcd = o3d.io.read_point_cloud(join("C:\\", "Users", "bruno", "data", "from_realsense", "test1", "realsense", "scene", "integrated.ply" ))
pcd = o3d.io.read_point_cloud(join("C:\\", "Users", "bruno", "data", "from_realsense", "test1", "realsense", "fragments", "fragment_000.ply" ))

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