top of page
Anchor 1

ポイントクラウドライブラリ (PCL) + ROS (#1)

はじめに

近年、ロボットの認識に関する研究開発において、ポイントクラウド (点群)の活用が活発化しています。ポイントクラウドとは、計測対象の三次元構造を点の集合で表現したものです。これをリアルタイムに取得する方法として、距離画像カメラや3次元Lidarなどが広く用いられています。また、この三次元点群について様々な処理を行うライブラリとして、PCL (Point Cloud Library)が広く用いられています。

PCLは、ポイントクラウドの位置、色、法線ベクトル、反射強度等の様々なデータタイプを扱うことができるうえ、フィルタリング、特徴抽出、レジストレーション、モデルフィッテイングといった主要な点群処理アルゴリズムを提供します。


PCLによる処理の基本

PCLでは各種点群処理のテンプレートクラスが存在しており、これをインスタンス化した後に、処理対象のポイントクラウドやパラメータを入力して、処理メソッドを実行します。ポイントクラウドのデータ型をテンプレートとして指定することで、そのデータ型に即した処理が可能です。



PCLでは入出力のポイントクラウドのクラス型としては、[pcl::PointCloud]が使用されます。使用するポイントクラウドのデータ型の代表はpcl::PointXYZや、それに加えて反射強度を格納した[pcl::PointXYZI]等があります。


ROS API

PCLのデータとROSのデータを相互に変換するためのAPIが pcl_conversionsというパッケージで提供されます。pcl_conversions パッケージは、PCL の点群を ROS のメッセージに変換するために使用されます。ROS には、様々な種類のメッセージがありますが、pcl_conversions パッケージでは、PointCloud2 メッセージを使用します。PointCloud2 メッセージは、点群の座標、色、法線などの情報を含むバイナリデータを含みます。pcl_conversions パッケージは、PCL の点群を PointCloud2 メッセージに変換するための関数や、PointCloud2 メッセージを PCL の点群に変換するための関数を提供します。

以下は、pcl_conversions パッケージで使用される主な関数の例です。



この関数は、PCL の点群を PointCloud2 メッセージに変換します。第1引数には、変換する点群データが含まれる pcl::PointCloud オブジェクト、第2引数には、変換後の PointCloud2 メッセージを格納する sensor_msgs::PointCloud2 オブジェクトを指定します。この関数を使用すると、PCL の点群を ROS のメッセージに変換することができます。


　この関数は、PointCloud2 メッセージを PCL の点群に変換します。第1引数には、変換する PointCloud2 メッセージを格納する sensor_msgs::PointCloud2 オブジェクト、第2引数には、変換後の pcl::PointCloud オブジェクトを指定します。この関数を使用すると、ROS のメッセージを PCL の点群に変換することができます。


サンプルコード

以下に PCL を使用した ROS ノードのサンプルコードを示します。このコードは、ROS メッセージから PointCloud2 データを取得し、PCL を使用して指定した範囲内の点群データをフィルタリングするものです。具体的には、VoxelGrid フィルタを使用して、点群データをダウンサンプリングします。


#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>

// コールバック関数
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& input)
{
  // ROS メッセージから pcl::PointCloud オブジェクトに変換
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*input, *cloud);

  // VoxelGrid フィルタを作成
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud(cloud);
  sor.setLeafSize(0.1f, 0.1f, 0.1f); // フィルタリングする範囲を設定
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
  sor.filter(*cloud_filtered);

  // フィルタリング後の点群データを ROS メッセージに変換して配信
  sensor_msgs::PointCloud2 output;
  pcl::toROSMsg(*cloud_filtered, output);
  output.header = input->header;
  pub.publish(output);
}

int main(int argc, char** argv)
{
  // ROS ノードの初期化
  ros::init(argc, argv, "pcl_filter_node");
  ros::NodeHandle nh;

  // サブスクライバーの作成
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/input_topic", 1, cloudCallback);

  // パブリッシャーの作成
  ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/output_topic", 1);

  // ROS のメインループ
  ros::spin();

  return 0;
}


Comentários

bottom of page