ROSでPCLのサンプルを動かす - 点群の差分検出(octree change detection)

はじめに

点群から背景を削除したいと思い方法を調べました．

方法を調べた結果,PCLのOctreePointCloudChangeDetectorというクラスを使用すると増加している点群の抽出ができ，そのようなことが実現できるとわかりました．

使い方を知るためOctreePointCloudChangeDetectorクラスのサンプルコードの動作させてみたのでその内容を記載します．

環境

ubuntu20.04
ROS noetic

コード

下記URLから引用

octree_change_detection.cpp

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>

#include <iostream>
#include <vector>
#include <ctime>

int
main (int argc, char** argv)
{
  srand ((unsigned int) time (NULL));

  // Octree resolution - side length of octree voxels
  float resolution = 32.0f;

  // Instantiate octree-based point cloud change detection class
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );

  // Generate pointcloud data for cloudA
  cloudA->width = 128;
  cloudA->height = 1;
  cloudA->points.resize (cloudA->width * cloudA->height);

  for (std::size_t i = 0; i < cloudA->size (); ++i)
  {
    (*cloudA)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudA)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudA)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudA to octree
  octree.setInputCloud (cloudA);
  octree.addPointsFromInputCloud ();

  // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
  octree.switchBuffers ();

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
   
  // Generate pointcloud data for cloudB 
  cloudB->width = 128;
  cloudB->height = 1;
  cloudB->points.resize (cloudB->width * cloudB->height);

  for (std::size_t i = 0; i < cloudB->size (); ++i)
  {
    (*cloudB)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudB)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudB)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudB to octree
  octree.setInputCloud (cloudB);
  octree.addPointsFromInputCloud ();

  std::vector<int> newPointIdxVector;

  // Get vector of point indices from octree voxels which did not exist in previous buffer
  octree.getPointIndicesFromNewVoxels (newPointIdxVector);

  // Output points
  std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
  for (std::size_t i = 0; i < newPointIdxVector.size (); ++i)
    std::cout << i << "# Index:" << newPointIdxVector[i]
              << "  Point:" << (*cloudB)[newPointIdxVector[i]].x << " "
              << (*cloudB)[newPointIdxVector[i]].y << " "
              << (*cloudB)[newPointIdxVector[i]].z << std::endl;

}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(pcl_sample)

find_package(catkin REQUIRED)
find_package(PCL 1.8 REQUIRED)

catkin_package(
)

include_directories(
 include
 ${PCL_INCLUDE_DIRS}
 #${catkin_INCLUDE_DIRS}
)

add_executable(change_detection src/change_detection.cpp)

target_link_libraries(change_detection
  ${PCL_LIBRARIES}
  #${catkin_LIBRARIES}
)

###解説

• まずOctreePointCloudChangeDetectorクラスのインスタンスを作成しそのvoxel resolutionを定義する.

  srand ((unsigned int) time (NULL));

  // Octree resolution - side length of octree voxels
  float resolution = 32.0f;

  // Instantiate octree-based point cloud change detection class
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);

• そしてランダムなポイントデータで初期化されたcloudAというpoint cloudインスタンスを作成.作成した点群データは1つのoctree構造体の作成に使用されます.

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );

  // Generate pointcloud data for cloudA
  cloudA->width = 128;
  cloudA->height = 1;
  cloudA->points.resize (cloudA->width * cloudA->height);

  for (size_t i = 0; i < cloudA->points.size (); ++i)
  {
    cloudA->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudA->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudA->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudA to octree
  octree.setInputCloud (cloudA);
  octree.addPointsFromInputCloud ();

• cloudAは参照するための点群でそのoctree構造体がその空間分布を表現します.OctreePointCloudChangeDetectionクラスはOctree2BufBaseクラスを継承しており2つのoctreeを同時にメモリー内で保存したり管理することができます."octree.switchBuffers()"を使用すると過去のoctree構造体をメモリーに保存したままoctreeクラスをリセットすることができます.

  // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
  octree.switchBuffers ();

次に2つ目の点群のインスタンス"cloudB"を作成し,ランダムなデータで満たします.この点群は新しいoctree構造体の作成に使用されます.

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
   
  // Generate pointcloud data for cloudB 
  cloudB->width = 128;
  cloudB->height = 1;
  cloudB->points.resize (cloudB->width * cloudB->height);

  for (size_t i = 0; i < cloudB->points.size (); ++i)
  {
    cloudB->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudB->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudB->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudB to octree
  octree.setInputCloud (cloudB);
  octree.addPointsFromInputCloud ();

現在のoctree構造体(cloudBから作成された
)で過去のoctree構造体(cloudAから作成された)に存在しないvoxelの点を取得するためにpoint indicesのベクトルを返す"getPointIndicesFromNewVoxelsというメソッドを呼びます.

  std::vector<int> newPointIdxVector;

  // Get vector of point indices from octree voxels which did not exist in previous buffer
  octree.getPointIndicesFromNewVoxels (newPointIdxVector);

最後にstd::cout streamで結果を出力します.

  // Output points
  std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
  for (size_t i = 0; i < newPointIdxVector.size (); ++i)
    std::cout << i << "# Index:" << newPointIdxVector[i]
              << "  Point:" << cloudB->points[newPointIdxVector[i]].x << " "
              << cloudB->points[newPointIdxVector[i]].y << " "
              << cloudB->points[newPointIdxVector[i]].z << std::endl;

結果

以下のようにcloudAベースのoctreeのボクセルとcloudBベースのoctreeのボクセルを比較し増加しているボクセルのインデックスとその座標が表示されます.

result

Output from getPointIndicesFromNewVoxels:
0# Index:3  Point:20.3452 1.81795 17.8999
1# Index:29  Point:15.6769 1.23849 30.9812
2# Index:94  Point:19.2824 0.975498 33.1067
3# Index:36  Point:50.6501 0.0791533 20.7237
4# Index:60  Point:39.7209 0.0998207 15.1994
5# Index:89  Point:40.9676 1.40058 9.41052
6# Index:86  Point:55.8706 1.69124 6.36924
7# Index:33  Point:60.9991 50.969 62.419

参考

解説などは以下URLの内容を参考にしました.

ROS環境の構築やビルドの方法などは以下URLの内容を参照ください.

宣伝

販売製品

• 3D Vision Controllerというものを開発しました．ロボットアームによる3次元ピッキングをWebUIから構築するためのソフトウェアが内蔵されているコントローラです．
  ご興味ございましたら是非ご連絡下さい！

お仕事依頼

• メールアドレス : [e-mail]

• coconala : https://coconala.com/users/4162706

• CloudWorks : https://crowdworks.jp/public/employees/5569031