PCL手动生成点云PointCloud并体素过滤VoxelFilter

实现生成三维点云并进行变换(RT)

然后通过体素滤波

最后通过open3d显示

#! /usr/bin/env python3
# -*-coding:utf-8-*-
#for reading pcd and json from task3 result files
import pcl
import open3d as o3d

import numpy as np
from pyquaternion import Quaternion
import pandas
from pathlib import Path

from math import radians, tanh
import os


def gen_3d_points(l,w,h,x,y,z,yaw):
    #delta dist 0.05
    step1=0.1/l
    step2=0.1/w
    alpha, beta = np.mgrid[0:2*np.pi:step1, 0:2*np.pi:step2]
    x1 = np.cos(alpha)*np.cos(beta)*l/2
    y1 = np.cos(alpha)*np.sin(beta)*w/2
    z1 = np.sin(alpha)*h/2
    x1=x1.flatten()
    y1=y1.flatten()
    z1=z1.flatten()
    ones=np.ones(x1.shape)
    ori=np.vstack((x1,y1,z1,ones))
    rot=Quaternion(axis=[0,0,1],radians=yaw).rotation_matrix
    rt=np.matrix(np.eye(4,4))
    rt[:3,:3]=rot
    rt[:3,3]=np.array([x,y,z]).reshape(3,1)
    res=np.matmul(rt,ori)
    r