Kinect for Windows SDKのFaceTakingで取得した顔の座標を表示する(C++)

KinectSensor.h

// Thanks : http://www.codeproject.com/Articles/394975/How-To-Use-Kinect-Face-Tracking-SDK
#pragma once 

// NuiApi.hの前にWindows.hをインクルードする
#include <Windows.h>
#include <NuiApi.h>
#include <FaceTrackLib.h>



#define ERROR_CHECK( ret )  \
    if ( ret != S_OK ) {    \
    std::stringstream ss;	\
    ss << "failed " #ret " " << std::hex << ret << std::endl;			\
    throw std::runtime_error( ss.str().c_str() );			\
    }

const NUI_IMAGE_RESOLUTION CAMERA_RESOLUTION = NUI_IMAGE_RESOLUTION_640x480;

class KinectSensor
{
private:

    INuiSensor* kinect;
    HANDLE imageStreamHandle;
    HANDLE depthStreamHandle;
    HANDLE streamEvent;

    DWORD width;
    DWORD height;

    IFTFaceTracker* pFT;				// 顔追跡する人
    FT_CAMERA_CONFIG videoCameraConfig;	// RGBカメラの設定
    FT_CAMERA_CONFIG depthCameraConfig;	// 距離カメラの設定

    IFTResult* pFTResult;				// 顔追跡結果

    IFTImage* pColorFrame;				// 顔追跡用のRGBデータ
    IFTImage* pDepthFrame;				// 顔追跡用の距離データ

    FT_SENSOR_DATA sensorData;			// Kinectセンサーデータ

    std::vector<unsigned char> colorCameraFrameBuffer;
    std::vector<unsigned char> depthCameraFrameBuffer;

    bool isFaceTracked;
    RECT faceRect;

public:


    KinectSensor()
        : pFT( 0 )
        , pFTResult( 0 )
        , isFaceTracked( false )
    {
    }

    ~KinectSensor()
    {
        // 終了処理
        if ( kinect != 0 ) {
            kinect->NuiShutdown();
            kinect->Release();

            pFTResult->Release();
            pColorFrame->Release();
            pDepthFrame->Release();
            pFT->Release();
        }
    }

    void initialize()
    {
        createInstance();

        // Kinectの設定を初期化する
        ERROR_CHECK( kinect->NuiInitialize(
            NUI_INITIALIZE_FLAG_USES_COLOR |
            NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX |
            NUI_INITIALIZE_FLAG_USES_SKELETON ) );

        // RGBカメラを初期化する
        ERROR_CHECK( kinect->NuiImageStreamOpen( NUI_IMAGE_TYPE_COLOR, CAMERA_RESOLUTION,
            0, 2, 0, &imageStreamHandle ) );

        // 距離カメラを初期化する
        ERROR_CHECK( kinect->NuiImageStreamOpen( NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, CAMERA_RESOLUTION,
            0, 2, 0, &depthStreamHandle ) );

        // Nearモード
        ERROR_CHECK( kinect->NuiImageStreamSetImageFrameFlags(
            depthStreamHandle, NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE ) );

        // スケルトンを初期化する
        ERROR_CHECK( kinect->NuiSkeletonTrackingEnable( 0,
            NUI_SKELETON_TRACKING_FLAG_ENABLE_IN_NEAR_RANGE | 
            NUI_SKELETON_TRACKING_FLAG_ENABLE_SEATED_SUPPORT ) );

        // フレーム更新イベントのハンドルを作成する
        streamEvent = ::CreateEvent( 0, TRUE, FALSE, 0 );
        ERROR_CHECK( kinect->NuiSetFrameEndEvent( streamEvent, 0 ) );

        // 指定した解像度の、画面サイズを取得する
        ::NuiImageResolutionToSize(CAMERA_RESOLUTION, width, height );

        // 顔認識の初期化
        initializeFaceTracker();
    }

