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