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_SEATED_SUPPORT ) );
// フレーム更新イベントのハンドルを作成する
streamEvent = ::CreateEvent( 0, TRUE, FALSE, 0 );
ERROR_CHECK( kinect->NuiSetFrameEndEvent( streamEvent, 0 ) );
// 指定した解像度の、画面サイズを取得する
::NuiImageResolutionToSize(CAMERA_RESOLUTION, width, height );
}
~KinectSample()
{
// 終了処理
if ( kinect != 0 ) {
kinect->NuiShutdown();
kinect->Release();
}
}
/// <summary>
/// Create the first connected Kinect found
/// </summary>
/// <returns>indicates success or failure</returns>
HRESULT CSkeletonBasics::CreateFirstConnected()
{
INuiSensor * pNuiSensor;
int iSensorCount = 0;
HRESULT hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
{
return hr;
}
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr))
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr)
{
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
if (NULL != m_pNuiSensor)
{
// Initialize the Kinect and specify that we'll be using skeleton
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_SKELETON);
if (SUCCEEDED(hr))
{
// Create an event that will be signaled when skeleton data is available
m_hNextSkeletonEvent = CreateEventW(NULL, TRUE, FALSE, NULL);
// Open a skeleton stream to receive skeleton data
hr = m_pNuiSensor->NuiSkeletonTrackingEnable(m_hNextSkeletonEvent, 0);
}
}
if (NULL == m_pNuiSensor || FAILED(hr))
{
SetStatusMessage(L"No ready Kinect found!");
return E_FAIL;
}
return hr;
}
void drawRgbImage( cv::Mat& image )
{
// RGBカメラのフレームデータを取得する
NUI_IMAGE_FRAME imageFrame = { 0 };
ERROR_CHECK( kinect->NuiImageStreamGetNextFrame(
imageStreamHandle, INFINITE, &imageFrame ) );
// 画像データを取得する
NUI_LOCKED_RECT colorData;
imageFrame.pFrameTexture->LockRect( 0, &colorData, 0, 0 );
// 画像データをコピーする
image = cv::Mat( height, width, CV_8UC4, colorData.pBits );
// フレームデータを解放する
ERROR_CHECK( kinect->NuiImageStreamReleaseFrame( imageStreamHandle, &imageFrame ) );
}
/**
@brief Create kinect sensor
*/
INuiSensor* KinectContext::Create(const OLECHAR* strInstanceId)
{
INuiSensor* sensor = NULL;
HRESULT ret = ::NuiCreateSensorById(strInstanceId, &sensor);
if (SUCCEEDED(ret)) {
lock_.lock();
int size = (int)kinects_.size();
for(int i = 0; i < size; i++){
if(0 == wcscmp(kinects_[i]->uniqueId_, sensor->NuiDeviceConnectionId())){
std::wcout << "Kinect[" << kinects_[i]->index_ << "] is connected, Device Name:: " << sensor->NuiDeviceConnectionId() << "\n" << std::endl;
lock_.unlock();
return sensor;
}
}
lock_.unlock();
}
return NULL;
}
/**
@brief Create kinect sensor
*/
INuiSensor* KinectContext::Create(int index)
{
INuiSensor* sensor = NULL;
HRESULT ret = ::NuiCreateSensorByIndex(index, &sensor);
if (SUCCEEDED(ret)) {
lock_.lock();
int size = (int)kinects_.size();
for(int i = 0; i < size; i++){
if(kinects_[i]->index_ == sensor->NuiInstanceIndex()){
std::wcout << "Kinect[" << kinects_[i]->index_ << "] is connected, Device Name:: " << sensor->NuiDeviceConnectionId() << "\n" << std::endl;
lock_.unlock();
return sensor;
}
}
lock_.unlock();
}
return NULL;
}
HRESULT KinectManager::initialize()
{
INuiSensor * nui;
int nuiCount = 0;
HRESULT hr;
NuiSetDeviceStatusCallback(OnSensorStatusChanged, NULL);
hr = NuiGetSensorCount(&nuiCount);
if ( FAILED(hr))
{
return hr;
}
// Look at each kinect sensor
for (int i = 0; i < nuiCount; i++)
{
// Create the sensor so we can check status, if we can't create it, move on.
hr = NuiCreateSensorByIndex(i, &nui);
if (FAILED(hr))
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it.
hr = nui->NuiStatus();
if (S_OK == hr)
{
nuiList.push_front(nui);
}
// This sensor was not okay, so we release it (into the wild!) since we're not using it.
nui->Release();
}
return hr;
}
INuiSensor*
GetNuiPointer()
{
int sensorCount = 0;
INuiSensor *nuiSensor = NULL;
HRESULT hr = NuiGetSensorCount(&sensorCount);
if (FAILED(hr))
{
throw "Failed to get NUI sensor count";
}
// Look at each Kinect sensor
for (int i=0; i<sensorCount; i++)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &nuiSensor);
if (FAILED(hr)) continue;
// Get the status of the sensor, and if connected, then we can initialize it
hr = nuiSensor->NuiStatus();
if (S_OK == hr) break;
// This sensor wasn't OK, so release it since we're not using it
nuiSensor->Release();
}
// If we couldn't get an instance, return failure
if (NULL == nuiSensor)
{
throw "Failed to get NUI Sensor instance";
}
// Initialize the Kinect and specify that we'll be using depth
hr = nuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR
| NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX);
if (FAILED(hr)) return NULL;
else return nuiSensor;
}
/// <summary>
/// Enumerate and construct all the sensors when the app starts up
/// </summary>
void CMainWindow::EnumerateSensors()
{
int iCount = 0;
HRESULT hr = NuiGetSensorCount(&iCount);
if (FAILED(hr))
{
return;
}
for (int i = 0; i < iCount; ++i)
{
INuiSensor* pNuiSensor = nullptr;
if (SUCCEEDED(NuiCreateSensorByIndex(i, &pNuiSensor)))
{
UpdateMainWindow(pNuiSensor->NuiDeviceConnectionId(), pNuiSensor->NuiStatus());
}
SafeRelease(pNuiSensor);
}
}
// スケルトンを使用してマウス操作を行う
void skeletonMouse()
{
// スケルトンのフレームを取得する
NUI_SKELETON_FRAME skeletonFrame = { 0 };
kinect->NuiSkeletonGetNextFrame( 0, &skeletonFrame );
// トラッキングしている最初のスケルトンを探す
NUI_SKELETON_DATA* skeletonData = 0;
for ( int i = 0; i < NUI_SKELETON_COUNT; ++i ) {
NUI_SKELETON_DATA& data = skeletonFrame.SkeletonData[i];
if ( data.eTrackingState == NUI_SKELETON_TRACKED ) {
skeletonData = &data;
break;
}
}
// 追跡しているスケルトンがなければ終了
if ( skeletonData == 0 ) {
return;
}
// 右手が追跡されていなければ終了
if ( skeletonData->eSkeletonPositionTrackingState[NUI_SKELETON_POSITION_HAND_RIGHT]
== NUI_SKELETON_POSITION_NOT_TRACKED ) {
return;
}
// 右手の座標を取得する
Vector4 rh = skeletonData->SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT];
// 右手の座標を、Depthの座標(2次元)に変換する
FLOAT depthX = 0, depthY = 0;
::NuiTransformSkeletonToDepthImage( rh, &depthX, &depthY, CAMERA_RESOLUTION );
// スクリーン座標を取得する
SIZE screen = {
::GetSystemMetrics( SM_CXVIRTUALSCREEN ),
::GetSystemMetrics( SM_CYVIRTUALSCREEN )
};
// 右手の座標を、スクリーン座標に変換する
int x = (depthX * screen.cx) / width;
int y = (depthY * screen.cy) / height;
// マウスを動かす
SendInput::MouseMove( x, y, screen );
// クリックを認識した場合に、右クリックを行う
if ( isClicked( FramePoint( skeletonFrame.liTimeStamp.QuadPart, depthX, depthY ) ) ) {
SendInput::LeftClick();
}
}
/// <summary>
/// Funcion que crea la primera conexion de la aplicacion con Kinect. Comprueba e inicializa los sensores.
/// </summary>
/// <returns>Devuelve un HRESULT con las banderas de los flags inicializados y los que no.</returns>
HRESULT CreateFirstConnected() {
INuiSensor * pNuiSensor;
HRESULT hr;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr)) {
return hr;
}
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i) {
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr)) {
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr) {
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
// Initialize the Kinect and specify that we'll be using depth
DWORD dwFlags = 0;
if (m_bProcessColor) dwFlags |= NUI_INITIALIZE_FLAG_USES_COLOR;
if (m_bProcessDepth)
m_bUserDetection ? (dwFlags |= NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX) : (dwFlags |= NUI_INITIALIZE_FLAG_USES_DEPTH);
if (m_bProcessSkeleton)
dwFlags |= NUI_INITIALIZE_FLAG_USES_SKELETON;
hr = m_pNuiSensor->NuiInitialize(dwFlags);
return hr;
}
void initialize()
{
createInstance();
// Kinectの設定を初期化する
ERROR_CHECK( kinect->NuiInitialize(
NUI_INITIALIZE_FLAG_USES_COLOR |
NUI_INITIALIZE_FLAG_USES_SKELETON ) );
// RGBカメラを初期化する
ERROR_CHECK( kinect->NuiImageStreamOpen( NUI_IMAGE_TYPE_COLOR, CAMERA_RESOLUTION,
0, 2, 0, &imageStreamHandle ) );
// スケルトンを初期化する
ERROR_CHECK( kinect->NuiSkeletonTrackingEnable(
0, NUI_SKELETON_TRACKING_FLAG_SUPPRESS_NO_FRAME_DATA ) );
// フレーム更新イベントのハンドルを作成する
streamEvent = ::CreateEvent( 0, TRUE, FALSE, 0 );
ERROR_CHECK( kinect->NuiSetFrameEndEvent( streamEvent, 0 ) );
// 指定した解像度の、画面サイズを取得する
::NuiImageResolutionToSize(CAMERA_RESOLUTION, width, height );
}
void getJoint( Vector4 position, int part )
{
FLOAT depthX = 0, depthY = 0;
::NuiTransformSkeletonToDepthImage( position, &depthX, &depthY, CAMERA_RESOLUTION );
LONG colorX = 0;
LONG colorY = 0;
kinect->NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(
CAMERA_RESOLUTION, CAMERA_RESOLUTION,
0, (LONG)depthX , (LONG)depthY, 0, &colorX, &colorY );
kinectX[part] = (int)colorX;
kinectY[part] = (int)colorY;
}
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 getSkeleton( )
{
// スケルトンのフレームを取得する
NUI_SKELETON_FRAME skeletonFrame = { 0 };
kinect->NuiSkeletonGetNextFrame( 0, &skeletonFrame );
//ERROR_CHECK( kinect->NuiSkeletonGetNextFrame( 0, &skeletonFrame ) );
for ( int i = 0; i < NUI_SKELETON_COUNT; ++i ) {
NUI_SKELETON_DATA& skeletonData = skeletonFrame.SkeletonData[i];
if ( skeletonData.eTrackingState == NUI_SKELETON_TRACKED ) {
for (int j = 0; j < NUI_SKELETON_POSITION_COUNT; ++j){
if (skeletonData.eSkeletonPositionTrackingState[j] != NUI_SKELETON_POSITION_NOT_TRACKED) {
getJoint(skeletonData.SkeletonPositions[j], j);
}
}
}
}
}
// connect to a Kinect sensor and stream its data into LSL
void stream_from_sensor(int sensornum, bool smoothdata) {
HRESULT hr=0;
try {
// instantiate sensor
cout << "initializing sensor " << sensornum << "..." << endl;
INuiSensor *sensor;
if (FAILED(hr=NuiCreateSensorByIndex(sensornum,&sensor)))
throw runtime_error("Could not instantiate sensor.");
// initialize and enable skeletal tracking
if (FAILED(hr = sensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_SKELETON)))
throw runtime_error("Could not initialize NUI functionality.");
if (FAILED(hr = sensor->NuiSkeletonTrackingEnable(NULL,NUI_SKELETON_TRACKING_FLAG_SUPPRESS_NO_FRAME_DATA)))
throw runtime_error("Could not enable skeletal tracking.");
cout << "opening outlet..." << endl;
char tmp[1024]; ;
stream_info info(string("KinectMocap")+=boost::lexical_cast<string>(sensornum),"Mocap",NUM_CHANNELS_PER_STREAM,30,cf_float32,string(tmp,tmp+wcstombs(tmp,sensor->NuiUniqueId(),sizeof(tmp))));
// physical setup
xml_element setup = info.desc().append_child("setup");
xml_element cam = setup.append_child("cameras").append_child("camera");
cam.append_child_value("label","Kinect");
cam.append_child("position")
.append_child_value("X","0.0")
.append_child_value("Y","0.0")
.append_child_value("Z","0.0");
cam.append_child_value("diagonal_fov",boost::lexical_cast<string>(NUI_CAMERA_DEPTH_NOMINAL_DIAGONAL_FOV).c_str());
cam.append_child_value("horizontal_fov",boost::lexical_cast<string>(NUI_CAMERA_DEPTH_NOMINAL_HORIZONTAL_FOV).c_str());
cam.append_child_value("vertical_fov",boost::lexical_cast<string>(NUI_CAMERA_DEPTH_NOMINAL_VERTICAL_FOV).c_str());
// markers
xml_element mrks = setup.append_child("markers");
for (int k=0;k<NUI_SKELETON_POSITION_COUNT;k++) {
mrks.append_child("marker")
.append_child_value("label",joint_names[k])
.append_child_value("id",boost::lexical_cast<std::string>(k).c_str());
}
// channel layout
xml_element channels = info.desc().append_child("channels");
for (int s=0;s<NUI_SKELETON_MAX_TRACKED_COUNT;s++) {
for (int k=0;k<NUI_SKELETON_POSITION_COUNT;k++) {
channels.append_child("channel")
.append_child_value("label",(string(joint_names[k])+="_X").c_str())
.append_child_value("marker",joint_names[k])
.append_child_value("type","PositionX")
.append_child_value("unit","meters");
channels.append_child("channel")
.append_child_value("label",(string(joint_names[k])+="_Y").c_str())
.append_child_value("marker",joint_names[k])
.append_child_value("type","PositionY")
.append_child_value("unit","meters");
channels.append_child("channel")
.append_child_value("label",(string(joint_names[k])+="_Z").c_str())
.append_child_value("marker",joint_names[k])
.append_child_value("type","PositionZ")
.append_child_value("unit","meters");
channels.append_child("channel")
.append_child_value("label",(string(joint_names[k])+="_Conf").c_str())
.append_child_value("marker",joint_names[k])
.append_child_value("type","Confidence")
.append_child_value("unit","normalized");
}
channels.append_child("channel")
.append_child_value("label",(string("SkeletonTrackingId")+=boost::lexical_cast<string>(s)).c_str())
.append_child_value("type","TrackingId");
channels.append_child("channel")
.append_child_value("label",(string("SkeletonQualityFlags")+=boost::lexical_cast<string>(s)).c_str());
}
// misc meta-data
info.desc().append_child("acquisition")
.append_child_value("manufacturer","Microsoft")
.append_child_value("model","Kinect 1.0");
if (smoothdata)
info.desc().append_child("filtering")
.append_child("holt_double_exponential")
.append_child_value("smoothing","0.5")
.append_child_value("correction","0.5")
.append_child_value("jitter_radius","0.05")
.append_child_value("max_deviation_radius","0.04");
stream_outlet outlet(info);
// acquisition loop
cout << "starting to track on sensor " << sensornum << endl;
while (true) {
// get next frame
NUI_SKELETON_FRAME frame = {0};
if (FAILED(sensor->NuiSkeletonGetNextFrame(100,&frame)))
continue;
// for each skeleton index s
vector<float> sample(NUM_CHANNELS_PER_STREAM);
for (int s=0,i=0; s<NUI_SKELETON_COUNT; s++) {
if (frame.SkeletonData[s].eTrackingState == NUI_SKELETON_TRACKED) {
// optionally smooth the data
if (smoothdata)
sensor->NuiTransformSmooth(&frame,NULL);
// assign the sample data (at slot i)
for (int k=0;k<NUI_SKELETON_POSITION_COUNT;k++) {
sample[i*NUM_CHANNELS_PER_SKELETON + k*NUM_CHANNELS_PER_JOINT + 0] = frame.SkeletonData[s].SkeletonPositions[k].x;
sample[i*NUM_CHANNELS_PER_SKELETON + k*NUM_CHANNELS_PER_JOINT + 1] = frame.SkeletonData[s].SkeletonPositions[k].y;
sample[i*NUM_CHANNELS_PER_SKELETON + k*NUM_CHANNELS_PER_JOINT + 2] = frame.SkeletonData[s].SkeletonPositions[k].z;
sample[i*NUM_CHANNELS_PER_SKELETON + k*NUM_CHANNELS_PER_JOINT + 3] = frame.SkeletonData[s].eSkeletonPositionTrackingState[k] / 2.0f; // 0.0, 0.5, or 1.0
}
sample[i*NUM_CHANNELS_PER_SKELETON + NUM_CHANNELS_PER_JOINT*NUI_SKELETON_POSITION_COUNT] = frame.SkeletonData[s].dwTrackingID;
sample[i*NUM_CHANNELS_PER_SKELETON + NUM_CHANNELS_PER_JOINT*NUI_SKELETON_POSITION_COUNT] = frame.SkeletonData[s].dwQualityFlags;
i++;
}
}
// push the sample into LSL
outlet.push_sample(sample);
}
// shut down
sensor->NuiShutdown();
sensor->Release();
}
catch(exception &e) {
cerr << "Error trying to stream from sensor " << sensornum << ": " << e.what() << endl;
}
}
// TODO: allow for customizing which streams are initialized
HRESULT SimpleKinect::Initialize()
{
INuiSensor * pNuiSensor;
HRESULT hr;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
{
return hr;
}
// Find the correct sensor
cout << "SimpleKinect: Finding sensor..." << endl;
for (int i = 0; i < iSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr))
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr)
{
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
// Open depth stream
if (NULL != m_pNuiSensor)
{
cout << "SimpleKinect: Opening depth stream.." << endl;
// Initialize the Kinect and specify that we'll be using depth, player index and color
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_SKELETON );
if (SUCCEEDED(hr))
{
// Open a depth image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX,
NUI_IMAGE_RESOLUTION_640x480,
0,
2,
NULL,
&m_pDepthStreamHandle);
} else {
cerr << "SimpleKinect: Failed to open depth and player stream!" << endl;
return E_FAIL;
}
// open color stream
cout << "SimpleKinect: Opening color stream.." << endl;
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_COLOR,
NUI_IMAGE_RESOLUTION_640x480,
0,
2,
NULL,
&m_pColorStreamHandle);
if(!SUCCEEDED(hr))
{
cerr << "SimpleKinect: Failed to open color stream!" << endl;
return E_FAIL;
}
cout << "SimpleKinect: Opening skeleton stream.." << endl;
// open skeleton stream
if(HasSkeletalEngine(m_pNuiSensor))
{
hr = m_pNuiSensor->NuiSkeletonTrackingEnable(NULL,
NUI_SKELETON_TRACKING_FLAG_ENABLE_IN_NEAR_RANGE |
NUI_SKELETON_TRACKING_FLAG_ENABLE_SEATED_SUPPORT |
NUI_SKELETON_TRACKING_FLAG_SUPPRESS_NO_FRAME_DATA);
if(!SUCCEEDED(hr))
{
cerr << "SimpleKinect: Failed to open skeleton stream!" << endl;
return E_FAIL;
}
}
else
{
cerr << "SimpleKinect: Sensor does not have skeletal engine!" << endl;
}
}
if (NULL == m_pNuiSensor || FAILED(hr))
{
cerr << "SimpleKinect: No ready Kinect found!" << endl;
return E_FAIL;
} else
{
BSTR uniqueId = m_pNuiSensor->NuiUniqueId();
cout << "SimpleKinect Connected to sensor " << uniqueId << endl;
}
m_isInitialized = true;
return hr;
}
HRESULT KinectSensor::createFirstConnected()
{
INuiSensor* pNuiSensor = NULL;
HRESULT hr = S_OK;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr) ) { return hr; }
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i) {
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr)) {
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr) {
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
if (NULL == m_pNuiSensor) {
return E_FAIL;
}
// Initialize the Kinect and specify that we'll be using depth
//hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX);
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH);
if (FAILED(hr) ) { return hr; }
// Create an event that will be signaled when depth data is available
m_hNextDepthFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a depth image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH,
//NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX,
cDepthResolution,
(8000 << NUI_IMAGE_PLAYER_INDEX_SHIFT),
2,
m_hNextDepthFrameEvent,
&m_pDepthStreamHandle);
if (FAILED(hr) ) { return hr; }
// Create an event that will be signaled when color data is available
m_hNextColorFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a color image stream to receive color frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_COLOR,
cColorResolution,
0,
2,
m_hNextColorFrameEvent,
&m_pColorStreamHandle );
if (FAILED(hr) ) { return hr; }
INuiColorCameraSettings* colorCameraSettings;
HRESULT hrFlag = m_pNuiSensor->NuiGetColorCameraSettings(&colorCameraSettings);
if (hr != E_NUI_HARDWARE_FEATURE_UNAVAILABLE)
{
m_kinect4Windows = true;
}
//TODO MATTHIAS: does this function have to be called every frame?
USHORT* test = new USHORT[getDepthWidth()*getDepthHeight()];
// Get offset x, y coordinates for color in depth space
// This will allow us to later compensate for the differences in location, angle, etc between the depth and color cameras
m_pNuiSensor->NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution(
cColorResolution,
cDepthResolution,
getDepthWidth()*getDepthHeight(),
test,
getDepthWidth()*getDepthHeight()*2,
m_colorCoordinates
);
SAFE_DELETE_ARRAY(test);
// Start with near mode on (if possible)
m_bNearMode = false;
if (m_kinect4Windows) {
toggleNearMode();
}
//toggleAutoWhiteBalance();
return hr;
}
int Init()
{
//Make sure our image types are the same as the OpenNI image types.
//assert(sizeof(XnRGB24Pixel) == sizeof(ColorPixel));
//assert(sizeof(XnDepthPixel) == sizeof(DepthPixel));
//assert(sizeof(XnStatus) == sizeof(int));
INuiSensor* nuiSensorPtr;
int ret;
// Get number of sensors
int sensorCount = 0;
ret = NuiGetSensorCount(&sensorCount);
if(ret < 0)
{
return -1;
}
// Connect to first sensor
for(int i=0; i<sensorCount; i++)
{
// Create sensor so we can check the status
ret = NuiCreateSensorByIndex(i, &nuiSensorPtr);
if(ret < 0)
continue;
// Get the status of the sensor
ret = nuiSensorPtr->NuiStatus();
if(ret < 0)
{
(void)nuiSensorPtr->Release();
nuiSensorPtr = NULL;
continue;
}
sensor = nuiSensorPtr;
}
// Make sure we have a sensor
if(sensor == NULL)
{
return -1;
}
// Initialize Kinect
ret = sensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH);
if(ret < 0)
{
return -1;
}
// Create an event that will be signaled when color data is available
colorImageReadyEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Create an event that will be signaled when depth data is available
depthImageReadyEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a color image stream to receive color frames
ret = sensor->NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, colorImageReadyEvent, &colorStream);
if(ret < 0)
{
return -1;
}
// Open a depth image stream to receive depth frames
ret = sensor->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH, NUI_IMAGE_RESOLUTION_640x480, 0, 2, depthImageReadyEvent, &depthStream);
if(ret < 0)
{
return -1;
}
return 0;
}
int _tmain(int argc, _TCHAR* argv[])
{
cv::setUseOptimized( true );
INuiSensor* pSensor;
HRESULT hResult = S_OK;
hResult = NuiCreateSensorByIndex( 0, &pSensor );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiCreateSensorByIndex" << std::endl;
return -1;
}
hResult = pSensor->NuiInitialize( NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiInitialize" << std::endl;
return -1;
}
HANDLE hColorEvent = INVALID_HANDLE_VALUE;
HANDLE hColorHandle = INVALID_HANDLE_VALUE;
hColorEvent = CreateEvent( nullptr, true, false, nullptr );
hResult = pSensor->NuiImageStreamOpen( NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, hColorEvent, &hColorHandle );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiImageStreamOpen( COLOR )" << std::endl;
return -1;
}
HANDLE hDepthPlayerEvent = INVALID_HANDLE_VALUE;
HANDLE hDepthPlayerHandle = INVALID_HANDLE_VALUE;
hDepthPlayerEvent = CreateEvent( nullptr, true, false, nullptr );
hResult = pSensor->NuiImageStreamOpen( NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_640x480, 0, 2, hDepthPlayerEvent, &hDepthPlayerHandle );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiImageStreamOpen( DEPTH&PLAYER )" << std::endl;
return -1;
}
HANDLE hSkeletonEvent = INVALID_HANDLE_VALUE;
hSkeletonEvent = CreateEvent( nullptr, true, false, nullptr );
hResult = pSensor->NuiSkeletonTrackingEnable( hSkeletonEvent, 0 );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiSkeletonTrackingEnable" << std::endl;
return -1;
}
unsigned long refWidth = 0;
unsigned long refHeight = 0;
NuiImageResolutionToSize( NUI_IMAGE_RESOLUTION_640x480, refWidth, refHeight );
int width = static_cast<int>( refWidth );
int height = static_cast<int>( refHeight );
INuiCoordinateMapper* pCordinateMapper;
hResult = pSensor->NuiGetCoordinateMapper( &pCordinateMapper );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiGetCoordinateMapper" << std::endl;
return -1;
}
std::vector<NUI_COLOR_IMAGE_POINT> pColorPoint( width * height );
HANDLE hEvents[3] = { hColorEvent, hDepthPlayerEvent, hSkeletonEvent };
cv::Vec3b color[7];
color[0] = cv::Vec3b( 0, 0, 0 );
color[1] = cv::Vec3b( 255, 0, 0 );
color[2] = cv::Vec3b( 0, 255, 0 );
color[3] = cv::Vec3b( 0, 0, 255 );
color[4] = cv::Vec3b( 255, 255, 0 );
color[5] = cv::Vec3b( 255, 0, 255 );
color[6] = cv::Vec3b( 0, 255, 255 );
cv::namedWindow( "Color" );
cv::namedWindow( "Depth" );
cv::namedWindow( "Player" );
cv::namedWindow( "Skeleton" );
while( 1 ){
ResetEvent( hColorEvent );
ResetEvent( hDepthPlayerEvent );
ResetEvent( hSkeletonEvent );
WaitForMultipleObjects( ARRAYSIZE( hEvents ), hEvents, true, INFINITE );
NUI_IMAGE_FRAME colorImageFrame = { 0 };
hResult = pSensor->NuiImageStreamGetNextFrame( hColorHandle, 0, &colorImageFrame );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiImageStreamGetNextFrame( COLOR )" << std::endl;
return -1;
}
INuiFrameTexture* pColorFrameTexture = colorImageFrame.pFrameTexture;
NUI_LOCKED_RECT colorLockedRect;
pColorFrameTexture->LockRect( 0, &colorLockedRect, nullptr, 0 );
NUI_IMAGE_FRAME depthPlayerImageFrame = { 0 };
hResult = pSensor->NuiImageStreamGetNextFrame( hDepthPlayerHandle, 0, &depthPlayerImageFrame );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiImageStreamGetNextFrame( DEPTH&PLAYER )" << std::endl;
return -1;
}
BOOL nearMode = false;
INuiFrameTexture* pDepthPlayerFrameTexture = nullptr;
pSensor->NuiImageFrameGetDepthImagePixelFrameTexture( hDepthPlayerHandle, &depthPlayerImageFrame, &nearMode, &pDepthPlayerFrameTexture );
NUI_LOCKED_RECT depthPlayerLockedRect;
pDepthPlayerFrameTexture->LockRect( 0, &depthPlayerLockedRect, nullptr, 0 );
NUI_SKELETON_FRAME skeletonFrame = { 0 };
hResult = pSensor->NuiSkeletonGetNextFrame( 0, &skeletonFrame );
if( FAILED( hResult ) ){
std::cout << "Error : NuiSkeletonGetNextFrame" << std::endl;
return -1;
}
/*
NUI_TRANSFORM_SMOOTH_PARAMETERS smoothParameter;
smoothParameter.fSmoothing = 0.5;
smoothParameter.fCorrection = 0.5;
smoothParameter.fPrediction = 0.0f;
smoothParameter.fJitterRadius = 0.05f;
smoothParameter.fMaxDeviationRadius = 0.04f;
hResult = NuiTransformSmooth( &skeletonFrame, &smoothParameter );
*/
cv::Mat colorMat( height, width, CV_8UC4, reinterpret_cast<unsigned char*>( colorLockedRect.pBits ) );
cv::Mat bufferMat = cv::Mat::zeros( height, width, CV_16UC1 );
cv::Mat playerMat = cv::Mat::zeros( height, width, CV_8UC3 );
NUI_DEPTH_IMAGE_PIXEL* pDepthPlayerPixel = reinterpret_cast<NUI_DEPTH_IMAGE_PIXEL*>( depthPlayerLockedRect.pBits );
pCordinateMapper->MapDepthFrameToColorFrame( NUI_IMAGE_RESOLUTION_640x480, width * height, pDepthPlayerPixel, NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, width * height, &pColorPoint[0] );
for( int y = 0; y < height; y++ ){
for( int x = 0; x < width; x++ ){
unsigned int index = y * width + x;
bufferMat.at<unsigned short>( pColorPoint[index].y, pColorPoint[index].x ) = pDepthPlayerPixel[index].depth;
playerMat.at<cv::Vec3b>( pColorPoint[index].y, pColorPoint[index].x ) = color[pDepthPlayerPixel[index].playerIndex];
}
}
cv::Mat depthMat( height, width, CV_8UC1 );
bufferMat.convertTo( depthMat, CV_8U, -255.0f / 10000.0f, 255.0f );
cv::Mat skeletonMat = cv::Mat::zeros( height, width, CV_8UC3 );
NUI_COLOR_IMAGE_POINT colorPoint;
for( int count = 0; count < NUI_SKELETON_COUNT; count++ ){
NUI_SKELETON_DATA skeletonData = skeletonFrame.SkeletonData[count];
if( skeletonData.eTrackingState == NUI_SKELETON_TRACKED ){
for( int position = 0; position < NUI_SKELETON_POSITION_COUNT; position++ ){
pCordinateMapper->MapSkeletonPointToColorPoint( &skeletonData.SkeletonPositions[position], NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, &colorPoint );
if( ( colorPoint.x >= 0 ) && ( colorPoint.x < width ) && ( colorPoint.y >= 0 ) && ( colorPoint.y < height ) ){
cv::circle( skeletonMat, cv::Point( colorPoint.x, colorPoint.y ), 10, static_cast<cv::Scalar>( color[count + 1] ), -1, CV_AA );
}
}
std::stringstream ss;
ss << skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER].z;
pCordinateMapper->MapSkeletonPointToColorPoint( &skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HEAD], NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, &colorPoint );
if( ( colorPoint.x >= 0 ) && ( colorPoint.x < width ) && ( colorPoint.y >= 0 ) && ( colorPoint.y < height ) ){
cv::putText( skeletonMat, ss.str(), cv::Point( colorPoint.x - 50, colorPoint.y - 20 ), cv::FONT_HERSHEY_SIMPLEX, 1.5f, static_cast<cv::Scalar>( color[count + 1] ) );
}
}
else if( skeletonData.eTrackingState == NUI_SKELETON_POSITION_ONLY ){
pCordinateMapper->MapSkeletonPointToColorPoint( &skeletonData.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER], NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, &colorPoint );
if( ( colorPoint.x >= 0 ) && ( colorPoint.x < width ) && ( colorPoint.y >= 0 ) && ( colorPoint.y < height ) ){
cv::circle( skeletonMat, cv::Point( colorPoint.x, colorPoint.y ), 10, static_cast<cv::Scalar>( color[count + 1] ), -1, CV_AA );
}
}
}
cv::imshow( "Color", colorMat );
cv::imshow( "Depth", depthMat );
cv::imshow( "Player", playerMat );
cv::imshow( "Skeleton", skeletonMat );
pColorFrameTexture->UnlockRect( 0 );
pDepthPlayerFrameTexture->UnlockRect( 0 );
pSensor->NuiImageStreamReleaseFrame( hColorHandle, &colorImageFrame );
pSensor->NuiImageStreamReleaseFrame( hDepthPlayerHandle, &depthPlayerImageFrame );
if( cv::waitKey( 30 ) == VK_ESCAPE ){
break;
}
}
pSensor->NuiShutdown();
pSensor->NuiSkeletonTrackingDisable();
pCordinateMapper->Release();
CloseHandle( hColorEvent );
CloseHandle( hDepthPlayerEvent );
CloseHandle( hSkeletonEvent );
cv::destroyAllWindows();
return 0;
}
int main(void)
{
//Set the error callback
glfwSetErrorCallback(error_callback);
//Initialize GLFW
if (!glfwInit())
{
exit(EXIT_FAILURE);
}
//Set the GLFW window creation hints - these are optional
//glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); //Request a specific OpenGL version
//glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); //Request a specific OpenGL version
//glfwWindowHint(GLFW_SAMPLES, 4); //Request 4x antialiasing
//glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
//Create a window and create its OpenGL context
window = glfwCreateWindow(960, 720, "Test Window", NULL, NULL);
//If the window couldn't be created
if (!window)
{
fprintf(stderr, "Failed to open GLFW window.\n");
glfwTerminate();
//exit(EXIT_FAILURE);
}
//This function makes the context of the specified window current on the calling thread.
glfwMakeContextCurrent(window);
//Sets the key callback
glfwSetKeyCallback(window, key_callback);
//Initialize GLEW
GLenum err = glewInit();
//If GLEW hasn't initialized
if (err != GLEW_OK)
{
fprintf(stderr, "Error: %s\n", glewGetErrorString(err));
return -1;
}
//Set a background color
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
glfwSetCursorPos(window, 1024 / 2, 768 / 2);
GLuint VertexArrayID;
glGenVertexArrays(1, &VertexArrayID);
glBindVertexArray(VertexArrayID);
// Create and compile our GLSL program from the shaders
GLuint red = LoadShaders("SimpleTransform.vertexshader", "SingleColorRed.fragmentshader");
GLuint grid = LoadShaders("SimpleTransform.vertexshader", "SingleColorGrid.fragmentshader");
glBindFragDataLocation(red, 0, "red");
glBindFragDataLocation(grid, 1, "grid");
// Get a handle for our "MVP" uniform
GLuint MatrixID = glGetUniformLocation(red, "MVP");
// Projection matrix : 45° Field of View, 4:3 ratio, display range : 0.1 unit <-> 100 units
glm::mat4 Projection = glm::perspective(45.0f, 4.0f / 3.0f, 0.1f, 1000.0f);
// Or, for an ortho camera :
//glm::mat4 Projection = glm::ortho(-10.0f,10.0f,-10.0f,10.0f,0.0f,100.0f); // In world coordinates
// Camera matrix
glm::mat4 View = glm::lookAt(
glm::vec3(4, 3, 3), // Camera is at (4,3,3), in World Space
glm::vec3(0, 0, 0), // and looks at the origin
glm::vec3(0, 1, 0) // Head is up (set to 0,-1,0 to look upside-down)
);
static const GLfloat g_vertex_buffer_data[] = {
-1.0f, -1.0f, 0.0f,
1.0f, -1.0f, 0.0f,
0.0f, 1.0f, 0.0f,
};
static const GLushort g_element_buffer_data[] = { 0, 1, 2 };
GLuint vertexbuffer;
glGenBuffers(1, &vertexbuffer);
glBindBuffer(GL_ARRAY_BUFFER, vertexbuffer);
glBufferData(GL_ARRAY_BUFFER, sizeof(g_vertex_buffer_data), g_vertex_buffer_data, GL_STATIC_DRAW);
static const GLfloat g_triangle_buffer_data[] = {
-1.0f, -1.0f, -1.0f,
1.0f, -1.0f, -1.0f,
0.0f, 1.0f, -1.0f,
};
GLuint triangle;
glGenBuffers(1, &triangle);
glBindBuffer(GL_ARRAY_BUFFER, triangle);
glBufferData(GL_ARRAY_BUFFER, sizeof(g_triangle_buffer_data), g_triangle_buffer_data, GL_STATIC_DRAW);
// Enable depth test
glEnable(GL_DEPTH_TEST);
// Accept fragment if it closer to the camera than the former one
glDepthFunc(GL_LESS);
glEnable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
glEnable(GL_SMOOTH);//OPENGL INSTANTIATION
HRESULT hr;
NUI_IMAGE_FRAME depthFrame;
HANDLE hDepth;
INuiSensor* pNuiSensor = NULL;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
return hr;
for (int i = 0; i < iSensorCount; i++)
{
INuiSensor* tempSensor;
hr = NuiCreateSensorByIndex(i, &tempSensor);
if (FAILED(hr))
continue;
hr = tempSensor->NuiStatus();
if (S_OK == hr)
{
pNuiSensor = tempSensor;
break;
}
tempSensor->Release();
}
for (int i = 0; i < 2048; i++) {
depthLookUp[i] = rawDepthToMeters(i);
}
rotation = getRotationMatrix(theta, psi, fi);
pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH);
pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH,
NUI_IMAGE_RESOLUTION_320x240,
0,
2,
NULL,
&hDepth);//KINECT INSTANTIATION
cout << "Starting Main Loop";
static double lastTime = glfwGetTime();
//Main Loop
do
{
double currentTime = glfwGetTime();
float deltaTime = float(currentTime - lastTime);
//Clear color buffer
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glUseProgram(grid);
modelMatrix(MatrixID);
hr = pNuiSensor->NuiImageStreamGetNextFrame(hDepth, 0, &depthFrame);
if (!FAILED(hr))
{
INuiFrameTexture* pTexture;
NUI_LOCKED_RECT LockedRect;
hr = pNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(
hDepth, &depthFrame, false, &pTexture);
if (FAILED(hr))
{
pNuiSensor->NuiImageStreamReleaseFrame(hDepth, &depthFrame);
continue;
}
pTexture->LockRect(0, &LockedRect, NULL, 0);//Kinect Image Grab
int skipX = 1;
int skipY = 1;
float scalar = 4.0f;
if (LockedRect.Pitch != 0)
{
for (int x = 0; x < width; x += skipX)
{
for (int y = 0; y < height; y += skipY)
{
const NUI_DEPTH_IMAGE_PIXEL * pBufferRun = reinterpret_cast<const NUI_DEPTH_IMAGE_PIXEL *>(LockedRect.pBits) + x + y * width;
//float depth = (float)(pBufferRun->depth);
//glm::vec3 location = realWorld(depth, height - y, x, 500.0f, 1000.0f);
//createCube(0.006f, location);
Vector4 locationDepth = NuiTransformDepthImageToSkeleton(x, y, (short)(pBufferRun->depth << 3));
glm::vec3 locationDepthxyz = glm::vec3(locationDepth.x * scalar, locationDepth.y * scalar, locationDepth.z * scalar);
createCube(0.009f, locationDepthxyz);
}
}
}
pTexture->UnlockRect(0);
pTexture->Release();
pNuiSensor->NuiImageStreamReleaseFrame(hDepth, &depthFrame);
}
createGrid();
//Test drawings
/*
glUseProgram(red);
modelMatrix(MatrixID);
//createCube(0.05f, glm::vec3(1.0f,1.0f,1.0f));
// 1rst attribute buffer : vertices
glEnableVertexAttribArray(0);
//createObject(vertexbuffer, GL_TRIANGLES, 3);
//createObject(triangle, GL_TRIANGLES, 3);
glDisableVertexAttribArray(0);
*/
//Swap buffers
glfwSwapBuffers(window);
//Get and organize events, like keyboard and mouse input, window resizing, etc...
glfwPollEvents();
std::string title = "Title | DELTA TIME " + std::to_string(1.0f/deltaTime);
const char* pszConstString = title.c_str();
glfwSetWindowTitle(window, pszConstString);
lastTime = currentTime;
} //Check if the ESC key had been pressed or if the window had been closed
while (!glfwWindowShouldClose(window));
//Close OpenGL window and terminate GLFW
glfwDestroyWindow(window);
//Finalize and clean up GLFW
glfwTerminate();
exit(EXIT_SUCCESS);
}
/// <summary>
/// Create the first connected Kinect found
/// </summary>
/// <returns>S_OK on success, otherwise failure code</returns>
HRESULT KinectEasyGrabber::CreateFirstConnected()
{
INuiSensor * pNuiSensor;
HRESULT hr;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
{
return hr;
}
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr))
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr)
{
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
if (NULL != m_pNuiSensor)
{
// Initialize the Kinect and specify that we'll be using depth
#ifdef RECORD_PLAYER
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_COLOR);
#else
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH | NUI_INITIALIZE_FLAG_USES_COLOR);
#endif
if (SUCCEEDED(hr))
{
// Create an event that will be signaled when depth data is available
m_hNextDepthFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
#ifdef RECORD_PLAYER
// Open a depth image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX,
cDepthResolution,
0,
2,
m_hNextDepthFrameEvent,
&m_pDepthStreamHandle);
#else
// Open a depth image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH,
cDepthResolution,
0,
2,
m_hNextDepthFrameEvent,
&m_pDepthStreamHandle);
#endif
// Create an event that will be signaled when color data is available
m_hNextColorFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a color image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_COLOR,
cColorResolution,
0,
2,
m_hNextColorFrameEvent,
&m_pColorStreamHandle);
}
}
if (NULL == m_pNuiSensor || FAILED(hr))
{
SetStatusMessage(L"No ready Kinect found!");
return E_FAIL;
}
m_pNuiSensor->NuiSkeletonTrackingDisable();
return hr;
}
/*!
//\par Description:
// Sets all memeber to their default values,
// reads configuration from settings file and tries to find first connected kinect device.
//
//\par Return value:
// True value is a expected value. When this method returns false then something wrong is with
// with kinect sensor. It can be disconnected or drivers are incompatible. First thing to do
// in this case is to check green led on kinect sensor. It should shines all the time. Flashing
// is not allowed.
//
//\retval true kinect is ready to work
//\retval false kinect is not found
*/
bool CKinectHandler::createFirstConnectedKinectSensor()
{
qDebug() << "createFirstConnectedKinectSensor()";
INuiSensor * pSensor;
int iSensorCount = 0;
// HRESULT - just standard long
HRESULT hr = NuiGetSensorCount( & iSensorCount );
if ( FAILED( hr ) ) // checks if hr is less than 0
{
qDebug() << "Cannot get available kinect sensor count";
return false;
}
// checks all sensors and takes first one which can be created
for ( int i = 0 ; i < iSensorCount ; i++ )
{
hr = NuiCreateSensorByIndex( i, & pSensor );
if ( FAILED( hr ) )
{
qDebug() << "Cannot create sensor with index: " << i << ", check next index";
continue;
}
hr = pSensor->NuiStatus();
if ( hr == S_OK ) // sensor is connected, it can be initialized now
{
m_pSensor = pSensor;
qDebug() << "Sensor with index: " << i << " is connected - breaking loop";
break;
}
// release not ok sensor
pSensor->Release();
}
if ( m_pSensor != NULL )
{
hr = m_pSensor->NuiInitialize( NUI_INITIALIZE_FLAG_USES_SKELETON ); // in order to use skeleton
if ( SUCCEEDED( hr ) )
{
// create event to inform application about new skeleton frame
m_hNextSkeletonEvent = CreateEventW( NULL, TRUE, FALSE, NULL );
// open skeleton stream to get skeleton data
hr = m_pSensor->NuiSkeletonTrackingEnable( m_hNextSkeletonEvent, 0 );
}
}
if ( m_pSensor == NULL || FAILED( hr ) )
{
qDebug() << "No ready kinect found";
m_kinectDetails.insert( "connection", false );
return false;
}
m_kinectDetails.insert( "connection", true );
m_kinectDetails.insert( "skeleton_found", false );
return true;
}
// In current version, we can't set the resolution
// All sensors of Kinect is activated.
HRESULT Kinect::createFirstConnected(){
INuiSensor * pNuiSensor;
HRESULT hr;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
{
return hr;
}
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr))
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr)
{
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
if (NULL != m_pNuiSensor)
{
// Initialize the Kinect and specify that we'll be using color, depth and skeletion
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR |
NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX |
NUI_INITIALIZE_FLAG_USES_SKELETON);
if (SUCCEEDED(hr))
{
// Create an event that will be signaled when depth data is available
m_hNextDepthFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a depth image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX,
NUI_IMAGE_RESOLUTION_640x480,
NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE|NUI_IMAGE_STREAM_FLAG_DISTINCT_OVERFLOW_DEPTH_VALUES,
//NUI_IMAGE_STREAM_FRAME_LIMIT_MAXIMUM,
2,
m_hNextDepthFrameEvent,
&m_hDepthStreamHandle);
//m_pNuiSensor->NuiImageStreamSetImageFrameFlags(&m_hDepthStreamHandle, NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE|NUI_IMAGE_STREAM_FLAG_DISTINCT_OVERFLOW_DEPTH_VALUES);
}
if (SUCCEEDED(hr))
{
// Open a color image stream to receive depth frames
m_hNextColorFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_COLOR,
NUI_IMAGE_RESOLUTION_640x480,
0,
2,
m_hNextColorFrameEvent,
&m_hColorStreamHandle);
}
if (SUCCEEDED(hr)){
// Enable the skeleton tracking
m_hNextSkeletonEvent = CreateEventW(NULL, TRUE, FALSE, NULL);
//hr = m_pNuiSensor->NuiSkeletonTrackingEnable(m_hNextSkeletonEvent, 0);
hr = m_pNuiSensor->NuiSkeletonTrackingEnable(
m_hNextSkeletonEvent, NUI_SKELETON_TRACKING_FLAG_ENABLE_IN_NEAR_RANGE | NUI_SKELETON_TRACKING_FLAG_ENABLE_SEATED_SUPPORT);
}
}
if (NULL == m_pNuiSensor || FAILED(hr))
{
return E_FAIL;
}
m_bInitialized = true;
return hr;
}
KinectImageSource::KinectImageSource(int device) : ImageSource(lexical_cast<string>(device)), frame() {
#ifdef WITH_MSKINECT_SDK
m_pColorStreamHandle = INVALID_HANDLE_VALUE;
/// Create the first connected Kinect found
INuiSensor * pNuiSensor;
HRESULT hr;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
{
std::cout << "Error getting sensor count. No Kinect plugged in?" << hr << std::endl;
}
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr))
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr)
{
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
if (NULL != m_pNuiSensor)
{
// Initialize the Kinect and specify that we'll be using color
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR);
if (SUCCEEDED(hr))
{
// Create an event that will be signaled when color data is available
//m_hNextColorFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a color image stream to receive color frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_COLOR,
NUI_IMAGE_RESOLUTION_640x480,
0,
2,
NULL,
&m_pColorStreamHandle);
}
}
if (NULL == m_pNuiSensor || FAILED(hr))
{
std::cout << "No ready Kinect found!" << std::endl;
}
//std::cout << "hr: " << hr << std::endl;
#else
std::cerr << "Error! This is the Microsoft Kinect SDK interface and not available under Linux." << std::endl;
#endif
}
int _tmain(int argc, _TCHAR* argv[])
{
cv::setUseOptimized( true );
// Kinectのインスタンス生成、初期化
INuiSensor* pSensor;
HRESULT hResult = S_OK;
hResult = NuiCreateSensorByIndex( 0, &pSensor );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiCreateSensorByIndex" << std::endl;
return -1;
}
hResult = pSensor->NuiInitialize( NUI_INITIALIZE_FLAG_USES_COLOR );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiInitialize" << std::endl;
return -1;
}
// Colorストリームの開始
HANDLE hColorEvent = INVALID_HANDLE_VALUE;
HANDLE hColorHandle = INVALID_HANDLE_VALUE;
hColorEvent = CreateEvent( nullptr, true, false, nullptr );
hResult = pSensor->NuiImageStreamOpen( NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, hColorEvent, &hColorHandle );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiImageStreamOpen( COLOR )" << std::endl;
return -1;
}
HANDLE hEvents[1] = { hColorEvent };
cv::namedWindow( "Color" );
while( 1 ){
// フレームの更新待ち
ResetEvent( hColorEvent );
WaitForMultipleObjects( ARRAYSIZE( hEvents ), hEvents, true, INFINITE );
// Colorカメラからフレームを取得
NUI_IMAGE_FRAME pColorImageFrame = { 0 };
hResult = pSensor->NuiImageStreamGetNextFrame( hColorHandle, 0, &pColorImageFrame );
if( FAILED( hResult ) ){
std::cerr << "Error : NuiImageStreamGetNextFrame( COLOR )" << std::endl;
return -1;
}
// Color画像データの取得
INuiFrameTexture* pColorFrameTexture = pColorImageFrame.pFrameTexture;
NUI_LOCKED_RECT sColorLockedRect;
pColorFrameTexture->LockRect( 0, &sColorLockedRect, nullptr, 0 );
// 表示
cv::Mat colorMat( 480, 640, CV_8UC4, reinterpret_cast<uchar*>( sColorLockedRect.pBits ) );
cv::imshow( "Color", colorMat );
// フレームの解放
pColorFrameTexture->UnlockRect( 0 );
pSensor->NuiImageStreamReleaseFrame( hColorHandle, &pColorImageFrame );
// ループの終了判定(Escキー)
if( cv::waitKey( 30 ) == VK_ESCAPE ){
break;
}
}
// Kinectの終了処理
pSensor->NuiShutdown();
CloseHandle( hColorEvent );
CloseHandle( hColorHandle );
cv::destroyAllWindows();
return 0;
}
HRESULT AcquisitionKinect1::CreateFirstConnected(){
INuiSensor * pNuiSensor = NULL;
HRESULT hr;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
cout << "sensori collegati: " << iSensorCount << endl;
if (FAILED(hr))
{
return hr;
}
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr))
{
continue;
}
cout << "Sensore " << i << " creato." << endl;
hr = m_frameHelper.Initialize(pNuiSensor); // QUI E' DOVE CI METTE TANTO TEMPO
cout << "Sensore " << i << " inizializzato." << endl;
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr)
{
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
/*
if (NULL != m_pNuiSensor)
{
// Initialize the Kinect and specify that we'll be using color, depth and skeleton
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON);
if (SUCCEEDED(hr))
{
// Create an event that will be signaled when depth data is available
m_hNextDepthFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a color image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX,
NUI_IMAGE_RESOLUTION_640x480,
0,
2,
m_hNextDepthFrameEvent,
&m_pDepthStreamHandle);
if (FAILED(hr)) return hr;
// Create an event that will be signaled when color data is available
m_hNextColorFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a color image stream to receive color frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_COLOR,
NUI_IMAGE_RESOLUTION_640x480,
0,
2,
m_hNextColorFrameEvent,
&m_pColorStreamHandle);
if (FAILED(hr)) return hr;
}
}
*/
return hr;
}
HRESULT KinectSDKGrabber::CreateFirstConnected()
{
INuiSensor *pNuiSensor;
HRESULT hr;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
{
return hr;
}
std::cout << "Count" << std::endl;
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr))
{
continue;
}
std::cout << "Created" << std::endl;
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr)
{
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
if (NULL != m_pNuiSensor)
{
// Initialize the Kinect and specify that we'll be using depth
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH | NUI_INITIALIZE_FLAG_USES_COLOR);
if (SUCCEEDED(hr))
{
// Create an event that will be signaled when depth data is available
m_hNextDepthFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
// Open a depth image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH,
NUI_IMAGE_RESOLUTION_640x480,
0,
2,
m_hNextDepthFrameEvent,
&m_pDepthStreamHandle);
m_pNuiSensor->NuiImageStreamSetImageFrameFlags(m_pDepthStreamHandle, NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE);
std::cout << "Depth" << std::endl;
}
if (SUCCEEDED(hr))
{
m_hNextColorFrameEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_COLOR,
NUI_IMAGE_RESOLUTION_640x480,
0,
2,
m_hNextColorFrameEvent,
&m_pColorStreamHandle);
std::cout << "Color" << std::endl;
}
}
if (NULL == m_pNuiSensor || FAILED(hr))
{
return E_FAIL;
}
return hr;
}
bool KinectInput::initSensor()
{
INuiSensor* tempSensor;
int numSensors = 0;
if (NuiGetSensorCount(&numSensors) != S_OK )
{
std::cout << "Kinect Error: Sensor lookup failed, Probably no sensors connected" << std::endl;
std::getchar();
return false;
}
// Loop through all sensors
for (int i = 0; i < numSensors; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
if (NuiCreateSensorByIndex(i, &tempSensor) < 0)
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
if (tempSensor->NuiStatus() == S_OK)
{
kinectSensor = tempSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
tempSensor->Release();
}
if (kinectSensor != NULL)
{
// Initialize the Kinect and specify that we'll be using skeleton
if (kinectSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_SKELETON) >= 0)
{
// Create an event that will be signaled when skeleton data is available
nextSkeletonUpdate = CreateEventW(NULL, TRUE, FALSE, NULL);
// Open a skeleton stream to receive skeleton data
if (kinectSensor->NuiSkeletonTrackingEnable(nextSkeletonUpdate, 0) < 0)
{
std::cout << "Kinect Error: Couldn't open a skeleton stream" << std::endl;
std::getchar();
return false;
}
}
else
{
std::cout << "Kinect Error: Sensor failed initialization" << std::endl;
std::getchar();
return false;
}
}
else
{
std::cout << "Kinect Error: No sensor could be initialized" << std::endl;
std::getchar();
return false;
}
return true;
}
/// <summary>
/// Create the first connected Kinect found
/// </summary>
/// <returns>indicates success or failure</returns>
HRESULT CKinectFusion::CreateFirstConnected()
{
INuiSensor * pNuiSensor;
HRESULT hr;
int iSensorCount = 0;
hr = NuiGetSensorCount(&iSensorCount);
if (FAILED(hr))
{
std::cout << "No ready Kinect found!" << std::endl;
return hr;
}
// Look at each Kinect sensor
for (int i = 0; i < iSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &pNuiSensor);
if (FAILED(hr))
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = pNuiSensor->NuiStatus();
if (S_OK == hr)
{
m_pNuiSensor = pNuiSensor;
break;
}
// This sensor wasn't OK, so release it since we're not using it
pNuiSensor->Release();
}
if (nullptr != m_pNuiSensor)
{
// Initialize the Kinect and specify that we'll be using depth
hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH);
if (SUCCEEDED(hr))
{
// Create an event that will be signaled when depth data is available
m_hNextDepthFrameEvent = CreateEvent(nullptr, TRUE, FALSE, nullptr);
// Open a depth image stream to receive depth frames
hr = m_pNuiSensor->NuiImageStreamOpen(
NUI_IMAGE_TYPE_DEPTH,
m_imageResolution,
0,
2,
m_hNextDepthFrameEvent,
&m_pDepthStreamHandle);
}
if (m_bNearMode)
{
// Set near mode based on our internal state
HRESULT nearHr = m_pNuiSensor->NuiImageStreamSetImageFrameFlags(m_pDepthStreamHandle, NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE);
}
}
if (nullptr == m_pNuiSensor || FAILED(hr))
{
std::cout << "No ready Kinect found!" << std::endl;
return E_FAIL;
}
return hr;
}