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

### はじめに

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

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

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

### 環境
ubuntu20.04
ROS noetic

### コード

下記URLから引用

https://github.com/PointCloudLibrary/pcl/blob/master/doc/tutorials/content/sources/octree_change_detection/octree_change_detection.cpp

```C++: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;

}

```

```cmake: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```を定義する.

```c++:
  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```構造体の作成に使用されます.

```c++:
  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```のボクセルを比較し増加しているボクセルのインデックスとその座標が表示されます.

```bash: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の内容を参考にしました.

https://vml.sakura.ne.jp/koeda/PCL/tutorials/html/octree_change.html

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

https://qiita.com/hoshianaaa/items/eff8b2ca6a1c92702141

# 宣伝
## 販売製品

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

https://hoshinarobot.base.shop/items/70144084

## お仕事依頼

- メールアドレス : ryotaro.hoshina@gmail.com

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

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