Point Cloud Library (PCL)を使ってみる


# PCLのインストール
* Windows：https://tecsingularity.com/pcl/environment/
* Ubuntu：https://qiita.com/tewi_r/items/941acb2af690f8f184a1

上記のサイトに従っていけばOK

# PCLの解説
PCL自体の日本語資料は少なく，基本的なことしかの書かれていない印象．（[参考サイト１](https://qiita.com/tewi_r/items/941acb2af690f8f184a1)，[２](https://tecsingularity.com/pcl/make_point_cloud/)，[３](https://tm-labo.hatenablog.com/entry/2019/09/19/193151)）
☝のサイトに書かれていることは，ここでは割愛する．

## ウサギのpoint cloud を表示する！
PCLが公式に出している[サンプルコードこちら](https://github.com/PointCloudLibrary/pcl/tree/master/examples)を参考にして，ウサギの形のpoint cloudを表示させてみた．

ただし，ウサギのもととなる[bunny.pcdファイル](https://github.com/PointCloudLibrary/pcl/blob/master/test/bunny.pcd)はgithubからダウンロード（またはコピペ）した．コピペする場所はファイル構成を参照．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/1619130e-9e9b-135a-a00b-27d7f74542f8.png)

1. フォルダの作成
    ```
    $ mkdir -p pcl-test/src
    $ cd pcl-test
    $ mkdir build

    <ファイル構成>
    pcl-test 
       |----- CMakeList.txt
       |----- src
               |----- bunny_rgb.cpp
       |----- build
               |----- bunny.pcd
    ```
2. CMakeList.txtを記入
    ```txt:pcl-test/CMakeList.txt
    cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
    project(pcd_write)
    
    find_package(PCL 1.2 REQUIRED)
    
    include_directories(${PCL_INCLUDE_DIRS})
    link_directories(${PCL_LIBRARY_DIRS})
    add_definitions(${PCL_DEFINITIONS})
    
    add_executable (bunny_rgb src/bunny_rgb.cpp)
    target_link_libraries (bunny_rgb ${PCL_LIBRARIES})
    ```
3. ソースコードの作成
    もとのウサギはXYZ情報（各点の位置情報）のみであり，RGB情報は含まない（各点に色が付いていない）．そこで，読み込んだ情報に色を付けて表示する．

    ```cpp:pcl-test/src/bunny_rgb.cpp
    #include <pcl/visualization/cloud_viewer.h>
    #include <pcl/io/pcd_io.h>
    #include <iostream>
    #include <pcl/common/impl/io.hpp>

    using namespace std;

    // ビューワー起動時の一回だけ呼ばれる
    void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
    {
        // 背景色の設定 
        viewer.setBackgroundColor(1, 1, 1); //white
        cout << "viewerOneOff" << std::endl;
    }

    // ビューワー起動中の毎フレーム実行される
    void viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
    {
        cout << "viewerPsycho" << std::endl;
    }


    int main()
    {   // set Type
        using CloudType = pcl::PointCloud<pcl::PointXYZ>;
        using CloudType2 = pcl::PointCloud<pcl::PointXYZRGB>;

        CloudType::Ptr original_cloud(new CloudType);
        CloudType2::Ptr rgb_cloud(new CloudType2);

        // 作成したPointCloudを読み込む
        pcl::io::loadPCDFile("bunny.pcd", *original_cloud);

        // copy p_cloud data into rgb_cloud
        copyPointCloud(*original_cloud, *rgb_cloud);

        // add RGB data to rgb_cloud
        for (int h = 0; h < rgb_cloud->height; h++) {
                for (int w = 0; w < rgb_cloud->width; w++) {
                        pcl::PointXYZRGB &point = rgb_cloud->points[w + h * rgb_cloud->width];
                        point.r = 255;
                        point.g = 0;
                        point.b = 0;
                }
        }

        // ビューワーの作成
        pcl::visualization::CloudViewer viewer("PointCloudViewer");
        viewer.showCloud(rgb_cloud);
        
        // ビューワー起動時の一回だけ呼ばれる関数をセット
        viewer.runOnVisualizationThreadOnce(viewerOneOff);
        
        // ビューワー起動中の毎フレーム実行される関数をセット
        viewer.runOnVisualizationThread(viewerPsycho);
        
        // ビューワー視聴用ループ
        while (!viewer.wasStopped())
        {
        
        }
        return 0;
    }  
    ```

4. ビルドする
    ```
    $ cd ~/pcl-test/build
    $ cmake ..
    $ make
    ```
5. 実行する
    ```
    $ ./bunny_rgb
    ```
    <table>
    <tr>
        <td> red points </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/1619130e-9e9b-135a-a00b-27d7f74542f8.png" alt="Red">
        </td>
    </tr>
    <tr>
        <td> rainbow points </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/8c8ab9c3-c1e4-5364-3d2b-fc2e2b427108.png" alt="rainbow">
        </td>
    </tr>
    </table>

## カラーマップでウサギをグラデーション
3次元画像で高低差を色で示すように，y方向の値の大きさによって点の色を変えた．
（**☟の写真の緑がy軸，赤がx軸，青がz軸であることに注意！**）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/3a53b16d-a190-dc2c-1a3a-a886dc79cc89.png)

下のコードの流れとしては，
[1. 各点を赤く設定していたコードを削除](#1-各点を赤く設定していたコードを削除)
[2. main関数の外にカラーマップ用の関数GetColorを作成](#2-main関数の外にカラーマップ用の関数GetColorを作成)
[3. main関数の中に，y方向の最大/最小値を取得する関数を作成](#3-main関数の中に，y方向の最大/最小値を取得する関数を作成)
[4. main関数の中で，GetColor関数を実行](#4-main関数の中で，GetColor関数を実行)


### 1. 各点を赤く設定していたコードを削除
```c++:pcl-test/src/bunny_rgb.cpp
int main(){
    ．．．
    ．．．
    // add Red Points to rgb_cloud
    for (int h = 0; h < rgb_cloud->height; h++) {
        for (int w = 0; w < rgb_cloud->width; w++){
            pcl::PointXYZRGB &point = rgb_cloud->points[w + h * rgb_cloud->width];
            point.r = 255;
            point.g = 0;
            point.b = 0;
        }
    }
    
```



### 2. main関数の外にカラーマップ用の関数 *GetColor* を作成
```c++:pcl-test/src/bunny_rgb.cpp
// Color map
typedef struct {
    double r,g,b;
} COLOR;

COLOR GetColor(double v,double vmin,double vmax)
{
   COLOR c = {255, 255, 255}; // white
   double dv;

   if (v < vmin)
      v = vmin;
   if (v > vmax)
      v = vmax;
   dv = vmax - vmin;

   if (v < (vmin + 0.25 * dv)) {
      c.r = 0;
      c.g = 4 * (v - vmin) / dv * 255;
      c.b = 255;
   } else if (v < (vmin + 0.5 * dv)) {
      c.r = 0 * 255;
      c.g = 255;
      c.b = 255 + 4 * (vmin + 0.25 * dv - v) / dv * 255;
   } else if (v < (vmin + 0.75 * dv)) {
      c.r = 4 * (v - vmin - 0.5 * dv) / dv * 255;
      c.g = 255;
      c.b = 0 * 255;
   } else {
      c.r = 255;
      c.g = 255 + 4 * (vmin + 0.75 * dv - v) / dv * 255;
      c.b = 0 * 255;
   }

   return(c);
}

```

### 3. main関数の中に，y方向の最大/最小値を取得する関数を作成
```c++:pcl-test/src/bunny_rgb.cpp
int main(){
    ・・・
    ・・・

        // add Rainbow Point to rgb_cloud
        // get min&max value
        double min = INT_MAX; double max = INT_MIN;
        for (int h = 0; h < rgb_cloud->height; h++) {
                for (int w = 0; w < rgb_cloud->width; w++) {
                        pcl::PointXYZRGB &point = rgb_cloud->points[w + h * rgb_cloud->width];
                        if (point.y < min) {
                                min = point.y;
                        }
                        if (point.y > max) {
                                max = point.y;
                        }
                }
        }
```

### 4. main関数の中で，GetColor関数を実行
```c++:pcl-test/src/bunny_rgb.cpp
int main(){
    ・・・
    ・・・
       // add RGB data to rgb_cloud
        for (int h = 0; h < rgb_cloud->height; h++) {
                for (int w = 0; w < rgb_cloud->width; w++) {
                        pcl::PointXYZRGB &point = rgb_cloud->points[w + h * rgb_cloud->width];
                        COLOR c = GetColor(point.y, min, max);
                        point.r = c.r;
                        point.g = c.g;
                        point.b = c.b;
                }
        }
}
```

## ポイントクラウドの座標を回転
回転行列を用いて，ウサギを各軸まわりに回転させる．

:::note warn
注意！
回転行列が回転させるのは座標系なので
90°回転させる回転行列＝座標系が90°回転＝各点（ウサギ）は-90°回転
:::

https://rikei-tawamure.com/entry/2019/11/04/184049

```cpp:pcl-test/src/bunny_rgb.cpp
// Rotation module
typedef struct {
    double array[3];
} POSITION;

POSITION rot(POSITION p0, double phi_deg, double sita_deg, double psi_deg)
{
    POSITION pz = {0, 0, 0}; //position rotated around z axis
    POSITION py = {0, 0, 0}; //position roteted around y axis
    POSITION px = {0, 0, 0}; //position rotated around x axis

    double RAD = M_PI / 180; // convert degree to rad
    double phi = phi_deg * RAD;
    double sita = sita_deg * RAD;
    double psi = psi_deg * RAD;

    double Rx[3][3] = {
        {1, 0, 0},
        {0, cos(phi), sin(phi)},
        {0, -1 * sin(phi), cos(phi)}
    };

    double Ry[3][3] = {
        {cos(sita), 0, -1 * sin(sita)},
        {0, 1, 0},
        {sin(sita), 0, cos(sita)}
    };

    double Rz[3][3] = {
        {cos(psi), sin(psi), 0},
        {-1 * sin(psi), cos(psi), 0},
        {0, 0, 1}
    };

    for (int i = 0; i < 3; i++){
        for (int j = 0; j < 3; j++){
            pz.array[i] = pz.array[i] + Rz[i][j] * p0.array[j];
        }
    }

   for (int i = 0; i < 3; i++){
        for (int j = 0; j < 3; j++){
            py.array[i] = py.array[i] + Ry[i][j] * pz.array[j];
        }
    }


    for (int i = 0; i < 3; i++){
        for (int j = 0; j < 3; j++){
            px.array[i] = px.array[i] + Rx[i][j] * py.array[j];
        }
    }
    return (px);
}

void main(){
    ．．．
    ．．．
    ．．．
        // Rotate Point Cloud to rgb_cloud
        // rotate rgb_cloud around x-axis and name it rgb_cloud
        for (int h = 0; h < rgb_cloud->height; h++) {
                for (int w = 0; w < rgb_cloud->width; w++) {
                        pcl::PointXYZRGB &point = rgb_cloud->points[w + h * rgb_cloud->width];
                        POSITION p0 = {point.x, point.y, point.z};
                        POSITION p = rot(p0, 0, 0, 90);
                        point.x = p.array[0];
                        point.y = p.array[1];
                        point.z = p.array[2];
              }
        }

}
```

<table>
    <thead>
        <tr>
            <th> </th> <th> front view </th> <th> side view </th>
        </tr>
    </thead>
    <tr>
        <td> オリジナル </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/3a53b16d-a190-dc2c-1a3a-a886dc79cc89.png" alt="original front">
        </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/41da6c53-8a93-483f-9ede-6490278b1d03.png" alt="original side">
        </td>
    </tr>
    <tr>
        <td> x軸回り </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/947eb935-1937-5cc1-8c6e-14a36f1b9f03.png" alt="x-axis front">
        </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/7487f7df-56f5-2502-770e-8e8a9c63e060.png" alt="x-axis side">
        </td>
    </tr>
    <tr>
        <td> y軸回り </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/6daba524-ee4f-ad42-af10-9c1f420adfa1.png" alt="y-axis front">
        </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/0d0da920-3251-870b-bb38-bcce257935bb.png" alt="y-axis side">
        </td>
    </tr>
    <tr>
        <td> z軸回り </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/36438150-17ea-1729-3288-53b636c8da8a.png" alt="z-axis front">
        </td>
        <td> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/e766aa90-72cb-5d2d-7b8f-87487dd875d0.png" alt="z-axis side">
        </td>
    </tr>
</table>

# rosでPCLを使う
## rosインターフェースのインストール
[このページ](http://wiki.ros.org/pcl_ros)から，必要なパッケージは以下と分かる．
* perception_pcl
    * pcl_conversions
    * pcl_msgs
    * pcl_ros


`perception_pcl`パッケージに３つが含まれている形（のはず）なので，`perception_pcl`をインストールする．
```
sudo apt install ros-rosのバージョン-perception-pcl
```
## ros topicのpublish & subscribe
[ros wikiのページ](http://wiki.ros.org/pcl_ros)に載っている通りに書けばできる．

## point cloud の表示など
topicのpublish/subscribeはrosWikiの通りにやれば問題なくできるが，一方このままでは使えるPCLの機能が非常に少ない．

**具体的には，subscribeしたpoint cloudをウィンドウで表示することすらできない！**

そこで，`CMakeList`に<INS>PCL本体を使えるようにするコード</INS>を書き足す必要がある．（つまり，rosインターフェースはその名の通り，ros topicに対応するデータ型にPCLのデータ型を変換するだけ）

```diff:CMakeList.txt
+    find_package(PCL 1.8 REQUIRED)
    
+    include_directories(
+     include
+      ${PCL_INCLUDE_DIRS}
+    )
    
    /// node作成時のみ
    add_executable(rotation_pcl
      src/rotation_pcl.cpp
      src/func.cpp
    )
    add_dependencies(rotation_pcl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
    target_link_libraries(rotation_pcl
+   ${PCL_LIBRARIES}　　　<---通常のノード作成コードに加えて${PCL_LIBRARIES}を追加
    ${catkin_LIBRARIES}
    )
```

あとは，普通にPCLを使うのと同じ！