“pcl::KdTreeFLANN”非常慢

Question

我在 Ubuntu 22.04 上使用 ROS2-Iron 根据以下标准从 rosbag 过滤 LiDAR 点云中的点：(1) 强度值低于特定阈值的点，以及 (2) 邻居数少于某个阈值的点指定的最小数量。为了计算给定半径内的邻居，我使用 pcl::KdTreeFLANN 库。然而，在 RViz2 中可视化结果时，性能极其缓慢。如果我增加 KdTreeFLANN 的半径或最小邻居计数，点云过滤器处理将变得无响应。我想避免对点云进行下采样。是否有其他方法可以加快此过滤过程？

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <memory>
#include <string>
#include <vector>
#include <chrono>
#include <functional>
using namespace std::chrono_literals;


class KdTreeFilterNode : public rclcpp::Node
{
public:
    KdTreeFilterNode() : Node("kdtree_filter_node")
    {
        this->declare_parameter<std::string>("kdtree/inputTopic", "/ouster_front/points");
        this->declare_parameter<double>("kdtree/radius_search",50.0);//mm 
        this->declare_parameter<int>("kdtree/minNeighbours", 3);
        this->declare_parameter<double>("intensity_threshold", 100.0);

        this->get_parameter("kdtree/inputTopic", input_topic_);
        this->get_parameter("kdtree/radius_search", radius_);
        this->get_parameter("kdtree/minNeighbours", min_neighbours_);
        this->get_parameter("intensity_threshold", intensity_threshold_);

        RCLCPP_INFO(this->get_logger(), "The input topic is: %s", input_topic_.c_str());
        RCLCPP_INFO(this->get_logger(), "Radius search is set to: %.2f", radius_);
        RCLCPP_INFO(this->get_logger(), "Minimum neighbors required: %d", min_neighbours_);
        RCLCPP_INFO(this->get_logger(), "Intensity threshold: %.2f", intensity_threshold_);

        sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            input_topic_, rclcpp::SensorDataQoS(),
            std::bind(&KdTreeFilterNode::cloud_cb, this, std::placeholders::_1));

        pub_output_points_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("kdtree/output", 10);
    }

private:
    void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg)
    {
        // Convert ROS msg to PCL data type
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>());
        pcl::fromROSMsg(*cloud_msg, *cloud);

        // KDTree for neighbor search
        pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
        kdtree.setInputCloud(cloud);
        //kdtree.setEpsilon(0.1);  // Set the epsilon for approximate nearest neighbor search


        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>());
        std::vector<int> point_idx_radius_search;
        std::vector<float> point_radius_squared_distance;

        // Iterate over all points in the cloud
        for (const auto& point : cloud->points)
        {
            // Perform radius search to find neighbors within the specified radius
            int neighbors_count = kdtree.radiusSearch(point, radius_, point_idx_radius_search, point_radius_squared_distance);

            // Apply the filter condition
            if (!(point.intensity <= intensity_threshold_ && neighbors_count < min_neighbours_))
            {
                // Keep points that do not meet the removal condition
                cloud_filtered->points.push_back(point);
            }
        }

        cloud_filtered->width = cloud_filtered->points.size();
        cloud_filtered->height = 1;
        cloud_filtered->is_dense = true;

        // Convert the filtered PCL data back to ROS message
        sensor_msgs::msg::PointCloud2 output;
        pcl::toROSMsg(*cloud_filtered, output);
        output.header = cloud_msg->header;

        // Publish the filtered point cloud
        pub_output_points_->publish(output);
    }

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_output_points_;
    std::string input_topic_;
    double radius_;
    int min_neighbours_;
    double intensity_threshold_;
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<KdTreeFilterNode>());
    rclcpp::shutdown();
    return 0;
}

我感谢您提前提供的任何帮助。

谢谢， 阿巴斯

Answer 1

在您的评论中，您说节点应该能够每秒处理 2 条消息，因此每条消息需要 500 毫秒。这是否可行取决于您的计算机和点数，但以下一些提示可能会帮助您实现这一目标：

尽可能避免邻里搜索

从上面的代码中，如果满足以下条件，您希望保留积分：

!(point.intensity <= intensity_threshold_ && neighbors_count < min_neighbours_)

。这与

point.intensity > intensity_threshold_ || neighbors_count >= min_neighbours_

相同。这意味着您（有时）可以避免昂贵的邻里搜索，如下所示：

if (point.intensity > intensity_threshold_) {
  // keep point: add to cloud_filtered
}
else {
  int neighbors_count = kdtree.radiusSearch(point, radius_, point_idx_radius_search, point_radius_squared_distance);
  if (neighbors_count >= min_neighbours_) {
    // keep point: add to cloud_filtered
  }
}

让邻居搜索更快

你不需要知道每个点的确切个邻居，你只需要知道是否至少有

min_neighbours_

个邻居。通过向 radiusSearch 添加附加参数，您可以告诉它在找到足够的邻居时停止搜索：

kdtree.radiusSearch(point, radius_, point_idx_radius_search, point_radius_squared_distance, min_neighbours_);

另请参阅 https://pointclouds.org/documentation/classpcl_1_1_kd_tree_f_l_a_n_n.html#a9b2485acd008ac09559a95820592782e

有组织的邻居

我已经在评论中提到过这一点，但想在这里再次提及：PCL 有一个名为 OrganizedNeighbor 的搜索类，它比 KdTreeFLANN 快得多。它仅适用于特定的点云（有组织且可投影）。不确定您的数据是否适合此算法，但如果您检查点云的宽度和高度并且两者都大于 1，那么 OrganizedNeighbor 很有可能会起作用。

并行化

使用 OpenMP 并行化代码并不是很困难，并且可以大大加快速度。这是伪代码中的一个想法：

std::vector<std::uint8_t> keep_point(cloud->size(), 0);
// Iterate of all points in parallel
#pragma omp parallel for num_threads(4) schedule(dynamic, 32) shared(keep_point, kdtree)
for(int i=0; i<cloud->size(); ++i) {
  // check if point should be kept, if yes, write keep_point[i]=1;
}
// Iterate again, but not parallel
for(int i=0; i<cloud->size(); ++i) {
  if(keep_point[i]) {
    cloud_filtered->points.push_back(point);
  }
}

使用

keep_point

向量的解决方案避免了并行循环中的互斥锁（如果在并行循环中完成，向

cloud_filtered

添加点必须受到互斥锁的保护）。