LoginSignup
6
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@Seo-4d696b75

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

Posted at

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

基本的な内容な公式リファレンスAdding your own custom PointT typeの和訳＋まとめ

最終的なコードはこちら

本当に必要か？

PCLには様々なフィールドをもつ点の型が多数用意されている。あなたが欲しいものが既にあるかも？pcl/point_types.hppにすべての型が列挙されているので要チェック。

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

フィールドと共用体

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

// 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を考えると、

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

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

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単位だとメモリアライメントの観点から有利だそう。そのため、謎のパディングが生じるわけだ。

型の宣言の実際

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

 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&)のふたつは必須のようだ。

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で例を示すと、

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を定義して使ってみよう。

#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ファイルが出力される

# .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
6
3
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
6
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?