Go to Qiita Advent Calendar 2024 Top
LoginSignup
15
13

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 5 years have passed since last update.

@SatoshiGachiFujimoto(藤本 賢志)

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

Last updated at Posted at 2015-01-30

KinectFusion

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

ソースコードのダウンロード

ソースコードは書籍「Kinect for Windows SDK 実践プログラミング」のサンプルプログラムからダウンロードする。書籍をぜひ買ってください。数少ない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;
}

できたソースコードはこちら。（Releseモードでしか実行できませんでした。）

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

snapshot00 - sample.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はこちら

15
13
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
15
13

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?