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 実践プログラミング」のサンプルプログラムからダウンロードする。書籍をぜひ買ってください。数少ない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を同時に開き、見てみると、

こんな感じで、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はこちら