ブログ@kaorun55

HoloLensやKinectなどのDepthセンサーを中心に書いています。

Kinect for Windows SDK v1.7 の KinectFusionを使ってみる(C++)

C#でのKinectFusionに続いてC++でもやってみます。こちらも基本的な流れは同じです。

全体のソースはこちらにあります。

必要なもの

KinectFusionを扱うためには次のライブラリが必要です。

次のライブラリをリンクさせます。amd64x86はビルド方法に合わせます。

  • C:\Program Files\Microsoft SDKs\Kinect\Developer Toolkit v1.7.0\lib\amd64\KinectFusion170_64.lib
  • C:\Program Files\Microsoft SDKs\Kinect\Developer Toolkit v1.7.0\lib\x86\KinectFusion170_32.lib

次のDLLを実行ファイルの場所にコピーします。

  • C:\Program Files\Microsoft SDKs\Kinect\Developer Toolkit v1.7.0\Redist\amd64\KinectFusion170_64.dll
  • C:\Program Files\Microsoft SDKs\Kinect\Developer Toolkit v1.7.0\Redist\x86\KinectFusion170_32.dll

コード

必要な変数

KinectFusionに最低限必要な変数は次の4つです。

  • PointCloudを構築するReconstructionのvolume(INuiFusionReconstruction)
  • Depthデータから変換するためのNUI_FUSION_IMAGE_FRAME
  • PointCloudのバッファのNUI_FUSION_IMAGE_FRAME
  • PointCloudをBitmap化するためのNUI_FUSION_IMAGE_FRAME

それぞれのインスタンスを生成し、一度リセットします。

KinectFusionの初期化(initializeKinectFusion)

volumeのインスタンス生成には ::NuiFusionCreateReconstruction() を使います。5つの引数を与えるようになっており、それぞれは次のようになっています。

  • NUI_FUSION_RECONSTRUCTION_PARAMETERS*

    • 構築するときのVoxelについてのパラメータ。サンプルの既定値を使用しています。

  • NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE

    • NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_AMPを選択するとGPUでの処理。
    • NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_CPUを選択するとCPUでの処理。
    • CPUを選択した場合リアルタイムにの処理は難しくなります。

  • int

    • NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPEにAMPを選択した場合に設定します。AMPで使用するプロセッサを0から始まるインデックスで設定します。自動で選択させる場合には-1を指定します。

  • Matrix4*

    • カメラ座標系への変換行列です。

  • INuiFusionReconstruction**

    • 生成されたvolumeのインスタンスを格納するポインタのポインタです。

NUI_FUSION_RECONSTRUCTION_PARAMETERS reconstructionParams;

reconstructionParams.voxelsPerMeter = 256;// 1000mm / 256vpm = ~3.9mm/voxel

reconstructionParams.voxelCountX = 512; // 512 / 256vpm = 2m wide reconstruction

reconstructionParams.voxelCountY = 384; // Memory = 512*384*512 * 4bytes per voxel

reconstructionParams.voxelCountZ = 512; // This will require a GPU with at least 512MB

// Reconstruction Volume のインスタンスを生成

hr = ::NuiFusionCreateReconstruction(

&reconstructionParams,

NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_AMP, // NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_CPU

-1, &IdentityMatrix(), &m_pVolume);

if (FAILED(hr)){

throw std::runtime_error( "::NuiFusionCreateReconstruction failed." );

}

バッファのインスタンスを生成し、volumeをリセットします。

// DepthFloatImage のインスタンスを生成

hr = ::NuiFusionCreateImageFrame(NUI_FUSION_IMAGE_TYPE_FLOAT, width, height, nullptr, &m_pDepthFloatImage);

if (FAILED(hr)) {

throw std::runtime_error( "::NuiFusionCreateImageFrame failed(Float)." );

}

// PointCloud のインスタンスを生成

hr = ::NuiFusionCreateImageFrame(NUI_FUSION_IMAGE_TYPE_POINT_CLOUD, width, height, nullptr, &m_pPointCloud);

