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

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

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

https://github.com/kaorun55/KinectSDKv17Sample

必要なもの

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

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

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つです。

DepthデータをDepthFloatFrameに変換する

DepthFloatFrameを処理する

PointCloudを取得する

PointCloudを2次元画像化する

画像データを表示する

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);