///////////////////////////////////////////////////////////////////////////////
//
// CAmbientLightAwareSensorManagerEvents::RemoveSensor
//
// Description of function/method:
// Helper function, clears the event sink for the sensor and then
// releases the sensor.
//
// Parameters:
// REFSENSOR_ID sensorID: Input sensor
//
// Return Values:
// S_OK on success, else an error
//
///////////////////////////////////////////////////////////////////////////////
HRESULT CAmbientLightAwareSensorManagerEvents::RemoveSensor(REFSENSOR_ID sensorID)
{
HRESULT hr = S_OK;
if (m_Sensors.Lookup(sensorID))
{
ISensor* pSensor = m_Sensors[sensorID];
m_Sensors.RemoveKey(sensorID);
pSensor->Release();
}
else
{
hr = HRESULT_FROM_WIN32(ERROR_NOT_FOUND);
}
return hr;
}
int fuckmain(void)
{
int numberOfGestures = 0;
int ret = 0;
// camera interface
ISensor* sensor = NULL;
// Omek API interface
IMotionSensor *pSensor = NULL;
// we want to read the input without waiting... (for the exit key)
int n, tem;
tem = fcntl(0, F_GETFL, 0);
fcntl (0, F_SETFL, (tem | O_NDELAY));
disable_waiting_for_enter();
char* sequencePath = NULL;
for(int i=1; i < 2; i++)
{
if(strcmp("-gest", "-help") == 0)
{
cout << "Usage: " << " [-seq sequence-path] [-gest gesture name] [-gest...\n";
goto end;
}
if(strcmp("-gest", "-gest") == 0)
{
if(numberOfGestures >= TOTAL_NUMBER_OF_SUPPORTED_GESTURES)
{
cerr << "Error: currently we support only " << TOTAL_NUMBER_OF_SUPPORTED_GESTURES << " gestures." << endl;
ret = 1;
goto end;
}
}
}
// create the sensor
if (sequencePath)
{
// from a sequence
pSensor = IMotionSensor::createSequenceSensor(sequencePath);
}
else
{
// from a cameras
pSensor = IMotionSensor::createCameraSensor(true);
}
if(pSensor == NULL)
{
cerr << "Error, failed creating sensor." << endl;
ret = 1;
goto end;
}
// initialize the tracking algorithm
if(pSensor->setTrackingOptions(TRACK_ALL) != OMK_SUCCESS)
{
cerr << "Error, failed to set tracking options." << endl;
ret = 1;
goto end;
}
sensor = pSensor->getSensor();
if(sensor == NULL)
{
cerr << "Error, failed to get sensor." << endl;
ret = 1;
goto end;
}
// disable the RGB input (not supported on the BeagleBoard)
pSensor->getSensor()->setCameraParameter("enableRGB", 0);
// enable the selected gestures
numberOfGestures=1;
for(int i = 0 ; i < numberOfGestures; i++)
{
if(pSensor->enableGesture("_rightScrollRight") != OMK_SUCCESS)
{
cerr << "Error, unrecognized gesture: " << "_rightScrollRight" << endl;
ret = 1;
goto end;
}
}
// setting the maximal number of players
if(pSensor->setMaxPlayers(MAX_NUM_OF_PLAYERS) != OMK_SUCCESS)
{
cerr << "Error, failed to set maximal number of players." << endl;
ret = 1;
goto end;
}
cout << "Please press ESC to stop." << endl;
{
uint32_t processedFrames = 0;
// run the main loop as long as there are frames to process
while (sensor->isAlive() && g_run_gestures)
{
// Handle the current frame only if we have successfully processed a new image
bool bHasNewImage = false;
if ((pSensor->processNextImage(true, bHasNewImage) == OMK_SUCCESS) && bHasNewImage)
{
processedFrames++;
while (pSensor->hasMoreGestures())
{
const IFiredEvent* pFiredEvent = pFiredEvent = pSensor->popNextGesture();
std::stringstream text;
text << endl << "Gesture (" << (pFiredEvent->getName()!=NULL?pFiredEvent->getName():"") << ") fired in frame " << processedFrames;
cout << text.str().c_str() << endl;
pSensor->releaseGesture(pFiredEvent);
g_run_gestures = 0;
}
}
// char c;
// n = read(0, &c, 1);
// if (n > 0)
// if(c == 0x1B)
// g_run_gestures = 0;
}
cout << processedFrames << " frames." << endl;
}
end:
// cleanup
if (pSensor != NULL)
{
IMotionSensor::releaseMotionSensor(pSensor);
pSensor = NULL;
}
fcntl(0, F_SETFL, tem);
restore_terminal_settings();
return ret;
}
///////////////////////////////////////////////////////////////////////////////
//
// CSensorManagerEvents::Initialize
//
// Description of function/method:
// Initialize the sensor data.
//
// Parameters:
// none
//
// Return Values:
// HRESULT S_OK on success
//
///////////////////////////////////////////////////////////////////////////////
HRESULT CSensorManagerEvents::Initialize(int NumSensorTypes, ...)
{
HRESULT hr;
va_list vl;
SENSOR_TYPE_ID TypeID;
va_start( vl, NumSensorTypes );
// Step through the list.
for ( int x = 0; x < NumSensorTypes; x++ ) // Loop until all numbers are added
{
TypeID = va_arg( vl, SENSOR_TYPE_ID );
AddRequestedSensor(TypeID);
}
va_end( vl );
hr = ::CoCreateInstance(CLSID_SensorManager, NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&m_spISensorManager));
if (FAILED(hr))
{
OutputDebugString(L"Unable to CoCreateInstance() the SensorManager.");
return E_FAIL;
}
if (SUCCEEDED(hr))
{
hr = m_spISensorManager->SetEventSink(this);
if (SUCCEEDED(hr))
{
// Find all Ambient Light Sensors
ISensorCollection* spSensors;
hr = m_spISensorManager->GetSensorsByCategory(SENSOR_CATEGORY_ALL, &spSensors);
if(FAILED(hr))
{
OutputDebugString(L"Unable to find any sensors on the computer.");
return E_FAIL;
}
if (SUCCEEDED(hr) && NULL != spSensors)
{
ULONG ulCount = 0;
hr = spSensors->GetCount(&ulCount);
if (SUCCEEDED(hr))
{
// Make a SensorCollection with only the sensors we want to get permission to access.
ISensorCollection *pSensorCollection = NULL;
HRESULT hr = ::CoCreateInstance(CLSID_SensorCollection, NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&pSensorCollection));
if (FAILED(hr))
{
OutputDebugString(L"Unable to CoCreateInstance() a SensorCollection.");
return E_FAIL;
}
for(ULONG i=0; i < ulCount; i++)
{
ISensor* spSensor;
hr = spSensors->GetAt(i, &spSensor);
SENSOR_TYPE_ID idType = GUID_NULL;
hr = spSensor->GetType(&idType);
std::list<SensorRequest*> ::iterator it;
for (it=RequestedSensors.begin(); it!=RequestedSensors.end() ; ++it)
{
if((*it)->m_Type == idType)
break;
}
if(it==RequestedSensors.end())
{
// we have never requested this sensor;
continue;
}
pSensorCollection->Clear();
pSensorCollection->Add(spSensor);
// Have the SensorManager prompt the end-user for permission.
hr = m_spISensorManager->RequestPermissions(NULL, pSensorCollection, TRUE);
if (FAILED(hr))
{
Setstatus(GUID_NULL, SENSOR_STATUS_DISABLED);
OutputDebugString(L"No permission to access Requested Sensor.");
}
spSensor->Release();
}
pSensorCollection->Release();
for(ULONG i=0; i < ulCount; i++)
{
ISensor* spSensor;
hr = spSensors->GetAt(i, &spSensor);
if (SUCCEEDED(hr))
{
SensorState state = SENSOR_STATE_ERROR;
hr = spSensor->GetState(&state);
OnSensorEnter(spSensor,state);
spSensor->Release();
}
}
}
}
spSensors->Release();
}
}
return hr;
}
void Tracking::updateFrame()
{
ISensor* sensor = m_pSensor->getSensor();
// run the main loop as long as there are frames to process
if (sensor->isAlive())
{
bool bHasNewImage;
// Processes the new input frame (if available) and executes the tracking algorithm
if(m_pSensor->processNextImage(true, bHasNewImage) == OMK_ERROR_ASSERTION_FAILURE)
emit shutdown();
int width_step;
// Copy the input depth image (if available) into the given buffer.
// (Note - the depth data represented by signed short per pixel)
int depthImageBufferSize = m_imageWidth*m_imageHeight*sizeof(uint16_t);
if(m_pSensor->copyRawImage((char*)m_depthBuffer ,
depthImageBufferSize,
width_step, false) == OMK_SUCCESS)
{
emit updateDepthImage(m_depthBuffer);
}
// the label of the player (only one exists)
int label = 0;
// the dimensions of the player's bounding rectangle
int width = 0, height = 0;
// the player blob's 3D center of mass in world space (cm)
float center3D[3];
// the player blob's 2D center of mass in local image space (pixels)
float center2D[2];
// do we have a players mask
int playerMaskBufferSize = m_imageWidth*m_imageHeight;
// Copy the player mask into the given buffer.
// (Note - the mask data is represented by char per pixel (255-player present/0-background))
if(m_pSensor->copyPlayerMask( (char*)m_maskBuffer,
playerMaskBufferSize,
label, width, height,
center3D, center2D) == OMK_SUCCESS)
{
// Copy the output raw skeleton into the given skeleton data structure.
if(m_pSensor->getRawSkeleton(m_pSkeleton) == OMK_SUCCESS)
{
if(m_pSkeleton != NULL)
{
// Run over all the joints in Skeleton and get their
// positions in image space.
int i = 4;
JointID id = (JointID)i;
// Check whether the joint is available (tracked)
// in the current frame.
if(m_pSkeleton->containsJoint(id))
{
float pos[3];
// Get the tracked joint position in the image space
// (ignore the z-coordinate) to draw them on the
// mask image (right application window) as cross marks.
//The x value increases when we move to left.
m_pSkeleton->getJointPosition(id, pos, false);
unsigned int x,y;
x = (unsigned int)pos[0];
y = (unsigned int)pos[1];
if((int)(x-pre_x) > 20)
{
//X increases, Turn left;
direction = 1;
}
else if((int)(x-pre_x) < -20)
{
//X decreases, Turn Right;
direction = 2;
}
else if((int)(y - pre_y) > 20)
{
//Y increases, Turn Down;
direction = 3;
}
else if((int)(y - pre_y) < -30)
{
//Y decreases, Turn Up;
direction = 4;
}
dir = direction;
pre_x = x;
pre_y = y;
/* QString str_x = QString::number(pre_x);
QString str_y = QString::number(pre_y);
QString position;
position = str_x + "," + str_y;
QString str_dir = QString::number(direction);
if(direction)
{
//QMessageBox::information(NULL, "Direction", str_dir, QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
}
*/ // offset from the image border to be taken when drawing markers,
// because of their size
const unsigned int imageBorderOffset = 2;
if( (x >= imageBorderOffset) &&
(y >= imageBorderOffset) &&
(x < (m_imageWidth-imageBorderOffset)) &&
(y < (m_imageHeight-imageBorderOffset)) )
{
// JointID_rightHip and JointID_leftHip joints are not valid
// in the raw skeleton.
if((id!=JointID_rightHip) && (id!=JointID_leftHip))
// Send the joint image coordinates to the TrackingWindow.
emit addPoint(x, y, id);
}
}//<=if(m_pSkeleton->containsJoint(id))
}//<=if(m_pSkeleton != NULL)
}//<=if(m_pSensor->getRawSkeleton..)
m_isTracking = true;
emit updateMaskImage(m_maskBuffer);
}//<=if(m_pSensor->copyPlayerMask..)
else
{
if(m_isTracking)
{
memset(m_maskBuffer, 0, m_imageWidth*m_imageHeight);
emit updateMaskImage(m_maskBuffer);
m_isTracking = false;
}
}
}//<=if(sensor->isAlive())
else
{
cout << "processed " << sensor->getFramenum() << "." << endl;
emit shutdown();
}
}
int Tracking::initialize(char* sequenceFileName, bool isUpper)
{
m_isTracking = true;
const unsigned int trackMode = (isUpper) ? TRACK_UPPERBODY : TRACK_ALL;
// create the sensor
if (sequenceFileName == NULL)
// create live camera sensor
m_pSensor = IMotionSensor::createCameraSensor();
else
// create a sequence file sensor (file containing sequence of recorded depth data frames)
m_pSensor = IMotionSensor::createSequenceSensor(sequenceFileName);
if(m_pSensor == NULL)
{
cerr << "Error creating sensor." << endl;
return 1;
}
// enable skeleton tracking
if(m_pSensor->setTrackingOptions(trackMode) != OMK_SUCCESS)
{
cerr << "Error initializing the tracking algorithm." << endl;
return 1;
}
// setting the maximal number of players (currently supported maximum 1 on BeagleBoard)
if(m_pSensor->setMaxCandidates(MAX_NUM_OF_PLAYERS) != OMK_SUCCESS)
{
cerr << " Error setting maximal number of candidates." << endl;
return 1;
}
if(m_pSensor->setMaxPlayers(MAX_NUM_OF_PLAYERS) != OMK_SUCCESS)
{
cerr << "Error setting maximal number of players." << endl;
return 1;
}
// Allocate empty Skeleton, to receive output data from the
// tracking algorithm.
m_pSkeleton = m_pSensor->createSkeleton();
if(m_pSkeleton == NULL)
{
cerr << "NULL Skeleton !!!" << endl;
return 1;
}
// Disable the RGB input (not supported on the BeagleBoard).
ISensor* sensor = m_pSensor->getSensor();
//sensor->setCameraParameter("enableRGB", 0);
m_imageWidth = sensor->getImageWidth(IMAGE_TYPE_DEPTH);
m_imageHeight = sensor->getImageHeight(IMAGE_TYPE_DEPTH);
m_maskBuffer = new unsigned char[m_imageWidth*m_imageHeight];
m_depthBuffer = new unsigned char[m_imageWidth*m_imageHeight*sizeof(uint16_t)];
memset(m_maskBuffer, 0, m_imageWidth*m_imageHeight);
memset(m_depthBuffer, 0, m_imageWidth*m_imageHeight*sizeof(uint16_t));
cout << "Tracking successfully initialized." << endl;
return 0;
}
