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

下面是一个简单的示例程序，使用PCL库将无序点云转换为有序点云，并根据输入的像素坐标生成新的点云：

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>

int main(int argc, char** argv)
{
    // 读取点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("input.pcd", *cloud);

    // 计算法线
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(cloud);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    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, *cloud_normals, *cloud_with_normals);

    // 根据像素坐标生成新的点云
    int x = 100;  // 像素坐标 x
    int y = 200;  // 像素坐标 y
    float fx = 525.0;  // 相机内参 fx
    float fy = 525.0;  // 相机内参 fy
    float cx = 319.5;  // 相机内参 cx
    float cy = 239.5;  // 相机内参 cy
    float depth = 1.0;  // 深度信息
    pcl::PointNormal point;
    point.x = (x - cx) * depth / fx;
    point.y = (y - cy) * depth / fy;
    point.z = depth;
    pcl::PointCloud<pcl::PointNormal>::Ptr new_cloud(new pcl::PointCloud<pcl::PointNormal>);
    pcl::KdTreeFLANN<pcl::PointNormal>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointNormal>);
    kdtree->setInputCloud(cloud_with_normals);
    std::vector<int> point_idx;
    std::vector<float> point_dist;
    if (kdtree->nearestKSearch(point, 1, point_idx, point_dist) > 0)
    {
        new_cloud->push_back(cloud_with_normals->points[point_idx[0]]);
    }

    // 保存新的点云
    pcl::io::savePCDFile("output.pcd", *new_cloud);

    return 0;
}

在上面的示例程序中，我们首先读取了一个无序点云数据，然后使用PCL库计算了点云的法线，并将点云和法线拼接成一个有序点云。接着，我们根据输入的像素坐标计算出相应的三维点，并使用Kd树查找最近的点，最后将找到的点保存为新的点云数据。请注意，上面的示例程序中使用的相机内参和深度信息是固定的，如果需要根据实际情况进行计算，请替换为实际的参数值。