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倍ぐらいの大きさになります。

FastGlobalRegistrationのMatlabのMexファイルのコンパイル

Matlabのコマンドプロンプトから次のオプションを付けてコンパイルしてMEXファイルを作ります。

mex read_features.cpp -I../External/Eigen -I../External
mex fast_global_registration.cpp ../FastGlobalRegistration/app.cpp -I../External/Eigen -I../External -I../FastGlobalRegistration

f:id:cvl-robot:20170809161351p:plain