1

点群の最近傍を取得しようとしていますが、何らかの理由で範囲外のベクトル添字を取得しています。デバッガーを使用して範囲外になった原因を調べてみたところ、すべての値が正常に見えます。

デバッガーのスクリーンショット

これは私が使用しているコードです(公式ドキュメントから取得しました)

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

1 に答える 1