import open3d as o3d
import numpy as np


def pcd_to_binary(pcd_file, output_file):
    pcd = o3d.io.read_point_cloud(pcd_file)
    points = np.asarray(pcd.points)
    binary_data = points[:, :3]
    with open(output_file, 'wb') as f:
        f.write(binary_data.tobytes())


# def binary_to_nparray(binary_file, output_file):
#     with open(binary_file, 'rb') as f:
#         binary_data = f.read()
#     # 将二进制数据转换为 NumPy 数组
#     point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
#     np.savetxt(output_file, point_cloud)


if __name__ == '__main__':
    pcd_to_binary(r'/home/robot/ROS/ren_yuan/ren_yuan_pointcloud/123.pcd', r'/home/robot/ROS/ren_yuan/ren_yuan_pointcloud/pointcloud.txt')
    #binary_to_nparray(r'./pointcloud.txt', r'./111123123123.txt')