C++ PCL 将一个点云投影到一个由法向量和点确定的平面

定义平面：使用法向量和平面上的一个点来定义平面方程。
计算投影矩阵：根据平面方程计算点到平面的投影矩阵。
应用投影矩阵：将点云中的每个点投影到平面上。

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/common/eigen.h>

// 计算点到平面的投影矩阵
Eigen::Matrix4f computeProjectionMatrix(const Eigen::Vector3f& point, const Eigen::Vector3f& normal) {
    Eigen::Matrix4f projection_matrix = Eigen::Matrix4f::Identity();
    
    Eigen::Vector3f normalized_normal = normal.normalized();
    float d = -normalized_normal.dot(point);
    
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            projection_matrix(i, j) -= normalized_normal[i] * normalized_normal[j];
        }
        projection_matrix(i, 3) = -normalized_normal[i] * d;
    }
    
    return projection_matrix;
}

int main(int argc, char** argv) {
    // 加载点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("input.pcd", *cloud) == -1) {
        PCL_ERROR("Couldn't read file input.pcd\n");
        return -1;
    }

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

    // 计算投影矩阵
    Eigen::Matrix4f projection_matrix = computeProjectionMatrix(point_on_plane, plane_normal);

    // 投影点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr projected_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::transformPointCloud(*cloud, *projected_cloud, projection_matrix);

    // 保存投影后的点云
    pcl::io::savePCDFileASCII("projected_cloud.pcd", *projected_cloud);

    return 0;
}