海洋ロボコンをやってた人です。
今回は、ROS2を用いたPCL+Realsenseで3次元点群を試しながら、これらを学んでみたので、自分なりに備忘録としてまとめました。

モチベーションは産ロボ/協働ロボでピック&プレースに使いたいなという点にあります。

PCLのROS2アプリケーションは様々公開されており、車輪の再発明になっていますが、自分で0から調べてプログラムを作るのも勉強ということで...

ご不明点等あれば、お気軽にコメントください。

※2023/02/26追記
※2023/04/10 Humbleでの使用方法を追記
本記事の「ROS2」表記、正しくは「ROS 2」です。

1: ROS2: PCL+Realsenseで学ぶ3次元点群処理を試す

この記述を読むことで

のように、Realsenseで物体認識を行うことができます。

使用環境は以下です。

Laptop PC
- Ubuntu 20.04 Foxy
- Ubuntu 22.04 Humble
Realsense D435/D435i

1.1: 本記事のプログラム

この記事で扱うプログラムのコードはGithubよりクローンしてご活用ください。

git clone -b refactor https://github.com/intel/ros2_intel_realsense

なお、realsense-rosを使用する場合は、Pointcloudのトピック名が「/camera/depth/color/points」となっているので、こちらも変更をしてください。

PCLのインストールも行っておきます。

sudo apt install ros-$ROS_DISTRO-pcl-*

また、今回は以下のようなフォルダ構成でパッケージを作成しています。

ros2_rs_pcl/
  ├ package.xml
  ├ CMakeLists.txt
  ├ src/
      ├ filter/
          ├ rspcl_filter_component.cpp
	  ├ rspcl_filter_node.cpp
      ├ clustering/
          ├ rspcl_clustering_component.cpp
	  ├ rspcl_clustering_node.cpp
  ├ include/
      ├ ros2_rs_pcl/
          ├ filter/
	        ├ rspcl_filter_component.hpp
	  ├ clustering/
	        ├ rspcl_clustering_component.hpp

rspcl_xxxxx_node.cppは以下のような構成になっており

rspcl_filter_node.cpp

#include "rclcpp/rclcpp.hpp"
#include "ros2_rs_pcl/filter/rspcl_filter_component.hpp"

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

rspcl_xxxxx_component.hppは以下のような構成になっています。

rspcl_filter_component.hpp

#ifndef ROS2_RS_PCL__RSPCL_FILTER_COMPONENT_HPP_
#define ROS2_RS_PCL__RSPCL_FILTER_COMPONENT_HPP_

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"


class RspclFilterComponent : public rclcpp::Node
{
  public:
    RspclFilterComponent();

  private:
    void timer_callback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg);
    double leaf_size_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscriber_;
    size_t count_;
};

#endif // ROS2_RS_PCL__RSPCL_FILTER_COMPONENT_HPP_

clusteringの場合は、呼び出すヘッダーおよびクラス名がFilter>Clusteringと変更されているだけです。

そして、実際にプログラムを変更する箇所はrspcl_xxxxx_component.cppのコメントアウトしてある箇所になります。

rspcl_filter_component.cpp

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>

#include <pcl/filters/passthrough.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>

#include "ros2_rs_pcl/filter/rspcl_filter_component.hpp"


RspclFilterComponent::RspclFilterComponent() : Node("pclsub")
{
  subscriber_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
    "/camera/pointcloud", 
    10, 
    std::bind(&RspclFilterComponent::timer_callback, this, std::placeholders::_1)\
  );

  using namespace std::chrono_literals;
  publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/pcl_data", 10);
}


void RspclFilterComponent::timer_callback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromROSMsg(*cloud_msg, *cloud);

  RCLCPP_INFO(this->get_logger(), "points_size(%d,%d)",cloud_msg->height,cloud_msg->width);

  // define a new container for the data
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);

  // この箇所にプログラムを追記

  sensor_msgs::msg::PointCloud2 sensor_msg;
  pcl::toROSMsg(*cloud_filtered, sensor_msg);
  publisher_->publish(sensor_msg);
}

忘れずに、CMakeLists.txtに依存関係も追記しましょう。

でも説明のあるように、CMakeLists.txtに、PCLの依存関係も記述しなければなりません。

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(ros2_rs_pcl)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io)

