用cpp调用pcl库写一个将xyz无序点云转变为有序点云,根据输入像素坐标切割生成新的点云的程序

下面是一个简单的示例代码，用于将无序点云转换为有序点云，并根据输入像素坐标切割生成新的点云。该示例使用了 PCL 库中的 VoxelGrid 过滤器来对点云进行下采样，以减少计算量。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;

int main(int argc, char** argv) {

    // 读取点云数据
    PointCloud::Ptr cloud(new PointCloud);
    if (pcl::io::loadPCDFile<PointT>("input_cloud.pcd", *cloud) == -1) {
        std::cerr << "Could not read input cloud file." << std::endl;
        return -1;
    }

    // 下采样点云
    pcl::VoxelGrid<PointT> vg;
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);
    PointCloud::Ptr cloud_filtered(new PointCloud);
    vg.filter(*cloud_filtered);

    // 计算法线
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    ne.setInputCloud(cloud_filtered);
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
    ne.setSearchMethod(tree);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    ne.setRadiusSearch(0.03);
    ne.compute(*cloud_normals);

    // 合并点云和法线
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields(*cloud_filtered, *cloud_normals, *cloud_with_normals);

    // 创建搜索树
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
    tree2->setInputCloud(cloud_with_normals);

    // 定义并计算三角化对象
    pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
    pcl::PolygonMesh triangles;
    gp3.setSearchRadius(0.025);
    gp3.setMu(2.5);
    gp3.setMaximumNearestNeighbors(100);
    gp3.setMaximumSurfaceAngle(M_PI / 4);
    gp3.setMinimumAngle(M_PI / 18);
    gp3.setMaximumAngle(2 * M_PI / 3);
    gp3.setNormalConsistency(false);
    gp3.setInputCloud(cloud_with_normals);
    gp3.setSearchMethod(tree2);
    gp3.reconstruct(triangles);

    // 可视化结果
    pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");
    viewer.setBackgroundColor(0, 0, 0);
    viewer.addPolygonMesh(triangles, "triangles");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "triangles");
    viewer.addCoordinateSystem(0.1);
    viewer.initCameraParameters();

    // 根据输入像素坐标切割生成新的点云
    int pixel_x = 100;
    int pixel_y = 200;
    float fx = 525.0f; // 相机内参 fx
    float fy = 525.0f; // 相机内参 fy
    float cx = 319.5f; // 相机内参 cx
    float cy = 239.5f; // 相机内参 cy
    float depth = 1.0f; // 输入深度值
    float x = (pixel_x - cx) * depth / fx;
    float y = (pixel_y - cy) * depth / fy;
    float z = depth;
    pcl::PointXYZRGB new_point;
    new_point.x = x;
    new_point.y = y;
    new_point.z = z;
    new_point.r = 255;
    new_point.g = 255;
    new_point.b = 255;
    PointCloud::Ptr new_cloud(new PointCloud);
    new_cloud->push_back(new_point);

    // 显示新的点云
    pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(new_cloud);
    viewer.addPointCloud<PointT>(new_cloud, rgb, "new_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "new_cloud");
    viewer.spin();

    return 0;
}

在上面的代码中，我们首先读取了一个无序点云文件（`input_cloud.pcd`），然后使用 VoxelGrid 过滤器对点云进行下采样，计算法线，并将点云和法线合并到一个新的点云中。接下来，我们使用 GreedyProjectionTriangulation 对象计算三角化，并可视化结果。最后，我们根据给定的输入像素坐标生成一个新的点云，并将其添加到可视化窗口中。