Kinect v1のKinectFusionから自己位置を得る方法（Visual Studio 2013, OpenCV, C++, Kinect v1）

# KinectFusion
KinectFusionとは、Kinectを用いてSLAM（Simultaneous Localization and Mapping）：自己位置推定と地図作成を同時に行う技術である。
Kinect Developer Toolkit 1.8でも体験できるが、実際にソースコードをいじって、Kinectを動かした位置を得てみよう。

# ソースコードのダウンロード
ソースコードは[書籍「Kinect for Windows SDK 実践プログラミング」のサンプルプログラム](https://github.com/UnaNancyOwen/KinectBook2)からダウンロードする。書籍をぜひ買ってください。数少ないKinect v1 C++編の良書です。

- KinectFusion.cpp　・・・メイン
- KinectFusionHelper.cpp　・・・関数
- KinectFusionHelper.h　・・・関数の定義

# 自己位置の表示

メインのプログラムを見てみよう。
自己位置が格納されている変数はworldToCameraTransformである。
worldToCameraTransformは4×4の行列になっている。

- worldToCameraTransform.M41
- worldToCameraTransform.M42
- worldToCameraTransform.M43

がそれぞれ自己位置の（X,Y,Z）を表している。単位は[m]。

コマンドラインに表示する場合、3次元形状の再構成処理の下に、

~~~
std::cout << "T = " << worldToCameraTransform.M41 << " " << worldToCameraTransform.M42 << " " << worldToCameraTransform.M43 << std::endl;
~~~

を追加する。

# 自己位置の保存

毎フレームごとの自己位置を保持していくために、mainの最初に、

~~~
std::vector<Matrix4> worldToCameraTransformSeq;
~~~

を定義する。

3次元形状の再構成処理の下に、


~~~
worldToCameraTransformSeq.push_back(worldToCameraTransform);
~~~

を追加し、自己位置をプッシュバックしていく。

3次元再構築のエラーが多い場合、マップがリセットされるため、保持してきた自己位置もリセットしよう。

~~~
ResetReconstruction(pReconstruction, &worldToCameraTransform);
~~~

の下に、リサイズで初期化する。

~~~
worldToCameraTransformSeq.resize(0);
~~~

's'キーで再構築したマップ(mesh.ply)を保存できる。これと一緒に自己位置も点群として保存しよう。KinectFusion.cppの中、メッシュファイルの保存(Sキー)の中に、

~~~
std::cout << "Save Camera Pose" << std::endl;
fileName = "CameraPose.ply";
WriteAsciiPlyCameraPoseFile(&worldToCameraTransformSeq, fileName, true, true);
~~~

を追加して、CameraPose.plyファイルを作成する。
実際にファイルを生成する関数はWriteAsciiPlyCameraPoseFileである。
KinectFusionHelper.hの中にWriteAsciiPlyCameraPoseFileを定義する。

~~~
/// <summary>
/// Write ASCII .PLY file
/// See http://paulbourke.net/dataformats/ply/ for .PLY format
/// </summary>
/// <param name="worldToCameraTransformSeq">The Kinect Fusion Camera Transform Matrix.</param>
/// <param name="lpOleFileName">The full path and filename of the file to save.</param>
/// <param name="flipYZ">Flag to determine whether the Y and Z values are flipped on save.</param>
/// <param name="outputColor">Set this true to write out the camera pose color to the file when it has been captured.</param>
/// <returns>indicates success or failure</returns>
/*HRESULT WriteAsciiPlyCameraPoseFile(std::vector<Matrix4> *worldToCameraTransformSeq, LPOLESTR lpOleFileName, bool flipYZ, bool outputColor)*/
HRESULT WriteAsciiPlyCameraPoseFile(std::vector<Matrix4> *worldToCameraTransformSeq, char* lpOleFileName, bool flipYZ, bool outputColor);
~~~

KinectFusionHelper.cppの中にWriteAsciiPlyCameraPoseFileの中身を記述する。

~~~
/// <summary>
/// Write ASCII .PLY file
/// See http://paulbourke.net/dataformats/ply/ for .PLY format
/// </summary>
/// <param name="worldToCameraTransformSeq">The Kinect Fusion Camera Transform Matrix.</param>
/// <param name="lpOleFileName">The full path and filename of the file to save.</param>
/// <param name="flipYZ">Flag to determine whether the Y and Z values are flipped on save.</param>
/// <param name="outputColor">Set this true to write out the camera pose color to the file when it has been captured.</param>
/// <returns>indicates success or failure</returns>
/*HRESULT WriteAsciiPlyCameraPoseFile(std::vector<Matrix4> *worldToCameraTransformSeq, LPOLESTR lpOleFileName, bool flipYZ, bool outputColor)*/
HRESULT WriteAsciiPlyCameraPoseFile(std::vector<Matrix4> *worldToCameraTransformSeq, char* lpOleFileName, bool flipYZ, bool outputColor)
{
	HRESULT hr = S_OK;

	If (NULL == worldToCameraTransformSeq)
	{
		return E_INVALIDARG;
	}

	unsigned int numVertices = worldToCameraTransformSeq->size();
	
	if (0 == numVertices)
	{
		return E_INVALIDARG;
	}

	// Open File
	/*USES_CONVERSION;*/
	char* pszFileName = NULL;

	try
	{
		/*pszFileName = OLE2A(lpOleFileName);*/
		pszFileName = lpOleFileName;

	}
	catch (...)
	{
		return E_INVALIDARG;
	}

	FILE *cameraPoseFile = NULL;
	errno_t err = fopen_s(&cameraPoseFile, pszFileName, "wt");

	// Could not open file for writing - return
	if (0 != err || NULL == cameraPoseFile)
	{
		return E_ACCESSDENIED;
	}

	// Write the header line
	std::string header = "ply\nformat ascii 1.0\ncomment file created by Microsoft Kinect Fusion\n";
	fwrite(header.c_str(), sizeof(char), header.length(), cameraPoseFile);

	const unsigned int bufSize = MAX_PATH * 3;
	char outStr[bufSize];
	int written = 0;

	if (outputColor)
	{
		// Elements are: x,y,z, r,g,b
		written = sprintf_s(outStr, bufSize, "element vertex %u\nproperty float x\nproperty float y\nproperty float z\nproperty uchar red\nproperty uchar green\nproperty uchar blue\n", numVertices);
		fwrite(outStr, sizeof(char), written, cameraPoseFile);
	}
	else
	{
		// Elements are: x,y,z
		written = sprintf_s(outStr, bufSize, "element vertex %u\nproperty float x\nproperty float y\nproperty float z\n", numVertices);
		fwrite(outStr, sizeof(char), written, cameraPoseFile);
	}

	written = sprintf_s(outStr, bufSize, "end_header\n");
	fwrite(outStr, sizeof(char), written, cameraPoseFile);

	if (flipYZ)
	{
		if (outputColor)
		{
			//red color camera pose x -y -z
			std::vector<Matrix4>::iterator begin = worldToCameraTransformSeq->begin();
			std::vector<Matrix4>::iterator end = worldToCameraTransformSeq->end();

			for (; begin != end; begin++){
				written = sprintf_s(outStr, "%lf %lf %lf 255 0 0\n", begin->M41, -begin->M42, -begin->M43);
				fwrite(outStr, sizeof(char), written, cameraPoseFile);
			}
		}
		else
		{
			//camera pose
			std::vector<Matrix4>::iterator begin = worldToCameraTransformSeq->begin();
			std::vector<Matrix4>::iterator end = worldToCameraTransformSeq->end();

			for (; begin != end; begin++){
				written = sprintf_s(outStr, "%lf %lf %lf\n", begin->M41, -begin->M42, -begin->M43);
				fwrite(outStr, sizeof(char), written, cameraPoseFile);
			}
		}
	}
	else
	{
		if (outputColor)
		{
			//read color camera pose
			std::vector<Matrix4>::iterator begin = worldToCameraTransformSeq->begin();
			std::vector<Matrix4>::iterator end = worldToCameraTransformSeq->end();

			for (; begin != end; begin++){
				written = sprintf_s(outStr, "%lf %lf %lf 255 0 0\n", begin->M41, begin->M42, begin->M43);
				fwrite(outStr, sizeof(char), written, cameraPoseFile);
			}
		}
		else
		{
			//camera pose
			std::vector<Matrix4>::iterator begin = worldToCameraTransformSeq->begin();
			std::vector<Matrix4>::iterator end = worldToCameraTransformSeq->end();

			for (; begin != end; begin++){
				written = sprintf_s(outStr, "%lf %lf %lf\n", begin->M41, begin->M42, begin->M43);
				fwrite(outStr, sizeof(char), written, cameraPoseFile);
			}
		}
	}

	fflush(cameraPoseFile);
	fclose(cameraPoseFile);

	return hr;
}
~~~

できたソースコードは[こちら](https://github.com/SatoshiRobatoFujimoto/KinectFusionV1/tree/master/KinectFusionV1)。（Releseモードでしか実行できませんでした。）

実行して、's'キーで保存し、'esc'キーで終了。
MeshLabでmesh.plyとCameraPose.plyを同時に開き、見てみると、

![snapshot00 - sample.png](https://qiita-image-store.s3.amazonaws.com/0/63863/0802eac5-f063-0104-f98c-729edbb57d39.png)

こんな感じで、Kinectカメラが動いた位置が赤い点群で表示されているのがわかる。

※Kinect XBOX360で確認する際に、一部エラーが発生したので、以下の部分をコメントアウト&変更した。Releseモードでしか実行できませんでした。

~~~
// Near Modeの設定 XBOX360ではできない
//hResult = pSensor->NuiImageStreamSetImageFrameFlags(hDepthPlayerHandle, NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE);
//if (FAILED(hResult)){
//	std::cerr << "Error : NuiImageStreamSetImageFrameFlags" << std::endl;
//	return -1;
//}
~~~
~~~
// 使用可能なメモリ量
UINT memorySize = reconstructionParameter.voxelCountX * reconstructionParameter.voxelCountY * reconstructionParameter.voxelCountZ * 4 / 1024;//だいたい4GB

//XBOX360でエラー
//hResult = NuiFusionGetDeviceInfo(NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_AMP/*NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_CPU*/, -1, nullptr, 0, nullptr, 0, &memorySize);
//if (FAILED(hResult)){
//	std::cerr << "Error : NuiFusionGetDeviceInfo" << std::endl;
//	return -1;
//}
~~~

Kinect v2によるKinectFusionは[こちら](http://qiita.com/SatoshiRobatoFujimoto/items/68da949478dd86a99905)