[PCL] カスタム型 PointT を追加する


[Point Cloud Library (PCL)](https://pointclouds.org/) では様々な点の型`PointT`に対応するために、各種関数とクラスがテンプレートで実現されている。そこでカスタム型を追加する方法を検討しつつ PCL のお勉強をする。

基本的な内容な公式リファレンス[Adding your own custom PointT type](https://pcl-tutorials.readthedocs.io/en/latest/adding_custom_ptype.html#adding-custom-ptype)の和訳＋まとめ

[最終的なコードはこちら](#カスタム型の追加)


## 本当に必要か？
PCLには様々なフィールドをもつ点の型が多数用意されている。あなたが欲しいものが既にあるかも？[pcl/point_types.hpp](https://github.com/PointCloudLibrary/pcl/blob/master/common/include/pcl/impl/point_types.hpp)にすべての型が列挙されているので要チェック。

ヘッダの先頭あたりに定義されている `PCL_POINT_TYPES` がすべての型のリスト。ほかにもフィールドの種類に応じて、例えば色情報のRGBチャネルをもつ型の場合は `PCL_RGB_POINT_TYPES` という具合にまとめられている。

## フィールドと共用体

具体的な例をひとつ示して説明。
以下は `pcl::PointXYZRGB` の構造体のフィールド。

```c++
// xyz座標
union
{
    float data[4];
    struct
  {
    float x;
    float y;
    float z;
  };
};
// RGB色情報
union
{
  union
  {
    struct
    {
      std::uint8_t b;
      std::uint8_t g;
      std::uint8_t r;
      std::uint8_t a;
    };
    float rgb;
  };
  std::uint32_t rgba;
};
```
こんな感じで共用体を多数用いて同じメモリに複数の型・名前をつけている。
**注意** 
`Point***RGB` にはαチャネルも含まれている！  
pcdファイルにおいて色情報をR・G・Bの各チャネルごとに分けず一つの32bit符号なし整数"rgba"で表現しているのはここの`std::uint32_t rgba`に由来している。もっともチャネルはR,G,B,Aの順番ではない。バイトオーダーにも依るから、Little Endian: "argb", Big Endian: "bgra" といった具合か。

### なぜxyzに`float`が４つ必要？
xyz座標を表現するならfloat３つで十分なのに`float data[4]`ではひとつ余分では？と当然の疑問が湧いてくる。例えば、xyz座標＋強度"intensity"（float×1）を表現する型`pcl::PointXYZI`を考えると、

```c++
union
{
    float data[4];
    struct
    {
        float x;
        float y;
        float z;
        float intensity;
    };
}
```

と16byte（float×4）で十分に思われる。しかし、実際の定義はこうだ。

```c++
union
{
  float data[4];
  struct
  {
    float x;
    float y;
    float z;
  };
};
union
{
  struct
  {
    float intensity;
  };
  float data_c[4];
};
```
メモリの無駄遣いにしか思えないが、公式のお言葉曰く、
> Simple XYZ + intensity point type. In an ideal world, these 4 components would create a single structure, SSE-aligned. However, because the majority of point operations will either set the last component of the data[4] array (from the xyz union) to 0 or 1 (for transformations), we cannot make intensity a member of the same structure, as its contents will be overwritten. For example, a dot product between two points will set their 4th component to 0, otherwise the dot product doesn’t make sense, etc.
> Therefore for SSE-alignment, we pad intensity with 3 extra floats. Inefficient in terms of storage, but good in terms of memory alignment.

要するに`data[3]`も座標計算の過程で使うから他の値を保持するな、とのこと。また、16Byte単位だとメモリアライメントの観点から有利だそう。そのため、謎のパディングが生じるわけだ。


## 型の宣言の実際
実際のヘッダファイルでは複数のマクロを用いているため見た目はだいぶ異なる。

```c++
 struct EIGEN_ALIGN16 _PointXYZRGB
{
    PCL_ADD_POINT4D; // xyz座標
    PCL_ADD_RGB; // RGB色情報
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
```
xyz座標やRGB色情報など主要なフィールドに関しては、前項で紹介した面倒な宣言の代わりにマクロが多用されている。`PCL_ADD_POINT4D`, `PCL_ADD_RGB`はプリプロセッサで展開されると先ほどと同じフィールドの宣言になる。
`EIGEN_ALIGN16`は対象がメモリ上で16Byte単位になるよう適宜パディングするマクロのようだ。
`EIGEN_MAKE_ALIGNED_OPERATOR_NEW`に関しては、
> make sure our new allocators are aligned

と説明があるからリファレンス通り記述しておこう。  


最後にコンストラクタを定義してpcl::PointXYZRGBの完成だ。
`PointXYZRGB()`,`PointXYZRGB(const _PointXYZRGB&)`のふたつは必須のようだ。

```c++
struct EIGEN_ALIGN16 PointXYZRGB : public _PointXYZRGB
  {
    inline PointXYZRGB (const _PointXYZRGB &p)
    {
      x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
      rgb = p.rgb;
    }

    inline PointXYZRGB ()
    {
      x = y = z = 0.0f;
      data[3] = 1.0f;
      r = g = b = 0;
      a = 255;
    }
    inline PointXYZRGB (uint8_t _r, uint8_t _g, uint8_t _b)
    {
      x = y = z = 0.0f;
      data[3] = 1.0f;
      r = _r;
      g = _g;
      b = _b;
      a = 255;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
```

## テンプレート関数・クラスの実体化
これで独自の点の型を定義できた。しかし、実際に使いたい場面とは、点群をこの独自型として扱いながらPCLの各種関数・クラスを使用することのはずだ。例えば点群を扱ううえで基本となる`pcl::PointCloud<PointT>`で`PointT`を独自型として使いたい！ところで、PCLライブラリではほとんどすべての関数・クラスはテンプレートで実装されている。これらを独自型で実体化するには？
  
マクロ`POINT_CLOUD_REGISTER_POINT_STRUCT`を使う！   
`pcl::PointXYGRGB`で例を示すと、

```c++
POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGB,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (uint32_t, rgba, rgba)
)
```

第一引数は対象となる型`pcl::_PointXYZRGB`を指定。第二引数には各フィールドの`(datatype,accessor,tag)`のタプルを列挙する。

- accessor: 対象のフィールドへのアクセス方法。配列の場合でもOK`(float, x, x) => (float, data[0], x)`
- tag: 対象のフィールドの名前。pcdファイルに書き出す時の`FIELDS`欄に現れる。

**注意**  
> Must be used in global namespace with name fully qualified  

と注意書きがあるとおり、このマクロはグローバルスコープで用いること。


## カスタム型の追加
例として xyz座標とカスタムフィールド×2(int, unsigned int)を持つ独自型`PointHoge`を定義して使ってみよう。

```c++
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>

struct PointHoge {
	PCL_ADD_POINT4D;
	uint32_t hoge;
	int piyo;

	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	inline PointHoge() {
		x = y = z = 0;
		data[3] = 0;
		hoge = 0;
		piyo = 0;
	}
	inline PointHoge(const PointHoge& p) {
		x = p.x;
		y = p.y;
		z = p.z;
		hoge = p.hoge;
		piyo = p.piyo;
	}
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(
	PointHoge,
	(float, x, x)
	(float, y, y)
	(float, z, z)
	(uint32_t, hoge, hoge)
	(int, piyo, piyo)
)

// test unit
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>

int main(void) {

	pcl::PointCloud<PointHoge>::Ptr cloud(
		new pcl::PointCloud<PointHoge>()
	);

	PointHoge hoge;
	hoge.hoge = 1;
	hoge.piyo = -10;

	cloud->points.push_back(hoge);
	cloud->is_dense = true;
	cloud->width = cloud->points.size();
	cloud->height = 1;

	pcl::io::savePCDFileASCII<PointHoge>("hoge.pcd", *cloud);

}
```

これを実行すると次のようなpcdファイルが出力される

```text
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z hoge piyo
SIZE 4 4 4 4 4
TYPE F F F U I
COUNT 1 1 1 1 1
WIDTH 1
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 1
DATA ascii
0 0 0 1 -10
```