Облака точек Orbbec в PCL: фильтры, сегментация плоскости, нормали

#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/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>

class CloudPipeline : public rclcpp::Node {
public:
  CloudPipeline() : Node("cloud_pipeline") {
    sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
        "/camera/depth/points", rclcpp::SensorDataQoS(),
        std::bind(&CloudPipeline::onCloud, this, std::placeholders::_1));
    pub_objects_ = create_publisher<sensor_msgs::msg::PointCloud2>(
        "cloud/objects", 10);
  }

private:
  void onCloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*msg, *cloud);
    if (cloud->empty()) return;

    // 1) прореживание: воксель 1 см
    pcl::VoxelGrid<pcl::PointXYZ> voxel;
    voxel.setInputCloud(cloud);
    voxel.setLeafSize(0.01f, 0.01f, 0.01f);
    voxel.filter(*cloud);

    // 2) рабочая зона по Z (оптический кадр: Z — вперёд от камеры)
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.5f, 4.0f);
    pass.filter(*cloud);

    // 3) главная плоскость через RANSAC
    pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> sac;
    sac.setOptimizeCoefficients(true);
    sac.setModelType(pcl::SACMODEL_PLANE);
    sac.setMethodType(pcl::SAC_RANSAC);
    sac.setDistanceThreshold(0.02);      // 2 см — толщина "плоскости"
    sac.setInputCloud(cloud);
    sac.segment(*inliers, *coeffs);

    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);

    pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
    extract.filter(*plane);              // инлайеры = плоскость

    extract.setNegative(true);
    pcl::PointCloud<pcl::PointXYZ>::Ptr objects(new pcl::PointCloud<pcl::PointXYZ>);
    extract.filter(*objects);            // всё, кроме плоскости

    // 4) нормали для "объектов"
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    ne.setInputCloud(objects);
    ne.setSearchMethod(pcl::search::KdTree<pcl::PointXYZ>::Ptr(
        new pcl::search::KdTree<pcl::PointXYZ>));
    ne.setRadiusSearch(0.05);
    ne.compute(*normals);

    sensor_msgs::msg::PointCloud2 out;
    pcl::toROSMsg(*objects, out);
    out.header = msg->header;
    pub_objects_->publish(out);
    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
                         "objects: %zu points", objects->size());
  }

  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_objects_;
};

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

Шаг 3. Смотрим результат

Соберите пакет, запустите камеру и узел, откройте rviz2 и добавьте PointCloud2 на cloud/objects. Сцена должна распасться: пол и стены ушли в «плоскость» (её легко посмотреть, заменив setNegative(true) на вывод инлайеров), мебель и предметы остались в «объектах». Если пол остался в объектах — увеличьте DistanceThreshold; если всё наоборот и в «плоскость» попала мебель — уменьшайте порог.

Для визуализации в rviz2 включите у дисплея раскраску по интенсивности высоты (Axis = z) — граница «плоскость/объекты» становится видимой мгновенно. Проверьте и частоту узла: если rviz2 обновляется рывками, а ros2 topic hz cloud/objects держит ровную частоту, проблема в графике, а не в обработке — ставьте Style=Points и уменьшайте размер точки.

Шаг 4. От точек к объектам: кластеры

Логичное продолжение конвейера — сгруппировать «объекты» в отдельные связные кластеры и работать с ними как со списком. В PCL это EuclideanClusterExtraction с поиском соседей через KdTree:

#include <pcl/segmentation/extract_clusters.h>
// после получения objects:
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(objects);

std::vector<pcl::PointIndices> clusters;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.05);   // 5 см: точки ближе - один объект
ec.setMinClusterSize(100);      // отсекаем мелкий мусор
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(objects);
ec.extract(clusters);
// clusters[i].indices - индексы точек i-го объекта

Порог ClusterTolerance — та же философия, что и у RANSAC: чуть больше шума, но меньше разрывов внутри объектов. Для сцены со столом 5 см — типичное начало, дальше подбираете по своей геометрии. На выходе у вас уже предметный список: центроиды кластеров отдают в планировщик, а bounding box каждого кластера — в логику захвата.

Разбор кода

Конвейер построен на четырёх стандартных операциях, и порядок здесь важнее самих вызовов. Воксельное прореживание мы делаем первым: оно снижает число точек в разы, а все последующие шаги (RANSAC, нормали) линейно зависят от их количества. Ограничение рабочей зоны — вторым: RANSAC ищет «самую большую плоскость», и без ограничения по дальности ей станет стена за объектом интереса, а не пол под ним. Заметьте, что фильтруем по полю z: в оптическом кадре камеры Z направлена от камеры вперёд, так что «от 0,5 до 4 метров от камеры» — это буквально лимиты по z.

