python 去除点云中的nan点，transform点云

去除点云中的nan点

import yaml
import time
import pcl
from tqdm import tqdm

def filter_nan(pcd_path):
    cloud = pcl.load_XYZI(pcd_path)
    pt = cloud.to_array()[:, :4]

    #### filter NAN points
    t=[]
    for n in range(0, len(pt)):
        x = pt[n][0]
        y = pt[n][1]
        z = pt[n][2]
        if (np.isnan(x) or np.isnan(y) or np.isnan(z)):
            continue
        t.append(n)
    pt = pt[t]

    cloud.from_array(pt)
    pcl.save(cloud, pcd_path)

with open("pcd_path_list.txt") as f:
    pcd_list = f.read().splitlines()
    for i, pcd_path in enumerate(tqdm(pcd_list)):
        filter_nan(pcd_path)

transform点云

# -*- coding: UTF-8 -*-
import numpy as np
import os
import argparse
import sys
import yaml
import time
import pcl
from tqdm import tqdm
from scipy.spatial.transform import Rotation as R

transform_matrix = np.zeros((4, 4), dtype=np.float32)

def merge_tr(translation, rotation):
    transform_matrix[0:3, 0:3] = rotation
    transform_matrix[0:3, 3] = np.array([translation])
    transform_matrix[3, 0:4] = np.array([0, 0, 0, 1])

def transform(pcd_path):
    # 这个办法会把好像intensity以及后面的去掉
    cloud = pcl.load_XYZI(pcd_path)
    pt = cloud.to_array()[:, :3]
    intensity = cloud.to_array()[:, 3].reshape(-1, 1)
    
    pad_pt = np.ones((pt.shape[0], 1), dtype=pt.dtype)
    pt = np.concatenate((pt, pad_pt), axis=1)
    trans_pt = np.dot(transform_matrix, pt.T).T
    #trans_pt = np.dot(np.linalg.inv(transform_matrix), pt.T).T

    trnas_pt = np.concatenate((trans_pt[:, 0:3], intensity), axis=1)
    trans_cloud = pcl.PointCloud_PointXYZI()
    trans_cloud.from_array(trans_pt)
    pcl.save(trans_cloud, pcd_path)

def get_transform_matrix(yaml_file):
    with open(yaml_file) as f:
        data = yaml.load(f, Loader=yaml.FullLoader)
        try:
            translation = data['transform']['translation']
            rotation = data['transform']['rotation']
        except:
            translation = data['translation']
            rotation = data['rotation']
        r = R.from_quat([rotation['x'], rotation['y'], rotation['z'], rotation['w']])
        rotation_matrix = r.as_matrix()
        ##### get init_transform_matrix(3*4)
        merge_tr([translation['x'], translation['y'], translation['z']] , \
                rotation_matrix)

if __name__ == '__main__':
    get_transform_matrix("D_2_A.yaml")
    with open("pcd_path_list.txt") as f:
        pcd_list = f.read().splitlines()
        for i, pcd_path in enumerate(tqdm(pcd_list)):
            transform(pcd_path)