Skip to content

Instantly share code, notes, and snippets.

@kaorun55 kaorun55/KinectSensor.h Secret
Created Oct 2, 2012

Embed
What would you like to do?
Kinect for Windows SDKのFaceTakingで取得した顔の座標を表示する(C++)
// 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;
}
}
}
};
// 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;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.