“pcl::KdTreeFLANN” очень медленный

Вопрос или проблема

    void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg)
{
    // Конвертировать сообщение ROS в тип данных PCL
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::fromROSMsg(*cloud_msg, *cloud);

    // KDTree для поиска соседей
    pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
    kdtree.setInputCloud(cloud);

    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;

    // Перебирать все точки в облаке
    for (const auto& point : cloud->points)
    {
        // Выполнить поиск радиуса для нахождения соседей в заданном радиусе
        int neighbors_count = kdtree.radiusSearch(point, radius_, point_idx_radius_search, point_radius_squared_distance);

        // Применить условие фильтрации
        if (!(point.intensity < intensity_threshold_ && neighbors_count < min_neighbours_))
        {
            // Сохранять точки, которые не соответствуют условию удаления
            cloud_filtered->points.push_back(point);
        }
    }

Ответ или решение

Проблемы с производительностью библиотеки pcl::KdTreeFLANN при фильтрации облака точек LiDAR, как описано в вашем вопросе, могут быть вызваны несколькими факторами. Ваша цель — фильтровать облако, учитывая интенсивность точек и количество соседей в заданном радиусе без произвольного уменьшения данных. Однако, при использовании KdTree для поиска соседей вы столкнулись с замедлением.

Вот несколько рекомендаций и альтернативных методов, которые могут помочь ускорить этот процесс:

1. Оптимизация параметров KdTree

a. Минимизация радиуса

Попробуйте уменьшить радиус поиска. Большие радиусы приводят к увеличению вычислительной нагрузки, поскольку KdTree обрабатывает большее количество точек. Экспериментируйте с различными радиусами, чтобы найти наиболее оптимальное значение.

b. Использование radiusSearch на меньшем количестве точек

Вы можете сделать предварительный фильтр, отбирая точки, которые явно не соответствуют критериям интенсивности. Это позволит уменьшить количество точек, которые нужно обрабатывать в KdTree.

2. Параллелизация обработки

Используйте возможности параллелизации для выполнения операций поиска соседей. Поскольку обработка каждой точки независима, вы можете использовать библиотеки многопоточности (например, OpenMP или TBB) для распараллеливания блока кода, который выполняет поиск соседей.

3. Предварительная фильтрация облака точек

Перед тем как применять KdTree, выполните предварительную фильтрацию облака точек. Например, вы можете удалить все точки, имеющие интенсивность ниже заданного порога, без использования KdTree, что сократит количество точек, обрабатываемых при поиске соседей.

// Предварительное удаление точек по интенсивности
for (const auto& point : cloud->points)
{
    if (point.intensity >= intensity_threshold_)
    {
        cloud_filtered->points.push_back(point);
    }
}

4. Использование других методов поиска

Если KdTree продолжает показывать плохую производительность, рассмотрите возможность использования других структур данных для поиска соседей. Например, Octree может быть более эффективным для ваших целей, особенно в случае разреженных облаков.

5. Динамическое изменение радиуса поиска

Если это возможно, подумайте о том, чтобы динамически изменять радиус поиска в зависимости от расстояния от точки. Это может помочь избежать чрезмерной сложности поиска на меньших расстояниях, сохраняя при этом точные результаты на больших.

6. Состояние ROS и Параметров

Убедитесь, что ваши параметры ROS настроены для обработки входящих сообщений эффективно. Например, посмотрите на частоту подписок и размер сообщений, чтобы не допустить излишней нагрузки на систему.

Заключение

Фильтрация облака точек требует аккуратного подхода к обработке данных, особенно в реальном времени. Параллельная обработка, предварительная фильтрация, оптимизация параметров KdTree и возможный переход на альтернативные методы поиска соседей — это ключевые факторы для повышения производительности фильтрации. Попробуйте применить предложенные методы и посмотрите на улучшение производительности. Если у вас остались вопросы или необходима более глубокая консультация, не стесняйтесь обращаться.

Удачи в ваших разработках!

С уважением,
[Ваше имя]