ROS2: PCL+Realsenseで3次元点群処理を試す
海洋ロボコンをやってた人です。
今回は、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は以下のような構成になっており
#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は以下のような構成になっています。
#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
のコメントアウトしてある箇所になります。
#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の依存関係も記述しなければなりません。
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()
というエラーが発生する場合
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:平面推定を行うクラス
まずは上記のクラスで平面推定を行っていきます。
// 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の領域を削除します。
// 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により近傍点探索を行い、ユークリッドクラスタリングにより各物体を識別します。
// 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のフィルター処理
- PCLのクラスタリング処理
Discussion