    void initializeFaceTracker()
    {
        // FaceTrackerのインスタンスを生成する
        pFT = ::FTCreateFaceTracker();
        if( !pFT ) {
            throw std::runtime_error( "ERROR:FTCreateFaceTracker" );
        }

        // RGBカメラおよび距離カメラの設定を行う
        videoCameraConfig.Width = width;
        videoCameraConfig.Height = height;
        videoCameraConfig.FocalLength = NUI_CAMERA_COLOR_NOMINAL_FOCAL_LENGTH_IN_PIXELS * (width / 640);

        depthCameraConfig.Width = width;
        depthCameraConfig.Height = height;
        depthCameraConfig.FocalLength = NUI_CAMERA_DEPTH_NOMINAL_FOCAL_LENGTH_IN_PIXELS * (width / 320);

        // FaceTrackerを初期化する
        HRESULT hr = pFT->Initialize( &videoCameraConfig, &depthCameraConfig, 0, 0) ;
        if( FAILED(hr) ) {
            throw std::runtime_error( "ERROR:Initialize" );
        }

        // FaceTrackerの結果を格納先を生成する
        hr = pFT->CreateFTResult( &pFTResult );
        if(FAILED(hr)) {
            throw std::runtime_error( "ERROR:CreateFTResult" );
        }

        // FaceTrackerで利用するRGBおよび距離データのインスタンスを生成する
        pColorFrame = FTCreateImage();
        pDepthFrame = FTCreateImage();
        if( !pColorFrame || !pDepthFrame ) {
            throw std::runtime_error( "ERROR:FTCreateImage" );
        }

        // RGBおよび距離データのバッファサイズを設定する
        // RGBは1pixelあたり4バイト。距離は1pixelあたり2バイト
        colorCameraFrameBuffer.resize( width*4 * height );
        depthCameraFrameBuffer.resize( width*2 * height );

        // フレームデータにバッファを設定する
        // You can also use Allocate() method in which case IFTImage interfaces own their memory.
        // In this case use CopyTo() method to copy buffers
        // CopyToでもOK?
        pColorFrame->Attach(width, height, &colorCameraFrameBuffer[0], FTIMAGEFORMAT_UINT8_B8G8R8X8, width*4);
        pDepthFrame->Attach(width, height, &depthCameraFrameBuffer[0], FTIMAGEFORMAT_UINT16_D13P3, width*2);

        // センサーデータを作成する
        sensorData.pVideoFrame = pColorFrame;
        sensorData.pDepthFrame = pDepthFrame;
        sensorData.ZoomFactor = 1.0f;			// Not used must be 1.0
        sensorData.ViewOffset.x = 0;			// Not used must be (0,0)
        sensorData.ViewOffset.y = 0;			// Not used must be (0,0)
    }

    void update()
    {
        // データの更新を待つ
        DWORD ret = ::WaitForSingleObject( streamEvent, INFINITE );
        ::ResetEvent( streamEvent );

        copyRgbImage();
        copyDepthImage();
        faceTracking();
    }

    const std::vector<unsigned char>& getColorFrameData() const
    {
        return colorCameraFrameBuffer;
    }

    const std::vector<unsigned char>& getDepthFrameData() const
    {
        return depthCameraFrameBuffer;
    }

    bool IsFaceTracked() const
    {
        return isFaceTracked;
    }

    const RECT& FaceRect()
    {
        return faceRect;
    }

    DWORD getWidth() const
    {
        return width;
    }

    DWORD getHeight() const
    {
        return height;
    }

private:

    void createInstance()
    {
        // 接続されているKinectの数を取得する
        int count = 0;
        ERROR_CHECK( ::NuiGetSensorCount( &count ) );
        if ( count == 0 ) {
            throw std::runtime_error( "Kinect を接続してください" );
        }

        // 最初のKinectのインスタンスを作成する
        ERROR_CHECK( ::NuiCreateSensorByIndex( 0, &kinect ) );

        // Kinectの状態を取得する
        HRESULT status = kinect->NuiStatus();
        if ( status != S_OK ) {
            throw std::runtime_error( "Kinect が利用可能ではありません" );
        }
    }

