Question

我试图获得点云的最近邻居，但由于某种原因，我得到了一个超出范围错误的向量下标。当我尝试使用调试器来确定它是如何超出范围时，所有值都显得正常。

这是我使用的代码（我从the official documentation获取的代码）

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(cloud0);
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(128.0f);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();

pcl::PointXYZ searchPoint;
searchPoint.x = 0;
searchPoint.y = 0;
searchPoint.z = 0;

std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;

float radius = 50;

std::cout << "Neighbors within radius search at (" << searchPoint.x
    << " " << searchPoint.y
    << " " << searchPoint.z
    << ") with radius=" << radius << std::endl;


if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
    for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
        float x = cloud->points[pointIdxRadiusSearch[i]].x;
        float y = cloud->points[pointIdxRadiusSearch[i]].y;
        float z = cloud->points[pointIdxRadiusSearch[i]].z;
        std::cout << "    " << x
            << " " << y
            << " " << z
            << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }
}

Answer 1

从

更改

if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
    for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
        float x = cloud->points[pointIdxRadiusSearch[i]].x;
        float y = cloud->points[pointIdxRadiusSearch[i]].y;
        float z = cloud->points[pointIdxRadiusSearch[i]].z;
        std::cout << "    " << x
            << " " << y
            << " " << z
            << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }
}

到

float min_distance = 10000000000000.0f;
pcl::PointXYZ nearestNeighbour;
if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
    for (size_t i = 0; i < pointRadiusSquaredDistance.size(); ++i) {
        if (pointRadiusSquaredDistance[i] < min_distance) {
            min_distance = pointRadiusSquaredDistance[i]; 
            float x = cloud->points[pointIdxRadiusSearch[i]].x;
            float y = cloud->points[pointIdxRadiusSearch[i]].y;
            float z = cloud->points[pointIdxRadiusSearch[i]].z;
            nearestNeighbour.x = x;
            nearestNeighbour.y = y;
            nearestNeighbour.z = z;
        }
    }
}
std::cout << "    " << nearestNeighbour.x << " " << nearestNeighbour.y << " " << nearestNeighbour.z << 
    " (squared distance: " << min_distance << ")" << std::endl;

似乎已经修复了超出范围的向量。

矢量下标超出范围，PCL八叉树

1 个答案: