Kinect SDKのソースコードを書き下した。
KinectSDKを使ってC++ソースコードを書いたので紹介します。使いたい方はご自由にどうぞ
- 概要
Kinectの深度画像、カメラ画像、スケルトン情報等を取得するKinectSDKのラッパーです。
Windows 7 + MS Visual Studio C++ 2010 expressで開発しました。
OpenCVをよく使うのでC/C++で書きました。
音源方位、音声認識は今後対応予定。
CKinectUtilというクラスをmain()から使うプログラムを以下に載せます。スケルトンを表示する部分は省いてます。
LogCommon.hはオレオレ用ログです。消してご利用ください。
- KinectUtil.h
/**
* @file KinectUtil.h
* Kinect for Windows SDK v1.0
* OpenCV v2.3.1
*/
#pragma once
#include <opencv/cv.h>
#include <memory>
#include <windows.h>
#include <NuiApi.h>
#include <vector>
namespace Utility
{
/**
* @brief 深度画像上の座標情報
*/
typedef class CDepthPoint
{
public:
float fX;
float fY;
USHORT usDepth;
static const int CAP_WIDTH = 320;
static const int CAP_HEIGHT = 240;
public:
CDepthPoint();
CDepthPoint(float x, float y, USHORT depth);
virtual ~CDepthPoint();
CDepthPoint(const CDepthPoint& obj);
} CDptPoint;
/**
* @brief トラッキング情報
*/
class CTrackingInfo
{
public:
// ID
long lTrackingID;
// 各関節点座標
std::vector<CDptPoint> skeletalPoints;
public:
CTrackingInfo();
virtual ~CTrackingInfo();
CTrackingInfo(const CTrackingInfo& obj);
};
/**
* @class CKinectController
* @brief Kinect制御
* OpenCV型依存
*/
typedef class CKinectController
{
public:
//---------------------------------------------------------
// API
//---------------------------------------------------------
/**
* @brief コンストラクタ
*/
CKinectController();
/**
* @brief デストラクタ
*/
virtual ~CKinectController();
/**
* @brief 初期化
* キャプチャを開始する
* @return 成功:0 / 失敗:0以外
*/
long Initialize();
/**
* @brief 終了処理
* キャプチャを終了する
* @return 成功:0 / 失敗:0以外
*/
long Finalize();
/**
* @brief 次フレームを取得する
* @return 成功:0 / 失敗:0以外
*/
long GrabFrame();
/**
* @brief Depth画像を取得する
* 1チャンネル uint16幅
* @return 成功:0 / 失敗:0以外
*/
long GetDepthFrame(IplImage*& pImage);
/**
* @brief RGB画像を取得する
* 4チャンネル uchar幅
* @return 成功:0 / 失敗:0以外
*/
long GetRGBFrame(IplImage*& pImage);
/**
* @brief トラッキングID分布画像を取得
* @return 成功:0 / 失敗:0以外
*/
long GetTrakingIdFrame(IplImage*& pImage);
/**
* @brief トラッキング情報を取得
* @return 成功:0 / 失敗:0以外
*/
long GetTrakingInfo(std::vector<CTrackingInfo>& trackingInfoList);
/**
* @brief ピッチ駆動
* @return 成功:0 / 失敗:0以外
*/
long SetElevationAngle(float fAgl);
/**
* @brief ピッチ現在角度を取得
* @return 成功:0 / 失敗:0以外
*/
long GetElevationAngle(float& fAgl);
public:
static long Depth16bitTo8bitImage(const IplImage* srcImage, IplImage*& dstImage);
protected:
//---------------------------------------------------------
// 操作
//---------------------------------------------------------
// 次フレーム取得
long UpdateFrame();
// 現フレーム破棄
long ReleaseFrame();
// Depth画像バッファリング
long BufferDepthImage(const NUI_IMAGE_FRAME* pSrcFrame, const NUI_LOCKED_RECT& lockRect, IplImage*& pDstImage, IplImage*& pUserIdImage);
// RGB画像バッファリング
long BufferRgbImage(const NUI_IMAGE_FRAME* pSrcFrame, const NUI_LOCKED_RECT& lockRect, IplImage*& pDstImage);
// LockRectから画素値を取り出す
long PixelVal(USHORT& val, int x, int y, const NUI_IMAGE_FRAME* pSrcFrame, const NUI_LOCKED_RECT& lockRect);
protected:
//---------------------------------------------------------
// 属性
//---------------------------------------------------------
//------ RGB関連 ------
HANDLE m_hImgStreamEvent;
HANDLE m_hImgStreamHandle;
const NUI_IMAGE_FRAME* m_pImgFrame; // フレームワーク側のバッファ.
NUI_LOCKED_RECT m_ImgLckRect;
IplImage* m_pRgbImageBuff;
//------ DEPTH関連 ------
HANDLE m_hDepStreamEvent;
HANDLE m_hDepStreamHandle;
const NUI_IMAGE_FRAME* m_pDepFrame; // フレームワーク側のバッファ.
NUI_LOCKED_RECT m_DepLckRect;
IplImage* m_pDepImageBuff;
//------ SKELTON/USER TRACKING関連 ------
HANDLE m_hSklStreamEvent;
NUI_SKELETON_FRAME m_SklFrame;
IplImage* m_pUserImageBuff;
protected:
//---------------------------------------------------------
// 定数
//---------------------------------------------------------
/**
* @brief フレーム解放-->次フレーム取得の実施を待つ時間(unit:msec).
*/
static const DWORD MSEC_REFRESH_WAIT_TIME = 50;
} CKinectCtrl;
}
- KinectUtil.cpp
/**
* @file KinectUtil.cpp
*/
#include "KinectUtil.h"
#include "LogCommon.h"
#include <vector>
#define _USE_MATH_DEFINES
#include <math.h>
#define LOG INF_LOG
namespace Utility
{
long CKinectController::GrabFrame()
{
// フレーム破棄.
long lRes = ReleaseFrame();
if(lRes != 0){
LOG(" ERR: 前フレームリリース失敗. res=%ld.", lRes);
}
Sleep(MSEC_REFRESH_WAIT_TIME); // 待つ.
// 次フレーム取得.
lRes = UpdateFrame();
if(lRes != 0){
LOG(" ERR: 次フレーム取得失敗. res=%ld.", lRes);
return lRes;
}
// RGB画像バッファリング.
if(m_pRgbImageBuff != NULL){
cvReleaseImage(&m_pRgbImageBuff);
m_pRgbImageBuff = NULL;
}
lRes = BufferRgbImage(m_pImgFrame, m_ImgLckRect, m_pRgbImageBuff);
if(lRes != 0){
LOG(" ERR: RGB画像バッファリング失敗. res=%ld.", lRes);
return lRes;
}
// DEPTH画像バッファリング.
if(m_pDepImageBuff != NULL){
cvReleaseImage(&m_pDepImageBuff);
m_pDepImageBuff = NULL;
}
lRes = BufferDepthImage(m_pDepFrame, m_DepLckRect, m_pDepImageBuff, m_pUserImageBuff);
if(lRes != 0){
LOG(" ERR: DEPTH画像バッファリング失敗. res=%ld.", lRes);
return lRes;
}
return 0;
}
long CKinectController::GetRGBFrame(IplImage*& pImage)
{
if(m_pRgbImageBuff == NULL){
LOG(" ERR: キャプチャがバッファされてない. Capture is Not Buffering!");
return 1;
}
pImage = cvCloneImage( m_pRgbImageBuff );
if(pImage == NULL){
LOG(" ERR: cvCloneImage() returns NULL.");
return 2;
}
return 0;
}
long CKinectController::GetDepthFrame(IplImage*& pImage)
{
if(m_pDepImageBuff == NULL){
LOG(" ERR: キャプチャがバッファされてない. Capture is Not Buffering!");
return 1;
}
pImage = cvCloneImage( m_pDepImageBuff );
if(pImage == NULL){
LOG(" ERR: cvCloneImage() returns NULL.");
return 2;
}
return 0;
}
long CKinectController::GetTrakingIdFrame(IplImage*& pImage)
{
if(m_pUserImageBuff == NULL){
LOG(" ERR: キャプチャがバッファされてない. Capture is Not Buffering!");
return 1;
}
pImage = cvCloneImage( m_pUserImageBuff );
if(pImage == NULL){
LOG(" ERR: cvCloneImage() returns NULL.");
return 2;
}
return 0;
}
long CKinectController::Initialize()
{
// 初期化.
static const DWORD dwFlag = NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX |
NUI_INITIALIZE_FLAG_USES_SKELETON |
NUI_INITIALIZE_FLAG_USES_COLOR |
NUI_INITIALIZE_FLAG_USES_AUDIO ;
HRESULT hRes = NuiInitialize(dwFlag);
if(hRes != S_OK){
LOG(" ERR: Kinect初期化失敗. NuiInitialize() returns %d.", hRes);
return 1;
}
// RGBストリームオープン.
hRes = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, m_hImgStreamEvent, &m_hImgStreamHandle);
if(hRes != S_OK){
LOG(" ERR: RGBストリームオープン失敗. NuiImageStreamOpen() returns %d.", hRes);
return 2;
}
// Depthストリームオープン.
hRes = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_320x240 , 0, 2, m_hDepStreamEvent, &m_hDepStreamHandle);
if(hRes != S_OK){
LOG(" ERR: Depthストリームオープン失敗. NuiImageStreamOpen() returns %d.", hRes);
return 3;
}
// スケルトントラッキングオン設定
hRes = NuiSkeletonTrackingEnable(m_hSklStreamEvent, 0);
if(hRes != S_OK){
LOG(" スケルトントラッキングオン設定失敗. NuiSkeletonTrackingEnable() returns %ld.", hRes);
return 4;
}
return 0;
}
long CKinectController::Finalize()
{
NuiSkeletonTrackingDisable();
// 終了処理.
NuiShutdown();
return 0;
}
long CKinectController::UpdateFrame()
{
// RGBフレーム取得.
HRESULT hRes = NuiImageStreamGetNextFrame(m_hImgStreamHandle, 0, &m_pImgFrame);
if(hRes != S_OK){
LOG(" ERR: RGB次フレーム取得失敗. NuiImageStreamGetNextFrame() returns %d.", hRes);
return 1;
}
// RGBバッファをロック.
hRes = m_pImgFrame->pFrameTexture->LockRect(0, &m_ImgLckRect, 0, 0);
if(hRes != S_OK){
LOG(" ERR: RGBフレームバッファロック失敗. res=%d.", hRes);
return 2;
}
// Depthフレーム取得.
hRes = NuiImageStreamGetNextFrame(m_hDepStreamHandle, 0, &m_pDepFrame);
if(hRes != S_OK){
LOG(" ERR: Depth次フレーム取得失敗. NuiImageStreamGetNextFrame() returns %d.", hRes);
return 11;
}
// Depthバッファをロック.
m_pDepFrame->pFrameTexture->LockRect(0, &m_DepLckRect, 0, 0);
if(hRes != S_OK){
LOG(" ERR: Depthフレームバッファロック失敗. res=%d.", hRes);
return 12;
}
hRes = NuiSkeletonGetNextFrame(0, &m_SklFrame);
if(hRes != S_OK){
LOG(" ERR: スケルトン次フレーム取得失敗. res=%d.", hRes);
//return 3;
}
return 0;
}
long CKinectController::ReleaseFrame()
{
if(m_pImgFrame != NULL){
HRESULT hRes = NuiImageStreamReleaseFrame(m_hImgStreamHandle, m_pImgFrame);
if(hRes != S_OK){
LOG(" ERR: RGBフレーム解放失敗. res=%d.", hRes);
return 1;
}
}
if(m_pImgFrame != NULL){
HRESULT hRes = NuiImageStreamReleaseFrame(m_hDepStreamHandle, m_pDepFrame);
if(hRes != S_OK){
LOG(" ERR: Depthフレーム解放失敗. res=%d.", hRes);
return 2;
}
}
return 0;
}
long CKinectController::BufferDepthImage(const NUI_IMAGE_FRAME* pSrcFrame,
const NUI_LOCKED_RECT& lockRect, IplImage*& pDstImage, IplImage*& pUserIdImage)
{
if(pSrcFrame == NULL){
LOG(" ERR: pSrcFrame is NULL.");
return -1;
}
// 画像サイズ取得.
NUI_SURFACE_DESC surfaceSize;
HRESULT hRes = pSrcFrame->pFrameTexture->GetLevelDesc(0, &surfaceSize);
if(hRes != S_OK){
LOG(" ERR: バッファ画像サイズ取得失敗. GetLevelDesc() returns %d.", hRes);
return 1;
}
// 画像バッファ作成.
CvSize imgSize = cvSize(surfaceSize.Width, surfaceSize.Height);
IplImage* dstImage = cvCreateImage(imgSize, IPL_DEPTH_16U, 1); // unsigned short x 1ch
if(dstImage == NULL){
LOG(" ERR: cvCreateImage() returns NULL. img(W=%d,H=%d).", imgSize.width, imgSize.height);
return 2;
}
cvSetZero(dstImage);
IplImage* usrImage = cvCreateImage(imgSize, IPL_DEPTH_8U, 1); // byte x 1ch.
if(usrImage == NULL){
LOG(" ERR: cvCreateImage() returns NULL. img(W=%d,H=%d).", imgSize.width, imgSize.height);
return 3;
}
cvSetZero(usrImage);
// Depthバッファコピー.
unsigned short* dstPtr = (unsigned short*)dstImage->imageData;
unsigned char* usrPtr = (unsigned char*) usrImage->imageData;
UINT* srcPtr = (UINT*)lockRect.pBits;
for(int y=0; y<imgSize.height; y++){
for(int x=0; x<imgSize.width; x++){
USHORT usValue = 0; // 画素値取得.
long lRes = PixelVal(usValue, x, y, pSrcFrame, lockRect);
if(lRes != 0){
LOG(" [%d] ERR: バッファ画素値取得失敗. res=%ld. pt(X=%,Y=%d). --> 終了.", lRes, (x/2), (y/2));
return lRes;
}
// 画素読み取り.
*usrPtr = (usValue & 0x7) * 10; // UserID.
*dstPtr = (usValue >> 3); // 3bitズラす.
// 次の画素.
dstPtr++;
usrPtr++;
}
}
// 引数に格納.
pDstImage = dstImage;
pUserIdImage = usrImage;
return 0;
}
long CKinectController::BufferRgbImage(const NUI_IMAGE_FRAME* pSrcFrame,
const NUI_LOCKED_RECT& lockRect, IplImage*& pDstImage)
{
if(pSrcFrame == NULL){
LOG(" ERR: pSrcFrame is NULL.");
return -1;
}
// 画像サイズ取得.
NUI_SURFACE_DESC surfaceSize;
HRESULT hRes = pSrcFrame->pFrameTexture->GetLevelDesc(0, &surfaceSize);
if(hRes != S_OK){
LOG(" ERR: バッファ画像サイズ取得失敗. GetLevelDesc() returns %d.", hRes);
return 1;
}
// 画像バッファ作成.
CvSize imgSize = cvSize(surfaceSize.Width, surfaceSize.Height);
IplImage* dstImage = cvCreateImage(imgSize, IPL_DEPTH_8U, 4);
if(dstImage == NULL){
LOG(" ERR: cvCreateImage() returns NULL. img(W=%d,H=%d).", imgSize.width, imgSize.height);
return 2;
}
unsigned int cpySize = imgSize.width * imgSize.height * dstImage->nChannels * sizeof(BYTE);
memcpy( dstImage->imageData, (BYTE*)lockRect.pBits, cpySize );
// 引数に格納.
pDstImage = dstImage;
return 0;
}
long CKinectController::PixelVal(USHORT& val, int x, int y, const NUI_IMAGE_FRAME* pSrcFrame, const NUI_LOCKED_RECT& lockRect)
{
// 引数チェック.
if(pSrcFrame == NULL){
return -1;
}
NUI_SURFACE_DESC surfaceSize;
HRESULT hRes = pSrcFrame->pFrameTexture->GetLevelDesc(0, &surfaceSize);
if(hRes != S_OK){
LOG(" ERR: バッファ画像サイズ取得失敗. GetLevelDesc() returns %d.", hRes);
return 1;
}
if( (x < 0) || (x > surfaceSize.Width) || (y < 0) || (y > surfaceSize.Height) ){
LOG(" ERR: Invalid Coord Input. pt(X=%d,Y=%d) <--> img(W=%d,H=%d).", x, y, surfaceSize.Width, surfaceSize.Height);
return -1;
}
// 計算.
const USHORT* usPtr = (const USHORT*)lockRect.pBits;
const UCHAR* ucPtr = (UCHAR*) &usPtr[ (surfaceSize.Width * y) + x ];
val = (USHORT)(ucPtr[0] | ucPtr[1] << 8);
return 0;
}
long CKinectController::GetTrakingInfo(std::vector<CTrackingInfo>& trackingInfoList)
{
// スケルトン情報の参照.
//LOG(" スケルトントラッキング情報取得.");
for(int ns=0; ns<NUI_SKELETON_COUNT; ns++){
if(m_SklFrame.SkeletonData[ns].eTrackingState == NUI_SKELETON_TRACKED){
// 関節情報の取り出し
CTrackingInfo cInfo;
cInfo.lTrackingID = m_SklFrame.SkeletonData[ns].dwTrackingID;
//LOG(" [%d/%d] TRACKING!! id=%d. ", ns, NUI_SKELETON_COUNT, cInfo.lTrackingID);
for(int np=0; np<NUI_SKELETON_POSITION_COUNT; np++){
long lX = 0, lY = 0;
USHORT usDp = 0;
// 座標変換.
NuiTransformSkeletonToDepthImage(
m_SklFrame.SkeletonData[ns].SkeletonPositions[np],
&lX, &lY, &usDp);
//LOG(" [%d/%d][skl:%d] (X=%d,Y=%d,D=%d).", ns, NUI_SKELETON_COUNT, np, lX, lY, usDp);
CDptPoint cDptPt((float)lX, (float)lY, usDp);
// リストに追加.
cInfo.skeletalPoints.push_back( cDptPt );
}
trackingInfoList.push_back( cInfo );
}else{
//LOG(" [%d/%d] NOT TRACKING..... ", ns, NUI_SKELETON_COUNT);
}
}
return 0;
}
long CKinectController::SetElevationAngle(float fAgl)
{
long lAngle = (long)fAgl;
HRESULT hRes = NuiCameraElevationSetAngle(lAngle);
LOG(" ピッチ角制御: res=%d. angle=%ld.", hRes, lAngle);
if(hRes != S_OK){
return 1;
}
return 0;
}
long CKinectController::GetElevationAngle(float& fAgl)
{
long lAngle = -1;
HRESULT hRes = NuiCameraElevationGetAngle(&lAngle);
LOG(" ピッチ角取得: res=%d. angle=%ld.", hRes, lAngle);
if(hRes != S_OK){
return 1;
}
fAgl = (float)lAngle;
return 0;
}
long CKinectController::Depth16bitTo8bitImage(const IplImage* srcImage, IplImage*& dstImage)
{
if(srcImage == NULL){
return -1;
}
if(srcImage->nChannels != 1){
return -1;
}
if(srcImage->depth != IPL_DEPTH_16U){
return -1;
}
IplImage* tmpImage = cvCreateImage( cvGetSize(srcImage), IPL_DEPTH_16U, 1 );
if(tmpImage == NULL){
LOG(" ERR: cvCreateImage() returns NULL.");
return 1;
}
static const int NORM_COEFF = 6;
cvSetZero(tmpImage);
unsigned short* pSrc = (unsigned short*)srcImage->imageData;
unsigned short* pDst = (unsigned short*)tmpImage->imageData;
for(int y=0; y<tmpImage->height; y++){
for(int x=0; x<tmpImage->width; x++, pSrc++, pDst++){
*pDst = *pSrc * NORM_COEFF;
}
}
dstImage = cvCreateImage( cvGetSize(tmpImage), IPL_DEPTH_8U, 1 );
if(dstImage == NULL){
LOG(" ERR: cvCreateImage() returns NULL.");
return 2;
}
double dScale = 1.0 / UCHAR_MAX;
cvCvtScale(tmpImage, dstImage, dScale);
cvReleaseImage(&tmpImage);
return 0;
}
CKinectController::CKinectController()
{
// RGB関連
m_hImgStreamEvent = NULL;
m_hImgStreamHandle = NULL;
m_pImgFrame = NULL;
m_pRgbImageBuff = NULL;
// Depth関連
m_hDepStreamEvent = NULL;
m_hDepStreamHandle = NULL;
m_pDepFrame = NULL;
m_pDepImageBuff = NULL;
//------ SKELTON/USER TRACKING関連 ------
m_hSklStreamEvent = NULL;
m_pUserImageBuff = NULL;
}
CKinectController::~CKinectController()
{
Finalize();
}
CDepthPoint::CDepthPoint() :fX(0.0), fY(0.0), usDepth(0) {
}
CDepthPoint::CDepthPoint(float x, float y, USHORT depth) {
fX = x; fY = y; usDepth = depth;
}
CDepthPoint::~CDepthPoint(){
}
CDepthPoint::CDepthPoint(const CDepthPoint& obj){
this->fX = obj.fX; this->fY = obj.fY; this->usDepth = obj.usDepth;
}
CTrackingInfo::CTrackingInfo() : lTrackingID(0) {
}
CTrackingInfo::~CTrackingInfo(){
}
CTrackingInfo::CTrackingInfo(const CTrackingInfo& obj){
this->lTrackingID = obj.lTrackingID; this->skeletalPoints = obj.skeletalPoints;
}
}
- main.cpp
#include "stdafx.h"
#include "KinectUtil.h"
#include <opencv/highgui.h>
#include <vector>
int _tmain(int argc, _TCHAR* argv[])
{
Utility::CKinectCtrl cKinectCtrl;
// 初期化.
long lRes = cKinectCtrl.Initialize();
if(lRes != 0){
return lRes;
}
cKinectCtrl.SetElevationAngle(0); // サーボ角制御.
while(1){
// キャプチャ処理.
lRes = cKinectCtrl.GrabFrame();
if(lRes != 0){
break;
}
// RGB画像.
IplImage* rgbImage = NULL;
lRes = cKinectCtrl.GetRGBFrame(rgbImage);
if(lRes != 0){
break;
}
cvShowImage("RGB", rgbImage);
// DEPTH画像.
IplImage* depthSrc = NULL;
lRes = cKinectCtrl.GetDepthFrame(depthSrc);
if(lRes != 0){
break;
}
cvShowImage("DEPTH", depthSrc);
int key = cvWaitKey(10);
if((key == 27)||(key == 'q')){
break;
}
}
return 0;
}
- 追伸
