基于181pcl库将点p的坐标求p的索引的c++代码

以下是基于pcl库1.8.1版本的C++代码，用于求点p的索引：

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

int main()
{
    // 创建点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->width = 5;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        cloud->points[i].x = static_cast<float>(i);
        cloud->points[i].y = static_cast<float>(i);
        cloud->points[i].z = static_cast<float>(i);
    }

    // 创建KD树对象
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud);

    // 设置查询点
    pcl::PointXYZ searchPoint;
    searchPoint.x = 2.0;
    searchPoint.y = 2.0;
    searchPoint.z = 2.0;

    // 最近邻搜索
    int K = 1;
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);
    if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
        std::cout << "Nearest neighbor search at (" << searchPoint.x
                  << " " << searchPoint.y
                  << " " << searchPoint.z
                  << ") with K=" << K << " : " << std::endl;
        for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
            std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x
                      << " " << cloud->points[pointIdxNKNSearch[i]].y
                      << " " << cloud->points[pointIdxNKNSearch[i]].z
                      << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    }

    return 0;
}

在这个代码中，我们首先创建了一个具有5个点的点云数据。然后，我们创建了一个KD树对象，并将点云数据设置为其输入。接下来，我们设置要查询的点searchPoint的坐标。然后，通过调用nearestKSearch函数，我们可以找到最近邻点的索引和平方距离。在这个例子中，我们设置K=1，表示只找到最近的一个点。

最后，我们通过循环打印出最近邻点的坐标和平方距离。

请注意，这只是一个简单的示例代码，用于说明如何使用pcl库进行最近邻搜索。实际使用时，你可能需要根据自己的需求进行修改和扩展。