バイナリ形式のpcd形式のファイルを出力する

仕事でポイントクラウドのデータを扱う必要が出てきて、PCLライブラリを入れたのですが、boostやvtkなどの謎のライブラリを大量に入れる必要があったり、コンパイルが重いので困りました。今のところ簡単な処理を行うだけなので、点群を処理して結果をpcd形式(point cloud dataフォーマット)に出力して、pcdビューワ(CloudCompare; http://www.danielgm.net/cc/) で見ることにしました。

pcdのフォーマット：
http://pointclouds.org/documentation/tutorials/pcd_file_format.php

これを見ると単純な形式だったので、PCL使わずにpcdを出力できそうです。ただし、アスキー形式だと読み込みが遅かったのでバイナリ形式で出力します。とはいっても、ヘッダはアスキー形式と同じで、データの部分からバイナリになっているだけです。

以下、適当に点群を作ってpcdファイルに出力するコードです。

c++-main.cpp

#include <stdio.h>
#include <vector>

struct pt_t {
	float x, y, z;
	pt_t() { ; }
	pt_t(float _x, float _y, float _z) {
		x = _x;
		y = _y;
		z = _z;
	}
};

#define RGB(r, g, b) ((int(r) << 16) + (int(g) << 8) + int(b) )


void main(){

	//***********************************************************
	// 適当な点群リストを作成
	std::vector<pt_t> pts;

	pt_t pt(7,23,35);
	float s = 10, b = 1.8, r = 20, dt = 0.0001;

	for (int i = 0; i < 500000; i++) {
		pt = pt_t(
			pt.x - dt * s*(pt.x - pt.y),
			pt.y + dt*(-pt.x*pt.z + r*pt.x - pt.y),
			pt.z + dt*(pt.x*pt.y - b*pt.z)
		);
		pts.push_back(pt);
	}
	//***********************************************************
	// pcdファイル（バイナリ形式）に出力
	FILE *fp = fopen("test.pcd", "wb");
	// ヘッダ
	fprintf(fp, "# .PCD v.7 - Point Cloud Data file format\n");
	fprintf(fp, "VERSION .7\nFIELDS x y z rgb\nSIZE 4 4 4 4\nTYPE F F F U\nCOUNT 1 1 1 1\n");
	fprintf(fp, "WIDTH %d\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0\nPOINTS %d\nDATA binary\n", pts.size(), pts.size());
	// 中身をバイナリで書き込み
	for (int i = 0; i < pts.size(); i++) {
		// 適当に色を付ける
		unsigned int col = RGB(pts[i].z * 5, 128 + 3*pts[i].x, 128 + 3 * pts[i].y);
		float xyz[3] = { pts[i].x, pts[i].y, pts[i].z};
		fwrite( xyz, sizeof(float), 3, fp);
		fwrite(&col, sizeof(unsigned int), 1, fp);
	}
	fclose(fp);

}
	

500000点ありますが、サクサク動きます。

点群の曲線式は以下を参考にしました。
https://sites.google.com/site/cinderellajapan/shindererarekucha-1/ka-osu