python 读写pcd

1. 读点云的3种方式

#第一种
# pip3 install python-pcl
import pcl 
#pcd_ndarray = pcl.load(args.pcd_path).to_array()[:, :3] #不要intensity
pcd_ndarray = pcl.load_XYZI(args.pcd_path).to_array()[:, :4]
point_num = pcd_ndarray.shape[0] 
# shape属性可以获取矩阵的形状（例如二维数组的行列），获取的结果是一个元组

#第二种
def read_pcd(pcd_path):
    lines = []
    num_points = None

    with open(pcd_path, 'r') as f:
        for line in f:
            lines.append(line.strip())
            if line.startswith('POINTS'):
                num_points = int(line.split()[-1])
    assert num_points is not None

    points = []
    for line in lines[-num_points:]:
        x, y, z, i = list(map(float, line.split()))
        #这里没有把i放进去，也是为了后面 x, y, z 做矩阵变换的时候方面
        #但这个理解我选择保留， 因为可能把i放进去也不影响代码的简易程度
        points.append((np.array([x, y, z, 1.0]), i))

    return points

#第三种
def load_pcd_to_ndarray(pcd_path):
    with open(pcd_path) as f:
        while True:
            ln = f.readline().strip()
            if ln.startswith('DATA'):
                break

        points = np.loadtxt(f)
        points = points[:, 0:4]
        return points 

#第四种（需要源码安装pypcd）链接在底部
import argparse
from pypcd import pypcd
import numpy as np
parser = argparse.ArgumentParser()
parser.add_argument('--pcd_path', default='', type=str)
args = parser.parse_args()

def read_pcd(pcd_path):
    pcd = pypcd.PointCloud.from_path(pcd_path)
    pcd_np_points = np.zeros((pcd.points, 5), dtype=np.float32)
    print(pcd.pc_data['x'])
    pcd_np_points[:, 0] = np.transpose(pcd.pc_data['x'])
    pcd_np_points[:, 1] = np.transpose(pcd.pc_data['y'])
    pcd_np_points[:, 2] = np.transpose(pcd.pc_data['z'])
    pcd_np_points[:, 3] = np.transpose(pcd.pc_data['intensity'])
    pcd_np_points[:, 4] = np.transpose(pcd.pc_data['is_ground'])
    
    del_index = np.where(np.isnan(pcd_np_points))[0]
    pcd_np_points = np.delete(pcd_np_points, del_index, axis=0)
    return pcd_np_points

read_pcd(args.pcd_path)

在python3中使用pypcd读取点云数据

2. 写点云的两种方式

HEADER = '''\
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 4 
TYPE F F F F 
COUNT 1 1 1 1 
WIDTH {}
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS {}
DATA ascii
'''

def write_pcd(points, save_pcd_path):
    n = len(points)
    lines = []
    for i in range(n):
        x, y, z, i, is_g = points[i]
        lines.append('{:.6f} {:.6f} {:.6f} {}'.format( \
            x, y, z, i))
    with open(save_pcd_path, 'w') as f:
        f.write(HEADER.format(n, n))
        f.write('\n'.join(lines))

def write_pcd(points, save_pcd_path):
    with open(save_pcd_path, 'w') as f:
        f.write(HEADER.format(len(points), len(points)) + '\n')
        np.savetxt(f, points, delimiter = ' ', fmt = '%f %f %f %d')