cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

FPFH特徴量をbinaryファイルに出力

FastGlobalRegistrationのテストデータを自分で作りたいと思います。
github.com
PCLに慣れている人ならば問題ないのでしょうが、上記サイトにはFPFH特徴量を出力するサンプルコードの抜粋しか載っていません。
欠けている部分を他のプログラムから探してきて、動くようにしたいと思います。

1.FPFH特徴量のサンプルプログラム

PCD形式のデータからFPFH特徴量を計算するサンプルプログラムを探すと、下記リンクに奇麗なコードがありました。
PCL/OpenNI tutorial 4: 3D object recognition (descriptors) - robotica.unileon.es

2.PCL1.8のインストー

下記サイトで提供してくださっているinstallerを使って、PCLをインストールします。インストーラに素直に従えば大丈夫です。
Point Cloud Library 1.8.0 has been released – Summary?Blog
3rd partyフォルダの下にあるopenNIのインストールも行ってください。

3.CMakeLists.txtの準備

適当にプロジェクト用のフォルダを作ってください。ここでは、

E://workspace/pcl/FPFHbinary

とします。このフォルダにCMakeLists.txtというファイルを作り、下記の例のようにします。

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(FPFH_BINARY)
set(PCL_DIR "E:/Program Files/PCL 1.8.0/cmake/PCLConfig.cmake")
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(fpfh_binary fpfh_binary.cpp)
target_link_libraries(fpfh_binary ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_FEATURES_LIBRARIES} ${PCL_SEARCH_LIBRARIES})

適当にプロジェクト名、およびファイル名はfpfh_binaryと命名することにします。
target_link_librariesに、common, io, features, searchを追加します。

4.fpfh_binary.cppの準備

githubのサンプルコードと、チュートリアルのサンプルコードを合体させて動くようにします。
githubの方はPointNormal型を入力データとしているので、チュートリアルの方のPointXYZとNormalを別々に扱う形式に変更します。

#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh_omp.h>

int
main(int argc, char** argv)
{
	// Object for storing the point cloud.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	// Object for storing the normals.
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	// Object for storing the FPFH descriptors for each point.
	// pcl::PointCloud<pcl::FPFHSignature33>::Ptr descriptors(new pcl::PointCloud<pcl::FPFHSignature33>());

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Note: you would usually perform downsampling now. It has been omitted here
	// for simplicity, but be aware that computation can take a long time.

	// Estimate the normals.
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
	normalEstimation.setInputCloud(cloud);
	normalEstimation.setRadiusSearch(0.03);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	normalEstimation.setSearchMethod(kdtree);
	normalEstimation.compute(*normals);

	// Assume a point cloud with normal is given as
	// pcl::PointCloud<pcl::PointNormal>::Ptr object

	pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fest;
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr object_features(new pcl::PointCloud<pcl::FPFHSignature33>());
	fest.setRadiusSearch(0.03); 
	fest.setInputCloud(cloud);
	fest.setInputNormals(normals);
	fest.compute(*object_features);

	// FILE* fid = fopen("features.bin", "wb");
	FILE* fid = fopen(argv[2], "wb");
	int nV = cloud->size(), nDim = 33;
	fwrite(&nV, sizeof(int), 1, fid);
	fwrite(&nDim, sizeof(int), 1, fid);
	for (int v = 0; v < nV; v++) {
		const pcl::PointXYZ &pt = cloud->points[v];
		float xyz[3] = { pt.x, pt.y, pt.z };
		fwrite(xyz, sizeof(float), 3, fid);
		const pcl::FPFHSignature33 &feature = object_features->points[v];
		fwrite(feature.histogram, sizeof(float), 33, fid);
	}
	fclose(fid);
}

5. テストデータの準備

適当にpcdデータを用意します。FPFH特徴量の計算は比較的速いのですが、FastGlobalRegistrationでは大きなデータはとんでもなく長い時間がかかるので、スタンフォードバニー辺りでテストしておくと良いです。
sourceforge.net

FPFHの出力は、コマンドプロンプトで次の例のようにします。第一引数が入力pcdで、第二引数が出力です。

fpfh_binary.exe scan_000.pcd scan_000.bin

ファイルサイズは入力の100倍ぐらいの大きさになります。

今日のアニメ化待ち漫画

邪神ちゃんドロップキック、アニメになったら絶対かわいい。