find_package(PCL 1.1 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

include_directories(
  ${PROJECT_SOURCE_DIR}/include
)

# Add executable
add_executable(rs_pcl_filter src/filter/rspcl_filter_node.cpp src/filter/rspcl_filter_component.cpp)
add_executable(rs_pcl_clustering src/clustering/rspcl_clustering_node.cpp src/clustering/rspcl_clustering_component.cpp)

ament_target_dependencies(rs_pcl_filter
  rclcpp 
  sensor_msgs
)

ament_target_dependencies(rs_pcl_clustering
  rclcpp 
  sensor_msgs
)

target_link_libraries(rs_pcl_filter ${PCL_LIBRARIES})
target_link_libraries(rs_pcl_clustering ${PCL_LIBRARIES})

# Install Cpp executables
install(TARGETS
  rs_pcl_filter
  rs_pcl_clustering
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

というエラーが発生する場合

CMakeLists.txt

ament_target_dependencies(rs_pcl_clustering
  rclcpp 
  sensor_msgs
+ pcl_conversions
)

と追記してください。Humbleではこちらで解決しました。

1.2: PCLのフィルター処理

pclのフィルター処理を扱う前にpclの型について触れておきます。

pclにはPointの型が以下のように準備されています。

pcl::PointXYZ : 位置
pcl::PointXYZI : 位置+輝度
pcl::PointXYZRGB : 位置+輝度（RGB）
pcl::PointNormal : 位置+法線

一般的なRGB画像は輝度を記録しているのでpcl::PointXYZIの型宣言でプログラムを実行すると

Failed to find match for field 'intensity'.

というエラーが生じます。

解決方法は単純で、型宣言をXYZやXYZRGBに変えてあげればよく、Realsenseも同様にXYZRGBの型でデータを扱っていきます。

PassThrough Filter

パススルーフィルターでは、指定した領域の点群を通します。
以下ではx軸が1.0mから3.0mの範囲の点群を通しています。

  // PassThrough Filter
  pcl::PassThrough<pcl::PointXYZRGB> pass;
  pass.setInputCloud(cloud);
  pass.setFilterFieldName("x");  // x axis
  // extract point cloud between 1.0 and 3.0 m
  pass.setFilterLimits(1.0,3.0);
  // pass.setFilterLimitsNegative (true);   // extract range reverse
  pass.filter(*cloud_filtered);

Rviz2側で、Color transformer: RGB8と設定するとRGB値の点群が表示できます。
下図では、パススルーフィルターが見やすいよう、Color transformer: Intensityとしています。

また、pass.setFilterLimitsNegative (true);をアンコメントすると

パススルーの範囲が反転します。

Voxel Grid

PCLのVoxel Gridには2種類のクラスが用意されており、それぞれ違いがあるそうです。

まずはApproximate Voxel Gridを試してみます。

  // Approximate Voxel Grid
  pcl::ApproximateVoxelGrid<pcl::PointXYZRGB> avg;
  avg.setInputCloud(cloud);
  avg.setLeafSize(0.2f, 0.2f, 0.2f);
  // avg.setDownsampleAllData(true);
  avg.filter(*cloud_filtered);

avg.setLeafSize(0.05f, 0.05f, 0.05f);のとき

点群の数が減っているのが分かります。

続いて、Voxel Grid

  // Voxel Grid: pattern 1
  pcl::VoxelGrid<pcl::PointXYZRGB> voxelGrid;
  voxelGrid.setInputCloud(cloud);
  leaf_size_ = 0.1;
  // set the leaf size (x, y, z)
  voxelGrid.setLeafSize(leaf_size_, leaf_size_, leaf_size_);
  // apply the filter to dereferenced cloudVoxel
  voxelGrid.filter(*cloud_filtered);

Voxel Gridで点群の総数が減っているのが確認できます。
点群の数が多い状態で様々な処理をかけるとFPSが著しく下がるため、こちらの処理で点群を間引くことが役立ちます。

  // Voxel Grid: pattern 2
  pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2);
  pcl::toPCLPointCloud2(*cloud, *cloud_blob);
  pcl::VoxelGrid<pcl::PCLPointCloud2> vg;
  vg.setInputCloud(cloud_blob);
  leaf_size_ = 0.1;
  vg.setLeafSize(leaf_size_, leaf_size_, leaf_size_);
  vg.filter(*cloud_filtered_blob);
  pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);

上記のように、プログラムすることもできます。

Outlier removal: 外れ値除去

データ内の不要なノイズを排除する手法として外れ値除去があり、こちらもPCLで実装することができます。

Statistical Outlier Removal

StatisticalOutlierRemoval : 点近傍統計を使用して外れ値データをフィルター処理

  // Statistical Outlier Removal
  pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
  sor.setInputCloud(cloud);
  sor.setMeanK(50);
  sor.setStddevMulThresh(0.1);
  sor.setNegative(false);
  sor.filter (*cloud_filtered);

細かな点群が削除されているのが確認できる

Radius OUtlier Removal

RadiusOutlierRemoval : Cloud内のポイントが持つ近傍の数に基づいてポイントをフィルター処理

  // Radius Outlier Removal
  pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> outrem;
  outrem.setInputCloud(cloud);
  outrem.setRadiusSearch(0.1);
  outrem.setMinNeighborsInRadius(2);
  outrem.setKeepOrganized(true);
  outrem.filter(*cloud_filtered);

Statistical Outlier Removalと異なり、必要そうな点群（点群内で囲まれた領域）は除去されていないことが確認できる。

