基于 PCL 库遍历点云并标记边，提取满足条件的点到新点云 C++ 代码

本文提供基于 PCL 库的 C++ 代码，用于遍历点云数据，根据边的源点和目标点信息，提取满足条件的点并存储到新点云中。

代码实现

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>

struct Edge {
    int src;
    int tgt;
    float weight;
    bool isRead;
};

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr yr(new pcl::PointCloud<pcl::PointXYZ>);

    std::vector<Edge> edges;
    // Populate the edges vector with your data

    // Iterate over the result point cloud
    for (size_t i = 0; i < result->size(); ++i) {
        pcl::PointXYZ point = result->at(i);

        // Check if the point is equal to src or tgt in any edge
        for (size_t j = 0; j < edges.size(); ++j) {
            if (point.x == edges[j].src || point.x == edges[j].tgt) {
                // Add the point to yr
                yr->push_back(point);

                // Mark the edge as read
                edges[j].isRead = true;
                break; // Exit the loop since the point is found
            }
        }
    }

    // Re-iterate over the result point cloud
    for (size_t i = 0; i < result->size(); ++i) {
        pcl::PointXYZ point = result->at(i);

        // Check if the point is equal to src or tgt in any edge
        for (size_t j = 0; j < edges.size(); ++j) {
            if ((point.x == edges[j].src || point.x == edges[j].tgt) && !edges[j].isRead) {
                // Add the point to yr
                yr->push_back(point);

                // Mark the edge as read
                edges[j].isRead = true;
                break; // Exit the loop since the point is found
            }
        }
    }

    // Visualize yr point cloud
    pcl::visualization::CloudViewer viewer("YR Point Cloud Viewer");
    viewer.showCloud(yr);
    while (!viewer.wasStopped()) {}

    return 0;
}

代码说明

1. 结构体定义: 定义 Edge 结构体，包含源点 (src)、目标点 (tgt)、权重 (weight) 和是否已读 (isRead) 信息。
2. 点云初始化: 创建两个点云指针 result 和 yr，分别用于存储原始点云数据和提取的点云数据。
3. 边数据填充: 使用 edges 容器存储边的信息，需要根据实际情况填充数据。
4. 遍历点云: 循环遍历 result 点云，对于每个点，再次遍历 edges 容器，判断点是否为某条边的源点或目标点，如果是，则将该点添加到 yr 点云中，并将该边标记为已读。
5. 二次遍历: 再次遍历 result 点云，目的是为了将之前未读的边对应的点也添加到 yr 点云中。
6. 可视化: 使用 CloudViewer 可视化 yr 点云。

注意事项

• 代码示例中的 point.x 用于判断点是否为源点或目标点，需要根据实际情况调整判断条件。
• 可以根据需求调整数据类型和变量名称。
• 记得填充 edges 容器中的边数据。

使用本代码示例时，请根据您的具体需求进行修改和调整。