"pcl::KdTreeFLANN" is extremely slow

Question

I am using ROS2-Iron on Ubuntu 22.04 to filter points in a LiDAR point cloud from a rosbag, based on the following criteria: (1) points with intensity values below a certain threshold, and (2) points with fewer neighbors than a specified minimum number. To count the neighbors within a given radius, I am using the pcl::KdTreeFLANN library. However, when visualizing the results in RViz2, the performance is extremely slow. If I increase the radius or the minimum neighbor count for KdTreeFLANN, the point cloud filter processing becomes unresponsive. I would like to avoid downsampling the point cloud. Are there alternative methods to speed up this filtering process?

Original code:

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

Modified code:

#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 <omp.h> // Include OpenMP for parallel processing
#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", 150.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;

        // Initialize the keep_point vector
        std::vector<std::uint8_t> keep_point(cloud->size(), 0);

        // Parallel loop using OpenMP
        #pragma omp parallel for num_threads(4) schedule(dynamic, 32) shared(keep_point, kdtree)
        for (int i = 0; i < cloud->points.size(); ++i)
        {
            const auto& point = cloud->points[i];

            // Check the intensity condition
            if (point.intensity > intensity_threshold_)
            {
                keep_point[i] = 1; // Mark the point to keep
            }
            else
            {
                // 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, min_neighbours_);
                if (neighbors_count >= min_neighbours_)
                {
                    keep_point[i] = 1; // Mark the point to keep
                }
            }
        }

        // Iterate over keep_point vector and add points to cloud_filtered
        for (int i = 0; i < cloud->points.size(); ++i)
        {
            if (keep_point[i])
            {
                cloud_filtered->points.push_back(cloud->points[i]);
            }
        }


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

I appreciate any help in advance.

Thanks, Abbas

Answer 1

In your comment you say that the node should be able to process 2 messages per second, so 500 milliseconds per message. Whether this is doable or not depends on your computer and the number of points, but here are some tips that might help you achieve that:

Avoid neighbourhood search if possible

From your code above, you want to keep a point if the following condition is met: !(point.intensity <= intensity_threshold_ && neighbors_count < min_neighbours_). This is the same as point.intensity > intensity_threshold_ || neighbors_count >= min_neighbours_. This means that you can (sometimes) avoid the costly neighbourhood search like this:

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

Make neighbourhood search faster

You do not need to know the exact number of neighbours of each point, you only need to know whether there are at least min_neighbours_ neighbours. By adding an additional parameter to radiusSearch, you can tell it to stop searching if enough neighbours are found: kdtree.radiusSearch(point, radius_, point_idx_radius_search, point_radius_squared_distance, min_neighbours_); See also https://pointclouds.org/documentation/classpcl_1_1_kd_tree_f_l_a_n_n.html#a9b2485acd008ac09559a95820592782e

OrganizedNeighbor

I already mentioned this in my comment but wanted to mentioned it here again: PCL has a search class called OrganizedNeighbor which is much faster than the KdTreeFLANN. It only works on specific point clouds (organized and projectable). Not sure if your data is suitable for this algorithm, but if you check width and height of your point clouds and both are larger than one, then there is a good chance that OrganizedNeighbor will work.

Parallelize

Using OpenMP to parallelize your code is not very difficult and can speed it up a lot. Here is an idea in pseudocode:

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

This solution with the keep_point vector avoids mutex locks in the parallel loop (adding points to cloud_filtered would have to be protected by a mutex lock if done in a parallel loop).