if (FAILED(hr)) {

throw std::runtime_error( "::NuiFusionCreateImageFrame failed(PointCloud)." );

}

// シェーダーサーフェースのインスタンスを生成

hr = ::NuiFusionCreateImageFrame(NUI_FUSION_IMAGE_TYPE_COLOR, width, height, nullptr, &m_pShadedSurface);

if (FAILED(hr)) {

throw std::runtime_error( "::NuiFusionCreateImageFrame failed(Color)." );

}

// リセット

m_pVolume->ResetReconstruction( &IdentityMatrix(), nullptr );

KinectFusionの処理(processKinectFusion)

KinectFusionの処理を行います。KinetFusionへは拡張されたDepthデータを渡します。

手順は大まかに次の5つです。

  1. DepthデータをDepthFloatFrameに変換する
  2. DepthFloatFrameを処理する
  3. PointCloudを取得する
  4. PointCloudを2次元画像化する
  5. 画像データを表示する

DepthデータをDepthFloatFrameに変換する

DepthImagePixel から DepthFloaatFrame に変換します。変換には ::NuiFusionDepthToDepthFloatFrame() を使います。

// DepthImagePixel から DepthFloaatFrame に変換する

HRESULT hr = ::NuiFusionDepthToDepthFloatFrame( depthPixel, width, height, m_pDepthFloatImage,

NUI_FUSION_DEFAULT_MINIMUM_DEPTH, NUI_FUSION_DEFAULT_MAXIMUM_DEPTH, TRUE );

if (FAILED(hr)) {

throw std::runtime_error( "::NuiFusionDepthToDepthFloatFrame failed." );

}

DepthFloatFrameを処理する

DepthFloatFrame を処理します。ここで処理が成功すればそれまでのデータをPointCloudとして取得することができます。KinectFusionはDepthのデータを連続的に取得して精度を上げているため、Kinectまたは対象物が大きく移動すると構築できなくなります。そこで一定回数以上エラーになった場合には構築をリセットします。

hr = m_pVolume->ProcessFrame( m_pDepthFloatImage, NUI_FUSION_DEFAULT_ALIGN_ITERATION_COUNT,

NUI_FUSION_DEFAULT_INTEGRATION_WEIGHT, &worldToCameraTransform );

if (FAILED(hr)) {

// 一定数エラーになったらリセット

// Kinectまたは対象を素早く動かしすぎ などの場合

++trackingErrorCount;

if ( trackingErrorCount >= 100 ) {

trackingErrorCount = 0;

m_pVolume->ResetReconstruction( &IdentityMatrix(), nullptr );

}

return;

}

PointCloudを取得する

PointCloudを取得し、2次元の画像データに変換します。

// PointCloudを取得する

hr = m_pVolume->CalculatePointCloud( m_pPointCloud, &worldToCameraTransform );

if (FAILED(hr)) {

throw std::runtime_error( "CalculatePointCloud failed." );

}

// PointCloudを2次元のデータに描画する

hr = ::NuiFusionShadePointCloud( m_pPointCloud, &worldToCameraTransform,

nullptr, m_pShadedSurface, nullptr );

if (FAILED(hr)) {

throw std::runtime_error( "::NuiFusionShadePointCloud failed." );

}

画像データを表示する

画像データを表示できる形式に変換します。ここではOpenCVの形にしています。

// 2次元のデータをBitmapに書きだす

INuiFrameTexture * pShadedImageTexture = m_pShadedSurface->pFrameTexture;

NUI_LOCKED_RECT ShadedLockedRect;

hr = pShadedImageTexture->LockRect(0, &ShadedLockedRect, nullptr, 0);

if (FAILED(hr)) {

throw std::runtime_error( "LockRect failed." );

}

mat = cv::Mat( height, width, CV_8UC4, ShadedLockedRect.pBits );

// We're done with the texture so unlock it

pShadedImageTexture->UnlockRect(0);