PointCloud に対するフィルタ

Updated on: 2018-05-15

トップに戻る

PointCloud に対するフィルタ

３次元点群に対して様々なフィルタを施し、移動ロボットの追跡対象など意味のある情報を抽出します。

PassThrough フィルタ

PassThroughフィルタは得られた点群のうち、一定の範囲内にある点群のみを抽出します。 テキストエディタでrsj_pointcloud_test_node.cppを開いてください。

1
2

$ cd ~/catkin_ws/src/rsj_pointcloud_test/src
任意のテキストエディタで rsj_pointcloud_test_node.cpp を開く

プログラム冒頭にinclude文を追記してください。

1
2
3
4
5
6

#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>  // 追記
#include <visualization_msgs/MarkerArray.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

RsjPointcloudTestNodeクラスの冒頭に、pcl::PassThroughフィルタのインスタンスを追加します。 また、フィルタの結果を格納するためのPointCloud型変数cloud_passthrough_、および処理結果をpublishするためのパブリッシャpub_passthrough_を追加します。

1
2
3
4
5
6
7
8
9

class RsjPointcloudTestNode
{
private:
  (略)
  PointCloud::Ptr cloud_tranformed_;
  // 以下を追記
  pcl::PassThrough<PointT> pass_;
  PointCloud::Ptr cloud_passthrough_;
  ros::Publisher pub_passthrough_;

RsjPointcloudTestNodeクラスのコンストラクタでPassThroughフィルタの設定、cloud_passthrough_およびpub_passthrough_を初期化します。

1
2
3
4
5
6
7
8
9
10
11
12

RsjPointcloudTestNode()
    : nh_()
    , pnh_("~")
{
  (略)
  cloud_tranformed_.reset(new PointCloud());
  // 以下を追記
  pass_.setFilterFieldName("z");  // Z軸（高さ）の値でフィルタをかける
  pass_.setFilterLimits(0.1, 1.0);  // 0.1 ～ 1.0 m の間にある点群を抽出
  cloud_passthrough_.reset(new PointCloud());
  pub_passthrough_ = nh_.advertise<PointCloud>("passthrough", 1);
}

cbPoints関数を次のように変更します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

void cbPoints(const PointCloud::ConstPtr &msg)
{
  try
  {
    (略)
    // ここに cloud_src に対するフィルタ処理を書く
    pass_.setInputCloud(cloud_src);
    pass_.filter(*cloud_passthrough_);
    pub_passthrough_.publish(cloud_passthrough_);
    // ROS_INFO("width: %u, height: %u", cloud_src->width, cloud_src->height);  // 削除
    ROS_INFO("points (src: %zu, paththrough: %zu)",
             cloud_src->size(), cloud_passthrough_->size());  // 追記
  }
  catch (std::exception &e)
  {
    ROS_ERROR("%s", e.what());
  }
}

ビルド＆実行

まず、catkin_wsでcatkin_makeを実行して、追加したコードをビルドします。

1
2

$ cd ~/catkin_ws
$ catkin_make

次にお手持ちの３次元センサごとに次のようにノードを起動します。

Xtion PRO Live の場合

ターミナルでセンサドライバノードを起動します。

1
2
3

$ cd ~/catkin_ws/
$ source devel/setup.bash
$ roslaunch rsj_pointcloud_to_laserscan rsj_pointcloud_to_laserscan.launch

新しいターミナルを開き、rsj_pointcloud_test_nodeを起動します。

1
2
3
4
5
6
7
8

$ cd ~/catkin_ws/
$ source devel/setup.bash
$ rosrun  rsj_pointcloud_test rsj_pointcloud_test_node \
    _target_frame:=camera_link _topic_name:=/camera/depth_registered/points
[ INFO] [1524040063.315596383]: target_frame='camera_link'
[ INFO] [1524040063.315656650]: topic_name='/camera/depth_registered/points'
[ INFO] [1524040063.320448185]: Hello Point Cloud!
[ INFO] [1524040064.148595331]: points (src: 307200, paththrough: 34350)

YVT-35LX の場合

ターミナルでセンサドライバノードを起動します。

1
2
3

$ cd ~/catkin_ws/
$ source devel/setup.bash
$ roslaunch rsj_pointcloud_to_laserscan rsj_pointcloud_to_laserscan_3durg.launch

新しいターミナルを開き、rsj_pointcloud_test_nodeを起動します。

1
2
3
4
5

$ cd ~/catkin_ws/
$ source devel/setup.bash
$ rosrun rsj_pointcloud_test rsj_pointcloud_test_node \
    _target_frame:= _topic_name:=/hokuyo3d/hokuyo_cloud2
[ INFO] [1528008816.751100536]: points (src: 2674, paththrough: 1019)

実行した際にpoints (src: xxxx, paththrough: xxx)というメッセージが表示されれば成功です。 src、paththroughに続けて表示されている値はセンサから得られた、もとのPointCloudにおける点の個数とPassThroughフィルタ実行後の点の個数を示しています。 フィルタ実行後の点の個数がゼロの場合はpass_.setFilterLimits(0.1, 1.0);の引数を調節してみてください。

フィルタ実行結果の可視化

RViz でフィルタ実行後の点群の様子を可視化します。rsj_pointcloud_test_node を起動したまま、新しいターミナルを開き、 RViz を起動します。

Xtion PRO Live の場合

1
2

$ cd ~/catkin_ws/src/rsj_pointcloud_test/config/rviz
$ rviz -d view_filters.rviz

YVT-35LX の場合 の場合

1
2

$ cd ~/catkin_ws/src/rsj_pointcloud_test/config/rviz
$ rviz -d view_filters_3durg.rviz

なお、 RViz はroscoreが起動していれば、上記のようにrvizとタイプするだけでも実行可能です。

図のように Rviz の左方にあるPointCloud2のチェックボックスのうち、上2つだけにチェックを入れてください。 フィルタ実行前と実行後の点群が重なって表示されています。

XtionPointsOrigin

PointCloud2のチェックボックスのうち上から2番めのチェックだけをチェックするとフィルタ実行後の点群だけが表示されます。

XtionPointsPassThrough

VoxelGrid フィルタ

３次元点群の処理には時間がかかることが多いため、低スペックの PC の場合はある程度点を間引いておいた方が都合が良いことがあります。 VoxelGridフィルタは等間隔に点群をダウンサンプリングします。 引き続きrsj_pointcloud_test_node.cppを編集します。 プログラム冒頭にinclude文を追記してください。

1
2
3
4
5

#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>  // 追記
#include <visualization_msgs/MarkerArray.h>

typedef pcl::PointXYZ PointT;

RsjPointcloudTestNodeクラスの冒頭に、pcl::VoxelGridフィルタのインスタンスを追加します。 また、フィルタの結果を格納するためのPointCloud型変数cloud_passthrough_、および処理結果をpublishするためのパブリッシャpub_voxel_を追加します。

1
2
3
4
5
6
7
8
9

class RsjPointcloudTestNode
{
private:
  (略)
  ros::Publisher pub_passthrough_;
  // 以下を追記
  pcl::VoxelGrid<PointT> voxel_;
  PointCloud::Ptr cloud_voxel_;
  ros::Publisher pub_voxel_;

RsjPointcloudTestNodeクラスのコンストラクタでVoxelGridフィルタの設定、cloud_voxel_およびpub_voxel_を初期化します。

1
2
3
4
5
6
7
8
9
10
11

RsjPointcloudTestNode()
    : nh_()
    , pnh_("~")
{
  (略)
  pub_passthrough_ = nh_.advertise<PointCloud>("passthrough", 1);
  // 以下を追記
  voxel_.setLeafSize(0.025f, 0.025f, 0.025f);  // 0.025 m 間隔でダウンサンプリング
  cloud_voxel_.reset(new PointCloud());
  pub_voxel_ = nh_.advertise<PointCloud>("voxel", 1);
}

cbPoints関数を次のように変更します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

void cbPoints(const PointCloud::ConstPtr &msg)
{
  try
  {
    (略)
    pub_passthrough_.publish(cloud_passthrough_);
    // 以下のように追記・修正
    voxel_.setInputCloud(cloud_passthrough_);
    voxel_.filter(*cloud_voxel_);
    pub_voxel_.publish(cloud_voxel_);
    ROS_INFO("points (src: %zu, paththrough: %zu, voxelgrid: %zu)",
             msg->size(), cloud_passthrough_->size(), cloud_voxel_->size());
    // 追記・修正箇所ここまで
  }
  catch (std::exception &e)
  {
    ROS_ERROR("%s", e.what());
  }
}

ビルド＆実行

PassThroughフィルタのときと同様にビルドして実行してください。

フィルタ実行結果の可視化

RViz でフィルタ実行後の点群の様子を可視化します。 rsj_pointcloud_test_nodeを起動したまま、新しいターミナルを開き、 RViz を起動します。

Xtion PRO Live の場合

1
2

$ cd ~/catkin_ws/src/rsj_pointcloud_test/config/rviz
$ rviz -d view_filters.rviz

YVT-35LX の場合

1
2

$ cd ~/catkin_ws/src/rsj_pointcloud_test/config/rviz
$ rviz -d view_filters_3durg.rviz

RViz の左にあるPointCloud2の一番下のチェックだけをONにするとVoxelGridフィルタ実行後の点群だけが表示されます。

XtionPointsVoxel

PassThrough実行後の結果と比較すると点がまばらになっていることが分かると思います。 もし違いがわかりにくい場合はsetLeafSize関数の引数を

1
2
3
4
5
6
7
8
9

RsjPointcloudTestNode()
    : nh_()
    , pnh_("~")
{
  (略)
  /* Xtion の場合 */
  voxel_.setLeafSize(0.05f, 0.05f, 0.05f);  // LeafSize 変更
  /* YVT-35LX の場合 */
  voxel_.setLeafSize(0.3f, 0.3f, 0.3f);  // LeafSize 変更

のように大きくしてみてください（確認後は元の値に戻しておいてください）。

VoxelGridフィルタはRGBDカメラ（Xtion）のような得られる点群の数が非常に多いセンサに特に有効です。

クラスタリング

点群のクラスタリング（いくつかの塊に分離すること）により物体認識などをする際の物体領域候補が検出できます。 プログラム冒頭にinclude文を追記してください。

1
2
3
4
5
6
7

#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>  // 追記
#include <pcl/kdtree/kdtree.h>  // 追記
#include <pcl/segmentation/extract_clusters.h>  // 追記
#include <visualization_msgs/MarkerArray.h>

typedef pcl::PointXYZ PointT;

RsjPointcloudTestNodeクラスの冒頭に、pcl::search::KdTreeクラスのポインタ、pcl::EuclideanClusterExtractionクラスのインスタンス、検出されたクラスタの可視化情報をパブリッシュする pub_clusterを追加します。

1
2
3
4
5
6
7
8
9

class RsjPointcloudTestNode
{
private:
  (略)
  ros::Publisher pub_voxel_;
  // 以下を追記
  pcl::search::KdTree<PointT>::Ptr tree_;
  pcl::EuclideanClusterExtraction<PointT> ec_;
  ros::Publisher pub_clusters_;

RsjPointcloudTestNodeクラスのコンストラクタでpcl::EuclideanClusterExtractionの設定、tree_、pub_clusters_の初期化をします。

Xtion PRO Live の場合

1
2
3
4
5
6
7
8
9
10
11
12
13
14

RsjPointcloudTestNode()
    : nh_()
    , pnh_("~")
{
  (略)
  pub_voxel_ = nh_.advertise<PointCloud>("voxel", 1);
  // 以下を追記
  tree_.reset(new pcl::search::KdTree<PointT>());
  ec_.setClusterTolerance(0.15);
  ec_.setMinClusterSize(100);
  ec_.setMaxClusterSize(5000);
  ec_.setSearchMethod(tree_);
  pub_clusters_ = nh_.advertise<visualization_msgs::MarkerArray>("clusters", 1);
}

YVT-35LX の場合

1
2
3
4
5
6
7
8
9
10
11
12
13
14

RsjPointcloudTestNode()
    : nh_()
    , pnh_("~")
{
  (略)
  pub_voxel_ = nh_.advertise<PointCloud>("voxel", 1);
  // 以下を追記
  tree_.reset(new pcl::search::KdTree<PointT>());
  ec_.setClusterTolerance(0.15);
  ec_.setMinClusterSize(5);
  ec_.setMaxClusterSize(5000);
  ec_.setSearchMethod(tree_);
  pub_clusters_ = nh_.advertise<visualization_msgs::MarkerArray>("clusters", 1);
}

pcl::EuclideanClusterExtractionの設定部分のプログラムは次のとおりです。

ec_.setClusterTolerance(0.15);
15cm以上離れていれば別のクラスタだとみなす
ec_.setMinClusterSize(100); ec_.setMaxClusterSize(5000);
クラスタを構成する点の数は最低でも100個、最高で5000個
ec_.setSearchMethod(tree_);
ある点とクラスタを形成可能な点の探索方法としてKD木を使用する。

cbPoints関数を次のように変更します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

void cbPoints(const PointCloud::ConstPtr &msg)
{
  try
  {
    (略)
    pub_voxel_.publish(cloud_voxel_);
    // 以下のように追記・修正
    std::vector<pcl::PointIndices> cluster_indices;
    tree_->setInputCloud(cloud_voxel_);
    ec_.setInputCloud(cloud_voxel_);
    ec_.extract(cluster_indices);
    visualization_msgs::MarkerArray marker_array;
    int marker_id = 0;
    for (std::vector<pcl::PointIndices>::const_iterator 
             it = cluster_indices.begin(),
             it_end = cluster_indices.end();            
         it != it_end; ++it, ++marker_id)
    {
      Eigen::Vector4f min_pt, max_pt;
      pcl::getMinMax3D(*cloud_voxel_, *it, min_pt, max_pt);
      Eigen::Vector4f cluster_size = max_pt - min_pt;
      if (cluster_size.x() > 0 && cluster_size.y() > 0 && cluster_size.z() > 0)
      {
        marker_array.markers.push_back(
          makeMarker(
            frame_id, "cluster", marker_id, min_pt, max_pt, 0.0f, 1.0f, 0.0f, 0.5f));
      }
    }
    if (marker_array.markers.empty() == false)
    {
      pub_clusters_.publish(marker_array);
    }
    ROS_INFO("points (src: %zu, paththrough: %zu, voxelgrid: %zu, cluster: %zu)",
             msg->size(), cloud_passthrough_->size(), cloud_voxel_->size(),
             cluster_indices.size());
    // 追記・修正箇所ここまで
  }
  catch (std::exception &e)
  {
    ROS_ERROR("%s", e.what());
  }
}

ビルド＆実行

VoxelGridフィルタのときと同様にビルドして実行してください。

フィルタ実行結果の可視化

RViz でフィルタ実行後の点群の様子を可視化します。 rsj_pointcloud_test_nodeを起動したまま新しいターミナルを開き、 RViz を起動します。

Xtion PRO Live の場合

1
2

$ cd ~/catkin_ws/src/rsj_pointcloud_test/config/rviz
$ rviz -d view_filters.rviz

YVT-35LX の場合 の場合

1
2

$ cd ~/catkin_ws/src/rsj_pointcloud_test/config/rviz
$ rviz -d view_filters_3durg.rviz

RViz の左にあるPointCloud2の一番下のチェックだけを ON にするとVoxelGridフィルタ実行後の点群だけが表示されます。 さらにクラスタリング結果が半透明の緑の BOX で表示されているのが分かります。 これはプログラム中でクラスタリング結果を RViz が可視化可能な型である visualization_msgs::MarkerArrayに変換してパブリッシュしているからです。

XtionClusters

特定の条件に合致するクラスタを検出する

検出したクラスタのうち、一定の大きさをもつものだけを抽出するようにしましょう。 最終的にはゴミ箱や人間の足など、特定の大きさなど何らかの条件を満たすクラスタに向かって走行するように制御します。

cbPoints関数を次のように変更します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71

void cbPoints(const PointCloud::ConstPtr &msg)
{
  try
  {
    (略)
    pub_voxel_.publish(cloud_voxel_);
    std::vector<pcl::PointIndices> cluster_indices;
    tree_->setInputCloud(cloud_voxel_);
    ec_.setInputCloud(cloud_voxel_);
    ec_.extract(cluster_indices);
    visualization_msgs::MarkerArray marker_array;
    int marker_id = 0;
    /*  */
    size_t ok = 0;  // 追記
    /*  */
    for (std::vector<pcl::PointIndices>::const_iterator 
             it = cluster_indices.begin(),
             it_end = cluster_indices.end();            
         it != it_end; ++it, ++marker_id)
    {
      Eigen::Vector4f min_pt, max_pt;
      pcl::getMinMax3D(*cloud_voxel_, *it, min_pt, max_pt);
      Eigen::Vector4f cluster_size = max_pt - min_pt;
      if (cluster_size.x() > 0 && cluster_size.y() > 0 && cluster_size.z() > 0)
      {
        // 以下を追記・修正
        bool is_ok = true;
        if (cluster_size.x() < 0.05 || cluster_size.x() > 0.4)
        {
          is_ok = false;
        }
        else if (cluster_size.y() < 0.05 || cluster_size.y() > 0.6)
        {
          is_ok = false;
        }
        else if (cluster_size.z() < 0.05 || cluster_size.z() > 0.5)
        {
          is_ok = false;
        }
        visualization_msgs::Marker marker =
            makeMarker(
                frame_id, "cluster", marker_id, min_pt, max_pt, 0.0f, 1.0f, 0.0f, 0.5f);
        if (is_ok)
        {
          marker.ns = "ok_cluster";
          marker.color.r = 1.0f;
          marker.color.g = 0.0f;
          marker.color.b = 0.0f;
          marker.color.a = 0.5f;
          ok++;
        }
        marker_array.markers.push_back(marker);
        // 追記・修正箇所ここまで
      }
    }
    if (marker_array.markers.empty() == false)
    {
      pub_clusters_.publish(marker_array);
    }
    /*** 修正 ***/
    ROS_INFO("points (src: %zu, paththrough: %zu, voxelgrid: %zu,"
             " cluster: %zu, ok_cluster: %zu)",
             msg->size(), cloud_passthrough_->size(), cloud_voxel_->size(),
             cluster_indices.size(), ok);
    /*** 修正 ***/
  }
  catch (std::exception &e)
  {
    ROS_ERROR("%s", e.what());
  }
}

ビルド＆実行

クラスタリングのときと同様にビルドして実行してください。

フィルタ実行結果の可視化

クラスタリングのときと同様に RViz で可視化してください。ある一定の大きさのクラスタだけを赤く表示しているのが分かります。

XtionClusters

最も近いクラスタを検出する

前項で抽出したクラスタのうち、センサに最も近いクラスタを選択するようにしましょう。

cbPoints関数を次のように変更します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

void cbPoints(const PointCloud::ConstPtr &msg)
{
  try
  {
    (略)
    pub_voxel_.publish(cloud_voxel_);
    std::vector<pcl::PointIndices> cluster_indices;
    tree_->setInputCloud(cloud_voxel_);
    ec_.setInputCloud(cloud_voxel_);
    ec_.extract(cluster_indices);
    visualization_msgs::MarkerArray marker_array;
    /*  */
    int target_index = -1;  // 追記
    /*  */
    int marker_id = 0;
    size_t ok = 0;
    for (std::vector<pcl::PointIndices>::const_iterator 
             it = cluster_indices.begin(),
             it_end = cluster_indices.end();            
         it != it_end; ++it, ++marker_id)
    {
        (略)
        if (is_ok)
        {
          marker.ns = "ok_cluster";
          marker.color.r = 1.0f;
          marker.color.g = 0.0f;
          marker.color.b = 0.0f;
          marker.color.a = 0.5f;
          ok++;
          // 以下のように追記
          if(target_index < 0){
            target_index = marker_array.markers.size();
          }else{
            float d1 = ::hypot(marker_array.markers[target_index].pose.position.x,
                               marker_array.markers[target_index].pose.position.y);
            float d2 = ::hypot(marker.pose.position.x, marker.pose.position.y);
            if(d2 < d1){
              target_index = marker_array.markers.size();
            }
          }
          // 追記箇所ここまで
        }
        marker_array.markers.push_back(marker);
      }
    }
    if (marker_array.markers.empty() == false)
    {
      // 以下のように追記
      if(target_index >= 0){
        marker_array.markers[target_index].ns = "target_cluster";
        marker_array.markers[target_index].color.r = 1.0f;
        marker_array.markers[target_index].color.g = 0.0f;
        marker_array.markers[target_index].color.b = 1.0f;
        marker_array.markers[target_index].color.a = 0.5f;
      }
      // 追記箇所ここまで
      pub_clusters_.publish(marker_array);
    }
    ROS_INFO("points (src: %zu, paththrough: %zu, voxelgrid: %zu,"
             " cluster: %zu, ok_cluster: %zu)",
             msg->size(), cloud_passthrough_->size(), cloud_voxel_->size(),
             cluster_indices.size(), ok);
  }
  catch (std::exception &e)
  {
    ROS_ERROR("%s", e.what());
  }
}

ビルド＆実行

前項と同様にビルドして実行してください。

フィルタ実行結果の可視化

前項と同様に RViz で可視化してください。ある一定の大きさのクラスタだけを赤く表示し、その中でセンサに最も近いクラスタを紫で表示しているのが分かります。

XtionClusters