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.
You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.