c++point-cloud-libraryoctree

Vector subscript out of range, PCL octree


I'm trying to get the nearest neighbours of a point cloud, but for some reason I'm getting a vector subscript out of range error. When I tried using the debugger to figure out how it went out of range, all the values seem normal.

Screenshot of debugger

This is the code I'm using (which I took from 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;
    }
}

Solution

  • Changing it from

    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;
        }
    }
    

    to

    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;
    

    seems to have fixed the vector going out of range.