    void copyRgbImage()
    {
        // RGBカメラのフレームデータを取得する
        NUI_IMAGE_FRAME frame = { 0 };
        ERROR_CHECK( kinect->NuiImageStreamGetNextFrame( imageStreamHandle, INFINITE, &frame ) );

        // 画像データを取得する
        NUI_LOCKED_RECT data;
        frame.pFrameTexture->LockRect( 0, &data, 0, 0 );

        // 画像データをコピーする
        std::copy( data.pBits, data.pBits + data.size, colorCameraFrameBuffer.begin() );

        // フレームデータを解放する
        ERROR_CHECK( kinect->NuiImageStreamReleaseFrame( imageStreamHandle, &frame ) );
    }

    void copyDepthImage()
    {
        // RGBカメラのフレームデータを取得する
        NUI_IMAGE_FRAME frame = { 0 };
        ERROR_CHECK( kinect->NuiImageStreamGetNextFrame( depthStreamHandle, INFINITE, &frame ) );

        // 画像データを取得する
        NUI_LOCKED_RECT data;
        frame.pFrameTexture->LockRect( 0, &data, 0, 0 );

        // 画像データをコピーする
        std::copy( data.pBits, data.pBits + data.size, depthCameraFrameBuffer.begin() );

        // フレームデータを解放する
        ERROR_CHECK( kinect->NuiImageStreamReleaseFrame( depthStreamHandle, &frame ) );
    }

    void faceTracking()
    {
        // 追跡中、未追跡によって処理が変わる
        if(!isFaceTracked) {
            // FaceTrakingを開始する
            auto hr = pFT->StartTracking(&sensorData, NULL, NULL, pFTResult);
            if(SUCCEEDED(hr) && SUCCEEDED(pFTResult->GetStatus())) {
                // 顔を見つけたので、追跡状態へ遷移
                ::OutputDebugString( L"StartTracking\n" );
                isFaceTracked = true;

                // 顔の領域を取得する
                pFTResult->GetFaceRect( &faceRect );
            }
            else {
                // 顔を見失ったので、未追跡状態のまま
                isFaceTracked = false;
                ::OutputDebugString( L"StartTracking failed\n" );
            }
        }
        else {
            // FaceTrakingを継続する
            auto hr = pFT->ContinueTracking(&sensorData, NULL, pFTResult);
            if(SUCCEEDED(hr) && SUCCEEDED(pFTResult->GetStatus())) {
                // 顔を見つけたので、追跡状態のまま
                ::OutputDebugString( L"ContinueTracking\n" );

                // 顔の領域を取得する
                pFTResult->GetFaceRect( &faceRect );
            }
            else {
                // 顔を見失ったので、未追跡状態へ遷移
                ::OutputDebugString( L"ContinueTracking failed\n" );
                isFaceTracked = false;
            }
        }
    }
};

main.cpp

// NuiApi.hの前にWindows.hをインクルードする
#include <Windows.h>
#include <NuiApi.h>

#include "KinectSensor.h"

#include <opencv2/opencv.hpp>


void main()
{
    try {
        KinectSensor kinect;
        kinect.initialize();

        while ( 1 ) {
            kinect.update();

            cv::Mat image( kinect.getHeight(), kinect.getWidth(), CV_8UC4,
                            (void*)&kinect.getColorFrameData()[0] );

            if ( kinect.IsFaceTracked() ) {
                cv::rectangle(image,
                    cv::Point( kinect.FaceRect().left, kinect.FaceRect().top),
                    cv::Point( kinect.FaceRect().right, kinect.FaceRect().bottom),
                    cv::Scalar( 0,0,255 ) );
            }

            cv::imshow( "KinectSample", image );

            int c = cvWaitKey( 2 );
            if (c == '\x1b')
                break;
        }
    }
    catch ( std::exception& ex ) {
        std::cout << ex.what() << std::endl;
    }
}