import open3d as o3d
import numpy as np

def load_txt_point_cloud(filename):
    # Load the data from the ASCII file
    data = np.loadtxt(filename)
    points = data[:, 0:3]               # X, Y, Z
    colors = data[:, 3:6] / 255.0       # R, G, B (normalize to [0,1])

    # Create Open3D point cloud
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd.colors = o3d.utility.Vector3dVector(colors)

    return pcd

# Path to your .txt file
txt_file_path = "/home/yoga/3D_Scanner/Jost/SubseaScanner_Convert_Disp_2_Pcl/output/Pointcloud_0000.txt"

# Load and visualize
pcd = load_txt_point_cloud(txt_file_path)
o3d.visualization.draw_geometries([pcd])