Conditional Removal

ConditionalRemoval : 特定の条件を満たすデータをフィルタリング

pass through filterと同じ役割を担い

  // Conditional Removal
  pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZRGB>());
  range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB>("x", pcl::ComparisonOps::GT, 0.0)));
  range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB>("x", pcl::ComparisonOps::LT, 3.0)));
  pcl::ConditionalRemoval<pcl::PointXYZRGB> condrem;
  condrem.setCondition(range_cond);
  condrem.setInputCloud(cloud);
  // condrem.setKeepOrganized(true);
  condrem.filter(*cloud_filtered);
  // vector<int> Idx;
  // pcl::removeNaNFromPointCloud(*cloud_filtered, *cloud_filtered, Idx);

x軸が0.0mから3.0mの範囲の点群が出力される

1.3: PCLのクラスタリング処理

クラスタリング処理では、一番始めに紹介した動画のような平面に置かれた物体をセグメンテーション化して認識するプロセスを紹介していきます。

SACSegmentation

SACSegmentation：平面推定を行うクラス

まずは上記のクラスで平面推定を行っていきます。

rspcl_clustering_component.cpp

  // Voxel Grid: pattern 1
  pcl::VoxelGrid<pcl::PointXYZRGB> voxelGrid;
  voxelGrid.setInputCloud(cloud);
  // set the leaf size (x, y, z)
  voxelGrid.setLeafSize(0.02, 0.02, 0.02);
  // apply the filter to dereferenced cloudVoxel
  voxelGrid.filter(*cloud_filtered);

  // LeafSizeを細かくしすぎると、エラーとなり、止まる
  // [pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.

  // SAC Segmentation
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);  
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);  
  // Create the segmentation object  
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;  
  double threshould = 0.01;
  // Optional  
  seg.setOptimizeCoefficients (true);  
  // Mandatory  
  seg.setModelType (pcl::SACMODEL_PLANE);  
  seg.setMethodType (pcl::SAC_RANSAC);  
  seg.setDistanceThreshold (threshould);  
  seg.setInputCloud (cloud_filtered);  
  seg.segment (*inliers, *coefficients);  

  for (size_t i = 0; i < inliers->indices.size (); ++i) {
    cloud_filtered->points[inliers->indices[i]].r = 255;  
    cloud_filtered->points[inliers->indices[i]].g = 0;  
    cloud_filtered->points[inliers->indices[i]].b = 0;  
  }

セグメンテーションすると、FPSがかなり下がるため、前処理としてVoxel Gridで点群を減らしています。

ExtractIndices

直訳するとインデックスの抽出となり、指定したIndicesを残すか削除することができます。
extract.setNegative(true)とすることで、inliersの領域を削除します。

rspcl_clustering_component.cpp

  // Extract the planar inliers from the input cloud
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  extract.setInputCloud (cloud_filtered);
  extract.setIndices(inliers);
  extract.setNegative(true);
  extract.filter (*cloud_filtered);

KdTree+EuclideanCluster

上記で平面を削除できたら、KdTreeにより近傍点探索を行い、ユークリッドクラスタリングにより各物体を識別します。

rspcl_clustering_component.cpp

  // Create the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
  tree->setInputCloud (cloud_filtered);

  // create the extraction object for the clusters
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ece;
  // specify euclidean cluster parameters
  ece.setClusterTolerance (0.02); // 2cm
  ece.setMinClusterSize (20);
  ece.setMaxClusterSize (10000);
  ece.setSearchMethod (tree);
  ece.setInputCloud (cloud_filtered);
  // exctract the indices pertaining to each cluster and store in a vector of pcl::PointIndices
  ece.extract (cluster_indices);


  pcl::PCDWriter writer;
  int j = 0;  
  float colors[6][3] ={{255, 0, 0}, {0,255,0}, {0,0,255}, {255,255,0}, {0,255,255}, {255,0,255}};  
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);  
  pcl::copyPointCloud(*cloud_filtered, *cloud_cluster);  
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)  
    {  
      for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) {  
  cloud_cluster->points[*pit].r = colors[j%6][0];  
  cloud_cluster->points[*pit].g = colors[j%6][1];  
  cloud_cluster->points[*pit].b = colors[j%6][2];  
      }  
      // std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;  
      // std::stringstream ss;  
      // ss << "cloud_cluster_" << j << ".pcd";  
      // writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_cluster, false);  
      j++;  
    }

物体のクラスタリングができていることが確認できますね。

以上。

Reference

PCLのフィルター処理

ros2中调用PCL进行点云滤波的使用总结

PCL 第13回名古屋CV・PRML勉強会

Outlier（外れ値、異常値）を除去するプログラム

PCLのクラスタリング処理

Point Cloud Libraryを試す（その４：平面抽出）

OUXT-Polaris/pcl_apps

PCL-ROS-cluster-Segmentation

Discussion