C++ PCL 沿着自定义的平面做横截面(直通滤波)

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/pass_through.h>
#include <pcl/common/transforms.h>
#include <pcl/common/centroid.h>

// 根据自定义平面过滤点云的函数
void filterPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, 
                      const Eigen::Vector3f& plane_normal, 
                      const Eigen::Vector3f& point_on_plane, 
                      pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud)
{
    // 归一化平面法向量
    Eigen::Vector3f normalized_normal = plane_normal.normalized();

    // 计算将平面移动到原点的平移向量
    Eigen::Vector3f translation = -point_on_plane;

    // 创建旋转矩阵，使平面法向量与Z轴对齐
    Eigen::Vector3f z_axis(0, 0, 1);
    Eigen::Quaternionf rotation = Eigen::Quaternionf::FromTwoVectors(normalized_normal, z_axis);

    // 变换点云，使平面与XY平面对齐
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() = translation;
    transform.rotate(rotation);

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::transformPointCloud(*cloud, *transformed_cloud, transform);

    // 使用直通滤波器沿Z轴进行过滤
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(transformed_cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(-0.01, 0.01);  // 根据需要调整过滤范围
    pass.filter(*filtered_cloud);

    // 将过滤后的点云变换回原始坐标系
    pcl::transformPointCloud(*filtered_cloud, *filtered_cloud, transform.inverse());
}

int main(int argc, char** argv)
{
    // 加载点云数据到 'cloud'
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    // 加载点云数据 ...

    // 定义平面的法向量
    Eigen::Vector3f plane_normal(1.0, 0.0, 0.0);  // 示例法向量
    Eigen::Vector3f point_on_plane(0.0, 0.0, 0.0);  // 示例平面上的点

    // 过滤后的点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>());

    // 根据自定义平面过滤点云
    filterPointCloud(cloud, plane_normal, point_on_plane, filtered_cloud);

    // 对过滤后的点云进行处理...

    return 0;
}