Отдельного разговора заслуживает плотность облака. Облако от драйвера сохраняет пиксельную сетку исходного кадра (организованное облако), и невалидные измерения в нём живут как NaN. Фильтры PCL работают с обоими вариантами, но оценка нормалей и кластеризация ведут себя предсказуемее на «чистом» облаке — поэтому удаление NaN перед тяжёлыми стадиями не паранойя, а гигиена. Проверка cloud->is_dense подскажет, есть ли в облаке невалидные точки; если после удаления облако пустое — сцена не читается вовсе, и это стоит проверять явно, до запуска алгоритмов.

Порог RANSAC в 2 см — не магия, а следствие плотности облака Gemini: в помещении стереокамера даёт разброс в единицы миллиметров вблизи и до сантиметров на дальней границе зоны. Порог должен быть чуть больше шума, иначе плоскость «рассыплется» на куски, и меньше перепада рельефа, иначе в плоскость попадёт бордюр. Нормали считаем радиусом 5 см уже на прореженном облаке: больший радиус сглаживает детали, меньший — усиливает шум; для поиска вогнутостей и выпуклостей на предметах 3–7 см — рабочая зона.

QoS подписки — SensorDataQoS, по тем же причинам, что и в гайде про rviz2: драйвер публикует сенсорный профиль. И заголовок исходного сообщения мы переносим в выходное (out.header = msg->header) — downstream-узлы и rviz2 продолжают видеть корректный фрейм и время.

Про производительность: колбэк узла выполняется в исполнителе ROS 2, и тяжёлая обработка прямо в нём может захлебнуться при 30 Гц. Первая линия обороны — уже описанное раннее прореживание. Вторая — обработка каждого n-го кадра (для многих задач 10 Гц достаточно) или вынос обработки в отдельный поток с очередью кадров на единицу глубины: старый кадр отбрасываем, берём свежий. Если облако приходит чаще, чем успевает обработаться, а очередь копится, — вы деградируете по задержке, и лучше потерять кадры, чем затягивать их обработку.

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

Частые проблемы

RANSAC находит «не ту» плоскость

Решение ищет самую согласованную плоскость во всём облаке — значит, в облаке доминирует не то, что вы хотите. Ограничьте рабочую зону по z и x/y до зоны интереса перед сегментацией (второй PassThrough по полям x и y), либо используйте SACMODEL_PERPENDICULAR_PLANE с осью и углом — «плоскость, примерно перпендикулярная вертикали» — если пол известен по направлению. Несколько плоскостей подряд получают циклом: сегментируем, извлекаем индексы, повторяем на остатке, пока размер инлайеров не упадёт ниже порога.

Нормали шумят или узлы падают на пустых облаках

Нормали на облаке с NaN/дырками — источник тихих артефактов: проверки is_dense и отбрасывание невалидных точек до NormalEstimation обязательны (вариант — setIndices только по валидным). Вторая причина шума — слишком маленький радиус поиска после агрессивного вокселя: если лист 2 см, радиус 3 см захватит мало соседей. Правило: радиус нормалей в 3–5 раз больше листа вокселя. Пустые и вырожденные облака проверяйте empty() до любой обработки — KdTree на пустом облаке приводит к исключениям.

Узел съедает всё CPU

Классика — обработка полного облака 30 Гц без прореживания плюс тяжёлый радиус нормалей. Включите профилирование по стадиям (два вызова rclcpp::Clock и лог разницы достаточно) — обычно 90% времени уходит в одну-две стадии, и это почти всегда нормали или RANSAC без ограничения зоны. Дальше — известные ходы: крупнее воксель, меньше радиус, обработка каждого n-го кадра, вынос тяжёлой стадии в отдельный поток. И проверьте сборку: релиз-флаг (-O3 в CMAKE_BUILD_TYPE=Release) ускоряет PCL-код в разы относительно Debug-сборки, и это первое, что стоит проверить на «медленном» стенде.

Узел не получает облако вовсе

Снова QoS: подписка reliable при best-effort издателе не соединится — ставьте SensorDataQoS. Проверьте, что launch действительно запущен с enable_point_cloud:=true (без него топика просто нет) и что частота ненулевая через ros2 topic hz. Если облако приходит, но пустое — вероятно, камера не видит валидную глубину в выбранной зоне: посмотрите сырой топик в rviz2 до узла.

Камеры под 3D-обработку есть в каталоге Orbbec на luwam.ru. Для PCL-конвейеров обычно берут Gemini 335L (база 100 мм, плотное облако в помещении) и Femto Bolt для сцен с большим разбросом дистанций. Консультация по SDK, счёт для юрлица, гарантия 1 год.