visualize.py

import math
import cv2
import numpy as np
import yaml
import colorsys
import argparse

def parse_args():
    parser = argparse.ArgumentParser()
    parser.add_argument("--scene_points", type=str)
    args = parser.parse_args()
    return args

def save_ply(filename, vertices, colors):

    assert vertices.shape[0] == colors.shape[0], "顶点数和颜色数必须一致"
    num_vertices = vertices.shape[0]
    
    with open(filename, 'w') as ply_file:
        # 写入 header
        ply_file.write("ply\n")
        ply_file.write("format ascii 1.0\n")
        ply_file.write("element vertex {}\n".format(num_vertices))
        ply_file.write("property float x\n")
        ply_file.write("property float y\n")
        ply_file.write("property float z\n")
        ply_file.write("property uchar red\n")
        ply_file.write("property uchar green\n")
        ply_file.write("property uchar blue\n")
        ply_file.write("end_header\n")
        
        # 写入每个点的数据
        for point, color in zip(vertices, colors):
            line = "{:.6f} {:.6f} {:.6f} {} {} {}\n".format(
                point[0], point[1], point[2],
                int(color[0]), int(color[1]), int(color[2])
            )
            ply_file.write(line)

args = parse_args()
points = np.load(args.scene_points)
points[:,0]=-points[:,0]
save_ply("visualization.ply", points[:,:3], points[:,3:6])