// -----------------------------------------------------------------------------------------------------
// generateFrame
// -----------------------------------------------------------------------------------------------------
bool CameraDevice::generateFrame(IplImage* imgRGB, IplImage* imgDepth)
{
XnStatus nRetVal = XN_STATUS_OK;
const XnDepthPixel* pDepthMap = NULL;
const XnRGB24Pixel* pImageMap = NULL;
xnFPSMarkFrame(&g_xnFPS);
nRetVal = g_context.WaitAndUpdateAll();
if (nRetVal==XN_STATUS_OK)
{
g_depth.GetMetaData(g_depthMD);
g_image.GetMetaData(g_imageMD);
pDepthMap = g_depthMD.Data();
pImageMap = g_image.GetRGB24ImageMap();
printf("Frame %02d (%dx%d) Depth at middle point: %u. FPS: %f\r",
g_depthMD.FrameID(),
g_depthMD.XRes(),
g_depthMD.YRes(),
g_depthMD(g_depthMD.XRes()/2, g_depthMD.YRes()/2),
xnFPSCalc(&g_xnFPS));
// convert to OpenCV buffers
convertImageRGB(pImageMap, imgRGB);
convertImageDepth(pDepthMap, imgDepth);
return true;
}
return false;
}
XnStatus prepare(char useScene, char useDepth, char useHistogram)
{
//TODO handle possible failures!
if (useDepth)
{
mDepthGen.GetMetaData(depthMD);
nXRes = depthMD.XRes();
nYRes = depthMD.YRes();
pDepth = depthMD.Data();
if (useHistogram)
{
calcHist();
// rewind the pointer
pDepth = depthMD.Data();
}
}
if (useScene)
{
mUserGen.GetUserPixels(0, sceneMD);
nXRes = sceneMD.XRes();
nYRes = sceneMD.YRes();
pLabels = sceneMD.Data();
}
}
int main()
{
XnStatus nRetVal = XN_STATUS_OK;
Context context;
nRetVal = context.Init();
CHECK_RC(nRetVal, "Initialize context");
DepthGenerator depth;
nRetVal = depth.Create(context);
CHECK_RC(nRetVal, "Create depth generator");
nRetVal = context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGeneratingAll");
DepthMetaData depthMD;
while (!xnOSWasKeyboardHit())
{
nRetVal = context.WaitOneUpdateAll(depth);
if (nRetVal != XN_STATUS_OK)
{
printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
continue;
}
depth.GetMetaData(depthMD);
const XnDepthPixel* pDepthMap = depthMD.Data();
printf("Frame %d Middle point is: %u.\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2));
}
context.Shutdown();
return 0;
}
void GeneratePointCloud(DepthGenerator& rDepthGen, const XnDepthPixel* pDepth, VISION_DATA &pData)
{
DepthMetaData mDepthMD;
rDepthGen.GetMetaData(mDepthMD);
pData.timeStamp = mDepthMD.Timestamp();
unsigned int uPointNum = mDepthMD.FullXRes() * mDepthMD.FullYRes();
XnPoint3D* pDepthPointSet = new XnPoint3D[uPointNum];
unsigned int i, j, idxshift, idx;
for( j = 0; j < mDepthMD.FullYRes(); ++j)
{
idxshift = j * mDepthMD.FullXRes();
for(i = 0; i < mDepthMD.FullXRes(); ++i)
{
idx = idxshift + i;
pDepthPointSet[idx].X = i;
pDepthPointSet[idx].Y = j;
pDepthPointSet[idx].Z = pDepth[idx];
}
}
XnPoint3D* p3DPointSet = new XnPoint3D[uPointNum];
rDepthGen.ConvertProjectiveToRealWorld(uPointNum, pDepthPointSet, p3DPointSet);
memcpy(pData.pointCloud, p3DPointSet, uPointNum*3*sizeof(float));
delete[] pDepthPointSet;
delete[] p3DPointSet;
}
// Updates to the latest image obtained from the Kinect
int kinectUpdate(void)
{
XnStatus nRetVal = context.WaitAndUpdateAll();
g_image.GetMetaData(g_imageMD);
//nRetVal = context.WaitOneUpdateAll(depth);
depth.GetMetaData(depthMD);
return nRetVal;
}
int main()
{
XnStatus nRetVal = XN_STATUS_OK;
Context context;
EnumerationErrors errors;
nRetVal = context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
DepthGenerator depth;
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
CHECK_RC(nRetVal, "Find depth generator");
XnFPSData xnFPS;
nRetVal = xnFPSInit(&xnFPS, 180);
CHECK_RC(nRetVal, "FPS Init");
DepthMetaData depthMD;
while (!xnOSWasKeyboardHit())
{
nRetVal = context.WaitOneUpdateAll(depth);
if (nRetVal != XN_STATUS_OK)
{
printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
continue;
}
xnFPSMarkFrame(&xnFPS);
depth.GetMetaData(depthMD);
const XnDepthPixel* pDepthMap = depthMD.Data();
printf("Frame %d Middle point is: %u. FPS: %f\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2), xnFPSCalc(&xnFPS));
}
context.Shutdown();
return 0;
}
// Gets the colour and depth data from the Kinect sensor.
bool GetColorAndDepthImages(ColorImage& colorImage, DepthImage& depthImage)
{
XnStatus rc = XN_STATUS_OK;
// Read a new frame, blocking operation
rc = deviceContext.WaitAnyUpdateAll();
if (rc != XN_STATUS_OK)
{
/*LOGE("Read failed: %s\n", xnGetStatusString(rc));*/
throw rc;
}
// Get handles to new data
static ImageMetaData colorImageMetaData;
static DepthMetaData depthImageMetaData;
colorImageGenerator.GetMetaData(colorImageMetaData);
depthImageGenerator.GetMetaData(depthImageMetaData);
// Validate images
if (!depthImageGenerator.IsValid() || !colorImageGenerator.IsValid())
{
/*LOGE("Error: Color or depth image is invalid.");*/
throw 1;
}
if (colorImageMetaData.Timestamp() <= mostRecentRGB)
return false;
// Fetch pointers to data
const XnRGB24Pixel* pColorImage = colorImageMetaData.RGB24Data(); //g_depth.GetRGB24ImageMap()
const XnDepthPixel* pDepthImage = depthImageMetaData.Data();// g_depth.GetDepthMap();
// Copy data over to arrays
memcpy(colorImage.data, pColorImage, sizeof(colorImage.data));
memcpy(depthImage.data, pDepthImage, sizeof(depthImage.data));
colorImage.rows = colorImage.maxRows;
colorImage.cols = colorImage.maxCols;
depthImage.rows = depthImage.maxRows;
depthImage.cols = depthImage.maxCols;
mostRecentRGB = colorImageMetaData.Timestamp();
return true;
}
void glutDisplay (void){
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
SceneMetaData sceneMD;
DepthMetaData depthMD;
ImageMetaData imageMD;
g_DepthGenerator.GetMetaData(depthMD);
glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
glDisable(GL_TEXTURE_2D);
//XnStatus rc = g_Context.WaitOneUpdateAll(g_DepthGenerator);
XnStatus rc = g_Context.WaitAnyUpdateAll();
CHECK_RC("Wait Data",rc);
g_DepthGenerator.GetMetaData(depthMD);
if(g_UserGenerator.IsValid())
g_UserGenerator.GetUserPixels(0, sceneMD);
g_ImageGenerator.GetMetaData(imageMD);
DrawDepthMap(depthMD, sceneMD);
DrawImage(imageMD);
glutSwapBuffers();
}//glutdisplay
XnStatus prepare(char useScene, char useDepth, char useImage, char useIr, char useHistogram)
{
//TODO handle possible failures! Gotcha!
if (useDepth)
{
mDepthGen.GetMetaData(depthMD);
nXRes = depthMD.XRes();
nYRes = depthMD.YRes();
pDepth = depthMD.Data();
if (useHistogram)
{
calcHist();
// rewind the pointer
pDepth = depthMD.Data();
}
}
if (useScene)
{
mUserGen.GetUserPixels(0, sceneMD);
nXRes = sceneMD.XRes();
nYRes = sceneMD.YRes();
pLabels = sceneMD.Data();
}
if (useImage)
{
mImageGen.GetMetaData(imageMD);
nXRes = imageMD.XRes();
nYRes = imageMD.YRes();
pRGB = imageMD.RGB24Data();
// HISTOGRAM?????
}
if (useIr)
{
mIrGen.GetMetaData(irMD);
nXRes = irMD.XRes();
nYRes = irMD.YRes();
pIR = irMD.Data();
// HISTOGRAM????
}
}
/*
* Class: org_OpenNI_Samples_Assistant_NativeMethods
* Method: initFromContext
* Signature: (JZZ)I
*/
JNIEXPORT jint JNICALL
Java_org_OpenNI_Samples_Assistant_NativeMethods_initFromContext
(JNIEnv *env, jclass cls, jlong pContext, jboolean _hasUserGen, jboolean _hasDepthGen)
{
LOGD("init_start");
hasUserGen = _hasUserGen;
hasDepthGen = _hasDepthGen;
mContext = new Context((XnContext*) pContext);
if (!(hasUserGen || hasDepthGen))
return XN_STATUS_BAD_PARAM;
int rc;
if (hasUserGen)
{
rc = mContext->FindExistingNode(XN_NODE_TYPE_USER, mUserGen);
if (rc != XN_STATUS_OK)
{
//TODO log&retval
printf("No user node exists!");
return 1;
}
mUserGen.GetUserPixels(0, sceneMD);
}
if (hasDepthGen)
{
rc = mContext->FindExistingNode(XN_NODE_TYPE_DEPTH, mDepthGen);
if (rc != XN_STATUS_OK)
{
//TODO log&retval
printf("No depth node exists! Check your XML.");
return 1;
}
mDepthGen.GetMetaData(depthMD);
}
initGraphics();
LOGD("init_end");
return XN_STATUS_OK;
}
void takePhoto() {
static int index = 1;
char fname[256] = {0,};
sprintf(fname, "kinect%03d.txt", index++);
g_depth.GetMetaData(g_depthMD);
g_image.GetMetaData(g_imageMD);
int const nx = g_depthMD.XRes();
int const ny = g_depthMD.YRes();
assert(nx == g_imageMD.XRes());
assert(ny == g_imageMD.YRes());
const XnDepthPixel* pDepth = g_depthMD.Data();
const XnUInt8* pImage = g_imageMD.Data();
FILE * file = fopen(fname, "wb");
fprintf(file, "%d\n%d\n\n", nx, ny);
for (int y = 0, di = 0, ri = 0, gi = 1, bi = 2; y < ny; y++) {
for (int x = 0; x < nx; x++, di++, ri += 3, gi += 3, bi += 3) {
int const r = pImage[ri];
int const g = pImage[gi];
int const b = pImage[bi];
int const d = pDepth[di];
assert(r >= 0);
assert(g >= 0);
assert(b >= 0);
assert(d >= 0);
assert(r <= 0xFF);
assert(g <= 0xFF);
assert(b <= 0xFF);
assert(d <= 0xFFFF);
fprintf(file, "%3d %3d %3d %5d\n", r, g, b, d);
}
fprintf(file, "\n");
}
fflush(file);
fclose(file);
}
/*
* Function: run
*
* Starts and continues generating data from the Kinect.
* A loop runs and updates data whenever new data is available from one of the Kinect
* devices, and then the data is processed to check for patient movement.
* The loop is controlled by the "quit" global boolean, which is set to false by the
* signal handler "stop()"
*/
void KinectMonitor::run() {
XnStatus status;
SceneMetaData scene;
DepthMetaData depth;
// Start the device
status = context.StartGeneratingAll();
// Running loop
while( !quit ) {
// Wait for any new incoming data
context.WaitOneUpdateAll(depthGenerator);
// Mark the new frame
xnFPSMarkFrame(&xnFPS);
// Get the depth data from the device
depthGenerator.GetMetaData(depth);
// Get the recognized users
XnUInt16 numUsers = 15;
XnUserID users[numUsers];
userGenerator.GetUsers(users, numUsers);
// Only track the patient if they are alone
if( numUsers != 1) continue;
// Get the user data
userGenerator.GetUserPixels(users[0], scene);
// Update patient position
previous = current;
current = getPosition(users[0]);
// Raise alerts based on the patient's state and position
if( previous != current ) {
if( current == TURNED && out == false ) {
// Patient is turned
printf("Patient getting out of bed.\n");
} else if( out && bedSet ) {
printf("Patient is out of bed.\n");
}
}
}
}
void readFrame()
{
if (!g_Depth.IsValid() && !g_Image.IsValid() && !g_IR.IsValid() && !g_Audio.IsValid()) // @@@dded
return;
XnStatus rc = XN_STATUS_OK;
if (g_pPrimary != NULL)
{
rc = g_Context.WaitOneUpdateAll(*g_pPrimary);
}
else
{
rc = g_Context.WaitAnyUpdateAll();
}
if (rc != XN_STATUS_OK)
{
printf("Error: %s\n", xnGetStatusString(rc));
}
if (g_Depth.IsValid())
{
g_Depth.GetMetaData(g_DepthMD);
}
if (g_Image.IsValid())
{
g_Image.GetMetaData(g_ImageMD);
}
if (g_IR.IsValid())
{
g_IR.GetMetaData(g_irMD);
}
if (g_Audio.IsValid())
{
g_Audio.GetMetaData(g_AudioMD);
}
}
// Set up OpenNI to obtain 8-bit mono images from the Kinect's RGB camera
int kinectInit(void)
{
XnStatus nRetVal = XN_STATUS_OK;
ScriptNode scriptNode;
EnumerationErrors errors;
printf("Reading config from: '%s'\n", SAMPLE_XML_PATH_LOCAL);
nRetVal = context.InitFromXmlFile(SAMPLE_XML_PATH_LOCAL, scriptNode, &errors);
nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
//g_image.SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT);
g_image.SetPixelFormat(XN_PIXEL_FORMAT_RGB24);
g_image.GetMetaData(g_imageMD);
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
depth.GetMetaData(depthMD);
// nRetVal = depth.GetAlternativeViewPointCap().SetViewPoint(g_image);
//nRetVal = depth.GetFrameSyncCap().FrameSyncWith(g_image);
return nRetVal;
}
//--------------------------------------------------------------
void testApp::setup(){
XnStatus rc;
EnumerationErrors errors;
rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, g_scriptNode, &errors);
if (rc == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return ;
}
else if (rc != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(rc));
return;
}
rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
if (rc != XN_STATUS_OK)
{
printf("No depth node exists! Check your XML.");
return;
}
g_depth.GetMetaData(g_depthMD);
// Texture map init
g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes()-1) / 512) + 1) * 512;
g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes()-1) / 512) + 1) * 512;
g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel));
std::cout << " w:" << g_depthMD.FullXRes() << " h:" << g_depthMD.FullYRes() << std::endl;
pixels = (unsigned char*)malloc(640*480*3*sizeof(unsigned char));
tex.allocate(640, 480, GL_RGB);
}
XnStatus Init_Kinect(EventOutSFNode* skltn,EventOutSFNode* hnz,EventOutSFNode* flr){
XnStatus rc=XN_STATUS_OK;
EnumerationErrors errors;
DepthMetaData g_depthMD;
ImageMetaData g_imageMD;
rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
CHECK_RC(rc, "InitFromXml");
rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
CHECK_RC(rc,"XN_NODE_TYPE_DEPTH");
rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
CHECK_RC(rc,"XN_NODE_TYPE_IMAGE");
rc= g_context.FindExistingNode(XN_NODE_TYPE_USER,g_user);
CHECK_RC(rc,"XN_NODE_TYPE_USER");
rc=g_context.FindExistingNode(XN_NODE_TYPE_SCENE,g_scene);
CHECK_RC(rc,"XN_NODE_TYPE_SCENE");
rc=g_context.FindExistingNode(XN_NODE_TYPE_HANDS,g_hands);
CHECK_RC(rc,"XN_NODE_TYPE_HANDS");
rc=g_context.FindExistingNode(XN_NODE_TYPE_GESTURE,g_gesture);
CHECK_RC(rc,"XN_NODE_TYPE_GESTURE");
g_depth.GetMetaData(g_depthMD);
g_fps=g_depthMD.FPS();
g_image.GetMetaData(g_imageMD);
rc=init_skeleton();
CHECK_RC(rc,"INIT SKELETON");
rc=init_hands();
CHECK_RC(rc,"INIT HANDS");
pix_w=g_depthMD.FullXRes();
pix_h=g_depthMD.FullYRes();
if(pix_h==0||pix_w==0){return XN_STATUS_ERROR;}
g_skltn=skltn;
g_hnz=hnz;
g_flr=flr;
if(NULL==g_skltn||NULL==g_hands||NULL==g_flr)return XN_STATUS_ERROR;
isInit=true;
return rc;
}
int main(int argc, char* argv[])
{
EnumerationErrors errors;
//rc = context.Init();
rc = context.InitFromXmlFile(strPathToXML,&errors);
if (rc == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (rc);
}
else if (rc != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(rc));
return (rc);
}
/* UNCOMMENT TO GET FILE READING
//rc = context.OpenFileRecording(strInputFile);
//CHECK_RC(rc, "Open input file");
//rc = context.FindExistingNode(XN_NODE_TYPE_PLAYER, player);
//CHECK_RC(rc, "Get player node"); */
rc = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
CHECK_RC(rc, "Find depth generator");
rc = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);
CHECK_RC(rc, "Find image generator");
depth.GetMetaData(depthMD);
image.GetMetaData(imageMD);
//rc = player.SetRepeat(FALSE);
XN_IS_STATUS_OK(rc);
//rc = player.GetNumFrames(image.GetName(), nNumFrames);
//CHECK_RC(rc, "Get player number of frames");
//printf("%d\n",nNumFrames);
//rc = player.GetNumFrames(depth.GetName(), nNumFrames);
//CHECK_RC(rc, "Get player number of frames");
//printf("%d\n",nNumFrames);
// Hybrid mode isn't supported
if (imageMD.FullXRes() != depthMD.FullXRes() || imageMD.FullYRes() != depthMD.FullYRes())
{
printf ("The device depth and image resolution must be equal!\n");
return 1;
}
// RGB is the only image format supported.
if (imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
{
printf("The device image format must be RGB24\n");
return 1;
}
avi = cvCreateVideoWriter(strOutputFile, 0, 30, cvSize(640,480), TRUE);
depthMetersMat = cvCreateMat(480, 640, CV_16UC1);
kinectDepthImage = cvCreateImage( cvSize(640,480),16,1 );
depthMetersMat2 = cvCreateMat(480, 640, CV_16UC1);
kinectDepthImage2 = cvCreateImage( cvSize(640,480),16,1 );
colorArr[0] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);
colorArr[1] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);
colorArr[2] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);
//prepare_for_face_detection();
int b;
int g;
int r;
while ((rc = image.WaitAndUpdateData()) != XN_STATUS_EOF && (rc = depth.WaitAndUpdateData()) != XN_STATUS_EOF) {
if (rc != XN_STATUS_OK) {
printf("Read failed: %s\n", xnGetStatusString(rc));
break;
}
depth.GetMetaData(depthMD);
image.GetMetaData(imageMD);
//XnUInt32 a;
//a = g_imageMD.FPS;
printf("%d\n",imageMD.FrameID());
//a = g_depthMD.DataSize();
//printf("%d\n",a);
pDepth = depthMD.Data();
pImageRow = imageMD.RGB24Data();
for (unsigned int y=0; y<imageMD.YRes(); y++) {
pPixel = pImageRow;
uchar* Bptr = colorArr[0].ptr<uchar>(y);
uchar* Gptr = colorArr[1].ptr<uchar>(y);
uchar* Rptr = colorArr[2].ptr<uchar>(y);
for(unsigned int x=0;x<imageMD.XRes();++x , ++pPixel){
Bptr[x] = pPixel->nBlue;
Gptr[x] = pPixel->nGreen;
Rptr[x] = pPixel->nRed;
depthMetersMat->data.s[y * XN_VGA_X_RES + x ] = 7*pDepth[y * XN_VGA_X_RES + x];
depthMetersMat2->data.s[y * XN_VGA_X_RES + x ] = pDepth[y * XN_VGA_X_RES + x];
}
pImageRow += imageMD.XRes();
}
cv::merge(colorArr,3,colorImage);
iplImage = colorImage;
//cvThreshold(depthMetersMat2, depthMetersMat2, 150, 1500, THRESH_BINARY);
cvGetImage(depthMetersMat,kinectDepthImage);
cvGetImage(depthMetersMat2,kinectDepthImage2);
depthImage = Bw2Image(kinectDepthImage2);
printf("1. Middle pixel is %u millimeters away\n",depthImage[240][320]);
rgbImage = RgbImage(&iplImage);
// we want to see on up to 2000 MM
int THRESH = 2000;
for (unsigned int y=0; y<imageMD.YRes(); y++) {
for(unsigned int x=0;x<imageMD.XRes();++x){
if ( depthImage[y][x] >= THRESH ) {
depthImage[y][x] = 0;
} else {
float tmp = depthImage[y][x];
tmp = tmp / THRESH * (65536)*(-1) + 65536;
depthImage[y][x] = (unsigned int)tmp;
}
}
}
// THE PART ABOUT FILTERING COLOURS IN HSV TO SEE ONLY SPECIFIC ONE
// AFTER ONE FEW MORPHOLOGICAL OPERATIONS TO MAKE IT LOOK BETTER
IplImage* imgHSV = cvCreateImage(cvGetSize(&iplImage), 8, 3);
cvCvtColor(&iplImage, imgHSV, CV_BGR2HSV);
imgThreshed = cvCreateImage(cvGetSize(&iplImage), 8, 1);
//cvInRangeS(imgHSV, cvScalar(100, 60, 80), cvScalar(110, 255, 255), imgThreshed); // BLUE
cvInRangeS(imgHSV, cvScalar(29, 95, 95), cvScalar(35, 255, 255), imgThreshed); // YELLOW
//cvInRangeS(imgHSV, cvScalar(29, 60, 60), cvScalar(35, 255, 255), imgThreshed); // YELLOW DARK
//cvInRangeS(imgHSV, cvScalar(150, 70, 70), cvScalar(160, 255, 255), imgThreshed); // PINK
//cvInRangeS(imgHSV, cvScalar(40, 76, 76), cvScalar(70, 255, 255), imgThreshed); // GREEN
IplConvKernel* kernel = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT, NULL);
//cvDilate(imgThreshed,imgThreshed,kernel);
//cvErode(imgThreshed,imgThreshed,kernel);
Mat mat = Mat(imgThreshed);
blur(Mat(imgThreshed),mat,cvSize(3,3));
imgThreshed = &IplImage(mat);
//cvInRangeS(imgThreshed,cvScalar(100),cvScalar(255),imgThreshed);
//cvErode(imgThreshed,imgThreshed,kernel);
cvDilate(imgThreshed,imgThreshed,kernel);
cvDilate(imgThreshed,imgThreshed,kernel);
cvErode(imgThreshed,imgThreshed,kernel);
cvErode(imgThreshed,imgThreshed,kernel);
mat = Mat(imgThreshed);
blur(Mat(imgThreshed),mat,cvSize(6,6));
imgThreshed = &IplImage(mat);
cvInRangeS(imgThreshed,cvScalar(100),cvScalar(255),imgThreshed);
cvReleaseImage(&imgHSV);
BwImage threshed = BwImage(imgThreshed);
if ( initialize == true ) {
normalizeReferenceFace();
int currentID = 0;
for ( int y = 30; y<480; y++ ) {
for ( int x = 30; x<640; x++ ) {
bool g2g = true;
//printf("%d %d %d\n",ID, y,x);
if ( threshed[y][x]!=0 ) {
for ( int ID2 = 0; ID2<nbOfPoints; ID2++) {
if ( (abs(markers[ID2].y-y)<proximityLimit) && (abs(markers[ID2].x-x)<proximityLimit)) {
g2g = false;
}
}
if (currentID >= nbOfPoints || g2g == false ) {
break;
}
markers[currentID].y=y;
markers[currentID].x=x;
currentID++;
printf("WHITE PIXEL INITIALIZED %d: %d %d\n",currentID, x,y);
}
}
}
if (isDebugConf==true || currentID == nbOfMarkers) {
printf("%d PIXELS INITIALIZED\n", currentID);
initialize = false;
//printf("%d,%d\n", currentID, nbOfPoints);
//return 0;
} else {
printf("WAITING FOR %d PIXELS TO APPEAR, %d SO FAR \n",nbOfMarkers, currentID);
continue;
}
// FIND TOP RIGHT AND CHIN PIXEL
int refPixID = 0;
int chinPixID = 0;
for ( int i = 0; i < nbOfMarkers; i++) {
if ( (markers[i].x + markers[i].y)*(markers[i].x + markers[i].y) < (markers[refPixID].x + markers[refPixID].y)* (markers[refPixID].x + markers[refPixID].y)) {
refPixID = i;
}
if (markers[i].y > markers[chinPixID].y) {
chinPixID = i;
}
}
float width = (markers[1].x-markers[0].x)*2;
float heigth = abs(markers[1].y-markers[0].y);
// WE GOT WIDTH & HEIGTH OF THE FACE, LETS ADJUST POINTS
// SET 0 to REF, SET 1 to CHIN
MyPoint tmp = MyPoint(markers[refPixID].x,markers[refPixID].y);
markers[refPixID].x = markers[0].x;
markers[refPixID].y = markers[0].y;
markers[0].x = tmp.x;
markers[0].y = tmp.y;
tmp = MyPoint(markers[chinPixID].x,markers[chinPixID].y);
markers[chinPixID].x = markers[1].x;
markers[chinPixID].y = markers[1].y;
markers[1].x = tmp.x;
markers[1].y = tmp.y;
// REST OF THE POINTS
for ( int i = 2; i < nbOfPoints; i++) {
int cost = 0;
int lowestCost = 0;
int closestPixID = -1;
for ( int j = 2; j < nbOfMarkers; j++ ) {
cost = (markers[j].x-points[i].x*width)*(markers[j].x-points[i].x*width) + (markers[j].y-points[i].y*heigth)*(markers[j].y-points[i].y*heigth);
if ( cost < lowestCost ) {
lowestCost = cost;
closestPixID = j;
}
if (closestPixID == -1) {
//printf("COS JEST SPORO NIE W PORZADKU, CHECK HERE\n");
break;
}
tmp.x = markers[i].x;
tmp.y = markers[i].y;
markers[i].x=markers[closestPixID].x;
markers[i].x=markers[closestPixID].y;
markers[closestPixID].x = tmp.x;
markers[closestPixID].y = tmp.y;
}
}
}
for ( int currentPixelID = 0; currentPixelID < nbOfMarkers; currentPixelID++) {
if (markers[currentPixelID].x == 0) {
continue;
}
if ( threshed[markers[currentPixelID].y][markers[currentPixelID].x] < 128 ) {
printf("PIXEL %d LOST\n",currentPixelID);
for ( int neighbSize = 2; neighbSize < maxNeighbSize; neighbSize = neighbSize + 2 ) {
int x1 = markers[currentPixelID].x - neighbSize/2;
if ( x1 < intoDepthX(0) ) {
x1 = (int)intoDepthX(0);
}
int y1 = (int)(markers[currentPixelID].y-neighbSize/2);
if ( y1 < intoDepthY(0) ) {
y1 = intoDepthY(0);
}
int y2 = markers[currentPixelID].y+neighbSize/2;
if ( y2 > intoDepthY(480) ) {
y2 = intoDepthY(480);
}
int x2 = markers[currentPixelID].x+neighbSize/2;
if ( x2 > intoDepthX(640) ) {
y2 = intoDepthX(640);
}
bool found = false;
for ( int y = y1; y < y2; y++) {
for ( int x = x1; x < x2; x++) {
bool g2g = true;
if (threshed[y][x] > 128) {
for ( int ID2 = 0; ID2<nbOfMarkers; ID2++) {
if ( currentPixelID == ID2 )
continue;
if ( (abs(markers[ID2].y-y)<proximityLimit) && (abs(markers[ID2].x-x)<proximityLimit)) {
g2g = false;
break;
}
}
if ( g2g ) {
markers[currentPixelID].x = x;
markers[currentPixelID].y = y;
found = true;
printf("Pixel %d, FOUND\n",currentPixelID);
break;
}
}
}
if (found == true ) {
break;
}
}
if (found == true ) {
break;
}
}
}
paintMarkerOnBoth(markers[currentPixelID]);
}
faceImage = cvCreateImage(cvGetSize(&iplImage), 8, 1);
paintFace();
// normal kinect depth
cvShowImage("Depth_Kinect", kinectDepthImage);
// depth within 80 - 200 mm, normalized
cvShowImage("Depth_Kinect_2", kinectDepthImage2);
// rgb with tracking points
cvShowImage("RGB_Kinect", &iplImage);
// colour detector
cvShowImage("RGB_Threshed", imgThreshed);
// attempt to draw a face
cvShowImage("Face Image", faceImage);
cvWaitKey(50); // wait 20 ms
if ( avi == NULL) {
printf ("dupa%d \n",1);
}
//cvWriteFrame (avi, &iplImage);
}
// cvReleaseImageHeader(kinectDepthImage);
cvReleaseVideoWriter(&avi);
// cvReleaseHaarClassifierCascade( &cascade );
context.Shutdown();
return 0;
}
void glutDisplay (void)
{
XnStatus rc = XN_STATUS_OK;
// Read a new frame
rc = g_context.WaitAnyUpdateAll();
if (rc != XN_STATUS_OK)
{
printf("Read failed: %s\n", xnGetStatusString(rc));
return;
}
g_depth.GetMetaData(g_depthMD);
g_image.GetMetaData(g_imageMD);
const XnDepthPixel* pDepth = g_depthMD.Data();
const XnUInt8* pImage = g_imageMD.Data();
unsigned int nImageScale = GL_WIN_SIZE_X / g_depthMD.FullXRes();
// Copied from SimpleViewer
// Clear the OpenGL buffers
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0);
// Calculate the accumulative histogram (the yellow display...)
xnOSMemSet(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
unsigned int nNumberOfPoints = 0;
for (XnUInt y = 0; y < g_depthMD.YRes(); ++y)
{
for (XnUInt x = 0; x < g_depthMD.XRes(); ++x, ++pDepth)
{
if (*pDepth != 0)
{
g_pDepthHist[*pDepth]++;
nNumberOfPoints++;
}
}
}
for (int nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (int nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
xnOSMemSet(g_pTexMap, 0, g_nTexMapX*g_nTexMapY*sizeof(XnRGB24Pixel));
// check if we need to draw image frame to texture
if (g_nViewState == DISPLAY_MODE_OVERLAY ||
g_nViewState == DISPLAY_MODE_IMAGE)
{
const XnRGB24Pixel* pImageRow = g_imageMD.RGB24Data();
XnRGB24Pixel* pTexRow = g_pTexMap + g_imageMD.YOffset() * g_nTexMapX;
for (XnUInt y = 0; y < g_imageMD.YRes(); ++y)
{
const XnRGB24Pixel* pImage = pImageRow;
XnRGB24Pixel* pTex = pTexRow + g_imageMD.XOffset();
for (XnUInt x = 0; x < g_imageMD.XRes(); ++x, ++pImage, ++pTex)
{
*pTex = *pImage;
}
pImageRow += g_imageMD.XRes();
pTexRow += g_nTexMapX;
}
}
// check if we need to draw depth frame to texture
if (g_nViewState == DISPLAY_MODE_OVERLAY ||
g_nViewState == DISPLAY_MODE_DEPTH)
{
const XnDepthPixel* pDepthRow = g_depthMD.Data();
XnRGB24Pixel* pTexRow = g_pTexMap + g_depthMD.YOffset() * g_nTexMapX;
for (XnUInt y = 0; y < g_depthMD.YRes(); ++y)
{
const XnDepthPixel* pDepth = pDepthRow;
XnRGB24Pixel* pTex = pTexRow + g_depthMD.XOffset();
for (XnUInt x = 0; x < g_depthMD.XRes(); ++x, ++pDepth, ++pTex)
{
if (*pDepth != 0)
{
int nHistValue = g_pDepthHist[*pDepth];
pTex->nRed = nHistValue;
pTex->nGreen = nHistValue;
pTex->nBlue = 0;
}
}
pDepthRow += g_depthMD.XRes();
pTexRow += g_nTexMapX;
}
}
// Create the OpenGL texture map
glTexParameteri(GL_TEXTURE_2D, GL_GENERATE_MIPMAP_SGIS, GL_TRUE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, g_nTexMapX, g_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, g_pTexMap);
// Display the OpenGL texture map
glColor4f(1,1,1,1);
glBegin(GL_QUADS);
int nXRes = g_depthMD.FullXRes();
int nYRes = g_depthMD.FullYRes();
// upper left
glTexCoord2f(0, 0);
glVertex2f(0, 0);
// upper right
glTexCoord2f((float)nXRes/(float)g_nTexMapX, 0);
glVertex2f(GL_WIN_SIZE_X, 0);
// bottom right
glTexCoord2f((float)nXRes/(float)g_nTexMapX, (float)nYRes/(float)g_nTexMapY);
glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
// bottom left
glTexCoord2f(0, (float)nYRes/(float)g_nTexMapY);
glVertex2f(0, GL_WIN_SIZE_Y);
glEnd();
// Swap the OpenGL display buffers
glutSwapBuffers();
}
int main(int argc, char* argv[])
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = xnLogInitFromXmlFile(SAMPLE_XML_PATH);
if (nRetVal != XN_STATUS_OK)
{
printf("Log couldn't be opened: %s. Running without log", xnGetStatusString(nRetVal));
}
if (argc < 3)
{
printf("usage: %s <inputFile> <outputFile>\n", argv[0]);
return -1;
}
const char* strInputFile = argv[1];
const char* strOutputFile = argv[2];
Context context;
nRetVal = context.Init();
CHECK_RC(nRetVal, "Init");
// open input file
Player player;
nRetVal = context.OpenFileRecording(strInputFile, player);
CHECK_RC(nRetVal, "Open input file");
// Get depth node from recording
DepthGenerator depth;
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
CHECK_RC(nRetVal, "Find depth generator");
// Create mock node based on depth node from recording
MockDepthGenerator mockDepth;
nRetVal = mockDepth.CreateBasedOn(depth);
CHECK_RC(nRetVal, "Create mock depth node");
// create recorder
Recorder recorder;
nRetVal = recorder.Create(context);
CHECK_RC(nRetVal, "Create recorder");
nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, strOutputFile);
CHECK_RC(nRetVal, "Set recorder destination file");
// add depth node to recorder
nRetVal = recorder.AddNodeToRecording(mockDepth);
CHECK_RC(nRetVal, "Add node to recording");
nRetVal = player.SetRepeat(FALSE);
XN_IS_STATUS_OK(nRetVal);
XnUInt32 nNumFrames = 0;
nRetVal = player.GetNumFrames(depth.GetName(), nNumFrames);
CHECK_RC(nRetVal, "Get player number of frames");
DepthMetaData depthMD;
while ((nRetVal = depth.WaitAndUpdateData()) != XN_STATUS_EOF)
{
CHECK_RC(nRetVal, "Read next frame");
// Get depth meta data
depth.GetMetaData(depthMD);
//-----------------------------------------------//
// Transform depth! This is the interesting part //
//-----------------------------------------------//
/* Enable the depth data to be modified. This is done implicitly by depthMD.WritableDepthMap(),
but we're calling it just to be clear. */
nRetVal = depthMD.MakeDataWritable();
CHECK_RC(nRetVal, "Make depth data writable");
transformDepthMD(depthMD);
// Pass the transformed data to the mock depth generator
nRetVal = mockDepth.SetData(depthMD);
CHECK_RC(nRetVal, "Set mock node new data");
/* We need to call recorder.Record explicitly because we're not using WaitAndUpdateAll(). */
nRetVal = recorder.Record();
CHECK_RC(nRetVal, "Record");
printf("Recorded: frame %u out of %u\r", depthMD.FrameID(), nNumFrames);
}
printf("\n");
return 0;
}
void captureDepthMap(unsigned char* g_ucDepthBuffer)
{
SceneMetaData smd;
DepthMetaData dmd;
_depth.GetMetaData(dmd);
//printf("AS3OpenNI :: Frame %d Middle point is: %u. FPS: %f\n", dmd.FrameID(), dmd(dmd.XRes() / 2, dmd.YRes() / 2), xnFPSCalc(&xnFPS));
_depth.GetMetaData(dmd);
_userGenerator.GetUserPixels(0, smd);
unsigned int nValue = 0;
unsigned int nHistValue = 0;
unsigned int nIndex = 0;
unsigned int nX = 0;
unsigned int nY = 0;
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
// Calculate the accumulative histogram
memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (_drawPixels)
{
XnUInt32 nIndex = 0;
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
g_ucDepthBuffer[0] = 0;
g_ucDepthBuffer[1] = 0;
g_ucDepthBuffer[2] = 0;
g_ucDepthBuffer[3] = 0x00;
if (_depthMapBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
if(_depthMapDetect)
{
g_ucDepthBuffer[0] = nHistValue * Colors[nColorID][0];
g_ucDepthBuffer[1] = nHistValue * Colors[nColorID][1];
g_ucDepthBuffer[2] = nHistValue * Colors[nColorID][2];
}
else
{
g_ucDepthBuffer[0] = nHistValue;
g_ucDepthBuffer[1] = nHistValue;
g_ucDepthBuffer[2] = nHistValue;
}
g_ucDepthBuffer[3] = 0xFF;
}
}
pDepth++;
pLabels++;
g_ucDepthBuffer+=4;
}
}
}
}
int main(int argc, char* argv[])
{
XnStatus rc;
EnumerationErrors errors;
rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
if (rc == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (rc);
}
else if (rc != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(rc));
return (rc);
}
rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
g_depth.GetMetaData(g_depthMD);
g_image.GetMetaData(g_imageMD);
// Hybrid mode isn't supported in this sample
if (g_imageMD.FullXRes() != g_depthMD.FullXRes() || g_imageMD.FullYRes() != g_depthMD.FullYRes())
{
printf ("The device depth and image resolution must be equal!\n");
return 1;
}
// RGB is the only image format supported.
if (g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
{
printf("The device image format must be RGB24\n");
return 1;
}
// Texture map init
g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes()-1) / 512) + 1) * 512;
g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes()-1) / 512) + 1) * 512;
g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel));
// OpenGL init
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
glutCreateWindow ("OpenNI Simple Viewer");
glutFullScreen();
glutSetCursor(GLUT_CURSOR_NONE);
glutKeyboardFunc(glutKeyboard);
glutDisplayFunc(glutDisplay);
glutIdleFunc(glutIdle);
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
// Per frame code is in glutDisplay
glutMainLoop();
return 0;
}
//----------------------------------------------------
// 描画処理
//----------------------------------------------------
void glutDisplay (void){
xnFPSMarkFrame(&g_xnFPS); // FPSの計測開始？
XnStatus rc = XN_STATUS_OK;
// 更新されたノードを待つ(どれでもいい)
rc = g_context.WaitAnyUpdateAll();
if (rc != XN_STATUS_OK){
printf("Read failed: %s\n", xnGetStatusString(rc));
printf("test\n");
return;
}
// イメージ・デプス・ユーザのデータを取得
g_image.GetMetaData(g_imageMD);
g_depth.GetMetaData(g_depthMD);
g_user.GetUserPixels(0, g_sceneMD);
// カラー・デプスバッファをクリア
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// 設定
setDepthHistgram(g_depth, g_depthMD, g_pDepthHist); // ヒストグラムの計算・作成
setTexture(); // テクスチャ設定
// 描画
drawImage(); // イメージデータの描画
// デバッグモードの文字は描画の行列と隔離
glMatrixMode(GL_PROJECTION); // 射影変換の行列の設定
//glLoadIdentity(); // スタックのクリア
glMatrixMode(GL_MODELVIEW); // モデルビュー変換の行列の設定
glLoadIdentity();
if(g_debugMode) glDebug(); // デバッグモード
// 一度だけスクリーンショットをとる
if(g_screenShotImageMode){
ostringstream fname;
fname << OUT_IMAGE_PATH ;//出力ファイル名
std::string name = fname.str();
g_glScreenShot.screenshot(name.c_str(), 24);
g_screenShotImageMode = !g_screenShotImageMode; // トグル
}
// 一度だけ深さデータを取得する
if(g_screenShotDepthMode){
ofstream ofs(OUT_DEPTH_PATH);
const XnDepthPixel* pDepth = g_depthMD.Data();
for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; y ++){
for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; x ++, pDepth ++){
if(*pDepth < 2000){
ofs << (int)((*pDepth) * 2) << ',';
}else{
ofs << (*pDepth) << ',';
}
}
ofs << endl;
}
g_screenShotDepthMode = !g_screenShotDepthMode; // トグル
}
// Swap the OpenGL display buffers
glutSwapBuffers();
}
int main(int argc, char* argv[])
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = xnLogInitFromXmlFile(SAMPLE_XML_PATH);
if (nRetVal != XN_STATUS_OK)
{
printf("Log couldn't be opened: %s. Running without log", xnGetStatusString(nRetVal));
}
if (argc < 3)
{
printf("usage: %s <inputFile> <outputFile>\n", argv[0]);
return -1;
}
const char* strInputFile = argv[1];
const char* strOutputFile = argv[2];
Context context;
nRetVal = context.Init();
CHECK_RC(nRetVal, "Init");
// open input file
Player player;
nRetVal = context.OpenFileRecording("/media/6B58CB581C0AACF6/7.oni", player);
CHECK_RC(nRetVal, "Open input file");
// Get depth node from recording
DepthGenerator depth;
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
CHECK_RC(nRetVal, "Find depth generator");
// Create mock node based on depth node from recording
MockDepthGenerator mockDepth;
nRetVal = mockDepth.CreateBasedOn(depth);
CHECK_RC(nRetVal, "Create mock depth node");
ImageGenerator image;
nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);
CHECK_RC(nRetVal, "Find depth generator");
// Create mock node based on depth node from recording
MockImageGenerator mockImage;
nRetVal = mockImage.CreateBasedOn(image);
CHECK_RC(nRetVal, "Create mock depth node");
// create recorder
Recorder recorder;
nRetVal = recorder.Create(context);
CHECK_RC(nRetVal, "Create recorder");
nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, "/home/shaghayegh/up.oni");
CHECK_RC(nRetVal, "Set recorder destination file");
// add depth node to recorder
nRetVal = recorder.AddNodeToRecording(mockDepth);
CHECK_RC(nRetVal, "Add node to recording");
// nRetVal = recorder.AddNodeToRecording(mockImage);
// CHECK_RC(nRetVal, "Add node to recording");
nRetVal = player.SetRepeat(FALSE);
XN_IS_STATUS_OK(nRetVal);
XnUInt32 nNumFrames = 0;
nRetVal = player.GetNumFrames(depth.GetName(), nNumFrames);
CHECK_RC(nRetVal, "Get player number of frames");
DepthMetaData depthMD;
ImageMetaData imageMD;
int frameNum = 0;
String path = "/media/6B58CB581C0AACF6/ebook/Articles/activity_recognition/data1/0512164529/";
while ((nRetVal = depth.WaitAndUpdateData()) != XN_STATUS_EOF)
{
++frameNum;
CHECK_RC(nRetVal, "Read next frame");
// Get depth meta data
depth.GetMetaData(depthMD);
image.GetMetaData(imageMD);
//-----------------------------------------------//
// Transform depth! This is the interesting part //
//-----------------------------------------------//
/* Enable the depth data to be modified. This is done implicitly by depthMD.WritableDepthMap(),
but we're calling it just to be clear. */
nRetVal = depthMD.MakeDataWritable();
CHECK_RC(nRetVal, "Make depth data writable");
// nRetVal = imageMD.MakeDataWritable();
// CHECK_RC(nRetVal, "Make depth data writable");
String ficheroActualRGB;
// ficheroActualRGB = path +"RGB_" + boost::to_string(frameNum) + ".png";
String ficheroActualDepth = path +"Depth_"+ boost::to_string(frameNum) + ".png";
// Mat matFrameImage = imread(ficheroActualRGB, 1);
// resize(matFrameImage, matFrameImage, Size(640, 480), 0, 0, INTER_CUBIC);
Mat matFrameDepth = imread(ficheroActualDepth,1);
resize(matFrameDepth, matFrameDepth, Size(480, 640), 0, 0, INTER_CUBIC);
transformDepthMD(matFrameDepth,depthMD);
// transformImageMD(matFrameImage,imageMD);
// Pass the transformed data to the mock depth generator
nRetVal = mockDepth.SetData(depthMD);
CHECK_RC(nRetVal, "Set mock node new data");
// nRetVal = mockImage.SetData(imageMD);
// CHECK_RC(nRetVal, "Set mock node new data");
/* We need to call recorder.Record explicitly because we're not using WaitAndUpdateAll(). */
nRetVal = recorder.Record();
CHECK_RC(nRetVal, "Record");
printf("Recorded: frame %u out of %u\r", depthMD.FrameID(), nNumFrames);
}
printf("\n");
return 0;
}
//----------------------------------------------------
// OpenNI関連の初期化
//----------------------------------------------------
void xnInit(void){
XnStatus rc;
EnumerationErrors errors;
rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
if (rc == XN_STATUS_NO_NODE_PRESENT){
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
exit(1);
}else if (rc != XN_STATUS_OK){
printf("Open failed: %s\n", xnGetStatusString(rc));
exit(1);
}
//playerInit();
rc = xnFPSInit(&g_xnFPS, 180); // FPSの初期化
//CHECK_RC(rc, "FPS Init");
// デプス・イメージ・ユーザジェネレータの作成
rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
errorCheck(rc, "g_depth"); // エラーチェック
rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
errorCheck(rc, "g_image");
rc = g_context.FindExistingNode(XN_NODE_TYPE_USER, g_user);
//rc = g_user.Create(g_context);
errorCheck(rc, "g_user");
// ユーザー検出機能をサポートしているか確認
if (!g_user.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
//throw std::runtime_error("ユーザー検出をサポートしてません");
cout << "ユーザー検出をサポートしてません" << endl;
exit(1);
}
// レコーダーの設定
//rc = setRecorder(g_recorder, rc);
// ユーザコールバックの登録
XnCallbackHandle userCallbacks;
g_user.RegisterUserCallbacks(UserDetected, UserLost, NULL, userCallbacks);
// デプス・イメージ・ユーザデータの取得
g_depth.GetMetaData(g_depthMD);
g_image.GetMetaData(g_imageMD);
g_user.GetUserPixels(0, g_sceneMD);
// Hybrid mode isn't supported in this sample
// イメージとデプスの大きさが違うとエラー
if (g_imageMD.FullXRes() != g_depthMD.FullXRes() || g_imageMD.FullYRes() != g_depthMD.FullYRes()){
printf ("The device depth and image resolution must be equal!\n");
exit(1);
}
// RGB is the only image format supported.
// フォーマットの確認
if (g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24){
printf("The device image format must be RGB24\n");
exit(1);
}
// Texture map init
// フルスクリーン画面の大きさ調整
g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes() - 1) / 512) + 1) * 512; // 大きさによって512の倍数に調整(1024)
g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes() - 1) / 512) + 1) * 512; // 512
g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel)); // スクリーンの大きさ分の色情報の容量を確保
// 座標ポインタの初期化
g_pPoint = (XnPoint3D*)malloc(KINECT_IMAGE_SIZE * sizeof(XnPoint3D)); // 座標を入れるポインタを作成
g_pBackTex = (XnRGB24Pixel*)malloc(KINECT_IMAGE_SIZE * sizeof(XnRGB24Pixel)); // 背景画像を入れるポインタを作成
g_pBackPoint = (XnPoint3D*)malloc(KINECT_IMAGE_SIZE * sizeof(XnPoint3D)); // 背景座標を入れるポインタを作成
g_pBackDepth = (XnDepthPixel*)malloc(KINECT_IMAGE_SIZE * sizeof(XnDepthPixel)); // 背景座標を入れるポインタを作成
}
int _tmain(int argc, _TCHAR* argv[])
{
XnStatus nRetVal = XN_STATUS_OK;
Context context;
nRetVal = context.Init();
DepthGenerator depth;
nRetVal = depth.Create(context);
XnMapOutputMode mapMode;
mapMode.nXRes = XN_VGA_X_RES;
mapMode.nYRes = XN_VGA_Y_RES;
mapMode.nFPS = 30;
nRetVal = depth.SetMapOutputMode(mapMode);
nRetVal = context.StartGeneratingAll();
XnUInt32 nMiddleIndex = XN_VGA_X_RES * XN_VGA_Y_RES/2 + XN_VGA_X_RES/2;
int count = 0;
const XnDepthPixel* pDepthMap;
while (count<50) { // Update to next frame
nRetVal = context.WaitOneUpdateAll(depth);
pDepthMap = depth.GetDepthMap();
printf("Middle pixel is %u millimeters away\n", pDepthMap[nMiddleIndex]);
count++;
}
DepthMetaData g_depthMD;
depth.GetMetaData(g_depthMD);
cout<<g_depthMD.FullXRes();
cout<<g_depthMD.FullYRes();
const double maxDepth = 3000;
const int xScale = 1;
const int yScale = 1;
const int xActualRes = XN_VGA_X_RES;
const int yActualRes = XN_VGA_Y_RES;
const int xRes = xActualRes/xScale;
const int yRes = yActualRes/yScale;
const int sizeOfMap = xRes * yRes + 2*xRes + 2*(yRes-2) +2 ; //multiply by two for back face
vertex* vertices = new vertex[sizeOfMap];
double xRealScale = 1.0;
double yRealScale = 1.0;
for (int j = 0; j < yRes; j++) {
for (int i = 0; i < xRes; i++) {
double x = xScale * xRealScale * i;
double y = yScale * yRealScale * j;
double z = -(double) g_depthMD.DepthMap()[xActualRes * j * yScale + i * xScale];
if (z == 0)
z = -maxDepth;
if (z > 1800)
z = -maxDepth;
vertices[xRes*j + i] = vertex(x,y,z);
}
}
int offset = xRes*yRes;
vector<int> indices;
//bottom facets
for (int i = 0; i< xRes; i++) {
double x = xRealScale * xScale* i;
double y = 0;
double z = -maxDepth - 100; //placeholder
vertices[offset + i] = vertex(x,y,z);
}
//bottom facet winding
for (int i = 0; i< xRes-1; i++) {
indices.push_back(i);
indices.push_back(i+1);
indices.push_back(xRes*yRes + 1 + i);
indices.push_back(i);
indices.push_back(xRes*yRes + 1 + i);
indices.push_back(xRes*yRes + i);
}
offset = offset + xRes;
//right side facets
for (int i = 1; i< yRes; i++) {
double x = xRealScale * xScale * (xRes-1);
double y = yRealScale * yScale * i;
double z = -maxDepth - 100; //placeholder
vertices[offset + i -1] = vertex(x,y,z);
}
offset = offset + yRes -1;
//right facet winding
for (int i = 0; i< yRes-1; i++) {
indices.push_back(xRes-1 + i * xRes); //2 good
indices.push_back(xRes-1 + xRes * (i+1)); //5
indices.push_back(xRes*yRes + xRes + i); //12
indices.push_back(xRes-1 + i * xRes); //2 good
indices.push_back(xRes*yRes + xRes + i -1); //11
indices.push_back(xRes*yRes + xRes + i); //12
}
//top side facets
for (int i = 1; i< xRes; i++) {
double x = xRealScale * xScale * (xRes-1 - i);
double y = yRealScale * yScale * (yRes-1);
double z = -maxDepth - 100; //placeholder
vertices[offset + i -1] = vertex(x,y,z);
}
//top facet winding
for (int i = 0; i< xRes-1; i++) {
indices.push_back(xRes*yRes-1 - i); //8 good
indices.push_back(xRes*yRes -1 + xRes + yRes + i); //14
indices.push_back(xRes*yRes -2 - i); //7
indices.push_back(xRes*yRes-1 - i); //8 good
indices.push_back(xRes*yRes -1 + xRes + yRes + i -1); //13
indices.push_back(xRes*yRes -1 + xRes + yRes + i); //14
}
offset = offset + xRes - 1; //check
//left side facet
for (int i = 1; i< yRes+1; i++) {
double x = 0;
double y = yRealScale * yScale * (yRes - i); //check
double z = -maxDepth - 100; //placeholder
vertices[offset + i -1] = vertex(x,y,z); //check
}
//left side winding
for (int i = 0; i < yRes-1; i++) {
indices.push_back(xRes*yRes - (xRes*(i+2))); //3
indices.push_back(xRes*yRes - (xRes*(i+1))); //6
indices.push_back(xRes*yRes + 2*xRes + yRes -3 +i); //15
indices.push_back(xRes*yRes + 2*xRes + yRes -3 + i + 1); //16
indices.push_back(xRes*yRes - (xRes*(i+2)));
indices.push_back(xRes*yRes + 2*xRes + yRes -3 +i); //15
//indices.push_back(xRes*yRes - xRes * (i+1)); //6
//indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i); //15
//indices.push_back(xRes*yRes - xRes * (i+2)); //3
//indices.push_back(xRes*yRes - xRes * (i+2)); //3
//indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i); //15
//indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i +1); //16
}
indices.push_back(xRes*yRes); //6
indices.push_back(0); //15
indices.push_back(xRes*yRes + 2*xRes + 2*yRes -4);
//bottom face
indices.push_back(xRes*yRes); //6
indices.push_back(xRes*yRes + xRes-1); //15
indices.push_back(xRes*yRes + xRes + yRes -2); //3
indices.push_back(xRes*yRes); //6
indices.push_back(xRes*yRes + xRes + yRes -2); //15
indices.push_back(xRes*yRes + 2*xRes + yRes -3); //3
printf("Created %u vertices\n",sizeOfMap);
//front face - this shows the actual depth map
for (int i = 0; i < xRes-1; i++) {
for (int j = 0; j < yRes-1; j++) {
indices.push_back(j * xRes + i);
indices.push_back((j+1) * xRes + i);
indices.push_back(j * xRes + i + 1);
indices.push_back(j * xRes + i + 1);
indices.push_back((j+1) * xRes + i);
indices.push_back((j+1) * xRes + i + 1);
}
}
int numFacets = indices.size()/3;
printf("Created %u facets\n",numFacets);
//int offset = sizeOfMap-1; // number of vertices on front face
////back face
////for (int i = 0; i < xRes-1; i++) {
//// for (int j = 0; j < yRes-1; j++) {
//// indices.push_back(j * xRes + i + 1 + offset);
//// indices.push_back((j+1) * xRes + i + offset);
//// indices.push_back(j * xRes + i + offset);
////
//// indices.push_back((j+1) * xRes + i + 1 + offset);
//// indices.push_back((j+1) * xRes + i + offset);
//// indices.push_back(j * xRes + i + 1 + offset);
//// }
////}
//int n = indices.size()/3;
ofstream stlOut;
stlOut.open ("stlOut.stl");
printf("Writing to STL...\n");
stlOut << "solid kinectout\n";
// printf("Face Count: %u Vertex Count: %u", numFacets, xRes*yRes);
for (int ii = 0; ii < numFacets; ++ii)
{
int v1 = indices[3*ii];
int v2 = indices[3*ii + 1];
int v3 = indices[3*ii + 2];
vertex p1 = vertices[v1];
vertex p2 = vertices[v2];
vertex p3 = vertices[v3];
vertex dir1 = p1.subtract(p2);
vertex dir2 = p3.subtract(p2);
vertex n = dir1.cross(dir2).normalize();
stlOut << " facet normal "<< n.x << " " << n.y << " " << n.z << "\n";
stlOut << " outer loop\n";
stlOut << " vertex " << p1.x << " " << p1.y << " " << p1.z << "\n";
stlOut << " vertex " << p2.x << " " << p2.y << " " << p2.z << "\n";
stlOut << " vertex " << p3.x << " " << p3.y << " " << p3.z << "\n";
stlOut << " endloop\n";
stlOut << " endfacet\n";
}
stlOut << "endsolid kinectout";
printf("Complete!\n");
stlOut.close();
}
int main(int argc, char *argv[])
{
//--------------------------------------------------------------------//
//------------------------- SETUP REQUIRED NODES ---------------------//
//--------------------------------------------------------------------//
// Setup the command line parameters.
setupParams(argc, argv);
// Setup all the sockets.
setupSockets();
// Setup the capture socket server for Mac.
#if (XN_PLATFORM == XN_PLATFORM_MACOSX)
if(_featureDepthMapCapture || _featureRGBCapture)
{
if(_useSockets)
{
g_AS3Network = network();
g_AS3Network.init(setupServer);
}
}
#endif
// Setup the status.
XnStatus _status = XN_STATUS_OK;
EnumerationErrors _errors;
// Context Init and Add license.
_status = _context.Init();
CHECK_RC(_status, "AS3OpenNI :: Initialize context");
_context.SetGlobalMirror(_mirror);
XnChar vendor[XN_MAX_NAME_LENGTH];
XnChar license[XN_MAX_LICENSE_LENGTH];
_license.strVendor[XN_MAX_NAME_LENGTH] = strcmp(vendor, "PrimeSense");
_license.strKey[XN_MAX_LICENSE_LENGTH] = strcmp(license, "0KOIk2JeIBYClPWVnMoRKn5cdY4=");
_status = _context.AddLicense(_license);
CHECK_RC(_status, "AS3OpenNI :: Added license");
// Set it to VGA maps at 30 FPS
_depthMode.nXRes = 640;
_depthMode.nYRes = 480;
_depthMode.nFPS = 30;
// Depth map create.
_status = _depth.Create(_context);
CHECK_RC(_status, "AS3OpenNI :: Create depth generator");
_status = _depth.SetMapOutputMode(_depthMode);
// Depth map create.
_status = _image.Create(_context);
CHECK_RC(_status, "AS3OpenNI :: Create image generator");
_status = _image.SetMapOutputMode(_depthMode);
_status = _image.SetPixelFormat(XN_PIXEL_FORMAT_RGB24);
// Create the hands generator.
_status = _hands.Create(_context);
CHECK_RC(_status, "AS3OpenNI :: Create hands generator");
_hands.SetSmoothing(0.1);
// Create the gesture generator.
_status = _gesture.Create(_context);
CHECK_RC(_status, "AS3OpenNI :: Create gesture generator");
// Create user generator.
_status = _userGenerator.Create(_context);
CHECK_RC(_status, "AS3OpenNI :: Find user generator");
// Create and initialize point tracker
_sessionManager = new XnVSessionManager();
_status = _sessionManager->Initialize(&_context, "Wave", "RaiseHand");
if (_status != XN_STATUS_OK)
{
printf("AS3OpenNI :: Couldn't initialize the Session Manager: %s\n", xnGetStatusString(_status));
CleanupExit();
}
_sessionManager->RegisterSession(NULL, &SessionStart, &SessionEnd, &SessionProgress);
// Start catching signals for quit indications
CatchSignals(&_quit);
//---------------------------------------------------------------//
//------------------------- SETUP FEATURES ---------------------//
//--------------------------------------------------------------//
// Define the Wave and SinglePoint detectors.
_waveDetector = new XnVWaveDetector();
// SinglePoint detector.
if(_featureSinglePoint) _waveDetector->RegisterPointUpdate(NULL, &OnPointUpdate);
// Feature Gesture.
if(_featureGesture)
{
// Wave detector.
_waveDetector->RegisterWave(NULL, &OnWave);
// Push detector.
_pushDetector = new XnVPushDetector();
_pushDetector->RegisterPush(NULL, &onPush);
// Swipe detector.
_swipeDetector = new XnVSwipeDetector();
_swipeDetector->RegisterSwipeUp(NULL, &Swipe_SwipeUp);
_swipeDetector->RegisterSwipeDown(NULL, &Swipe_SwipeDown);
_swipeDetector->RegisterSwipeLeft(NULL, &Swipe_SwipeLeft);
_swipeDetector->RegisterSwipeRight(NULL, &Swipe_SwipeRight);
// Steady detector.
_steadyDetector = new XnVSteadyDetector();
_steadyDetector->RegisterSteady(NULL, &Steady_OnSteady);
}
// Feature Circle.
if(_featureCircle)
{
// Circle detector.
_circleDetector = new XnVCircleDetector();
_circleDetector->RegisterCircle(NULL, &CircleCB);
_circleDetector->RegisterNoCircle(NULL, &NoCircleCB);
_circleDetector->RegisterPrimaryPointCreate(NULL, &Circle_PrimaryCreate);
_circleDetector->RegisterPrimaryPointDestroy(NULL, &Circle_PrimaryDestroy);
}
// Feature Slider.
if(_featureSlider)
{
// Left/Right slider.
_leftRightSlider = new XnVSelectableSlider1D(3, 0, AXIS_X);
_leftRightSlider->RegisterActivate(NULL, &LeftRightSlider_OnActivate);
_leftRightSlider->RegisterDeactivate(NULL, &LeftRightSlider_OnDeactivate);
_leftRightSlider->RegisterPrimaryPointCreate(NULL, &LeftRightSlider_OnPrimaryCreate);
_leftRightSlider->RegisterPrimaryPointDestroy(NULL, &LeftRightSlider_OnPrimaryDestroy);
_leftRightSlider->RegisterValueChange(NULL, &LeftRightSlider_OnValueChange);
_leftRightSlider->SetValueChangeOnOffAxis(false);
// Up/Down slider.
_upDownSlider = new XnVSelectableSlider1D(3, 0, AXIS_Y);
_upDownSlider->RegisterActivate(NULL, &UpDownSlider_OnActivate);
_upDownSlider->RegisterDeactivate(NULL, &UpDownSlider_OnDeactivate);
_upDownSlider->RegisterPrimaryPointCreate(NULL, &UpDownSlider_OnPrimaryCreate);
_upDownSlider->RegisterPrimaryPointDestroy(NULL, &UpDownSlider_OnPrimaryDestroy);
_upDownSlider->RegisterValueChange(NULL, &UpDownSlider_OnValueChange);
_upDownSlider->SetValueChangeOnOffAxis(false);
// In/Out slider.
_inOutSlider = new XnVSelectableSlider1D(3, 0, AXIS_Z);
_inOutSlider->RegisterActivate(NULL, &InOutSlider_OnActivate);
_inOutSlider->RegisterDeactivate(NULL, &InOutSlider_OnDeactivate);
_inOutSlider->RegisterPrimaryPointCreate(NULL, &InOutSlider_OnPrimaryCreate);
_inOutSlider->RegisterPrimaryPointDestroy(NULL, &InOutSlider_OnPrimaryDestroy);
_inOutSlider->RegisterValueChange(NULL, &InOutSlider_OnValueChange);
_inOutSlider->SetValueChangeOnOffAxis(false);
}
// Feature TrackPad.
if(_featureTrackPad)
{
// Track Pad.
if(trackpad_columns > 0 && trackpad_rows > 0)
{
_trackPad = new XnVSelectableSlider2D(trackpad_columns, trackpad_rows);
}
else
{
_trackPad = new XnVSelectableSlider2D(4, 9);
}
_trackPad->RegisterItemHover(NULL, &TrackPad_ItemHover);
_trackPad->RegisterItemSelect(NULL, &TrackPad_ItemSelect);
_trackPad->RegisterPrimaryPointCreate(NULL, &TrackPad_PrimaryCreate);
_trackPad->RegisterPrimaryPointDestroy(NULL, &TrackPad_PrimaryDestroy);
}
// Feature User Tracking.
if(_featureUserTracking)
{
// Setup user generator callbacks.
XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
if (!_userGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("AS3OpenNI :: Supplied user generator doesn't support skeleton\n");
return 1;
}
_userGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
// Setup Skeleton detection.
_userGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
if (_userGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
_needPose = true;
if (!_userGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
printf("AS3OpenNI :: Pose required, but not supported\n");
return 1;
}
_userGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
_userGenerator.GetSkeletonCap().GetCalibrationPose(_strPose);
}
_userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
}
// Create the broadcaster manager.
_broadcaster = new XnVBroadcaster();
// Start generating all.
_context.StartGeneratingAll();
// Set the frame rate.
_status = xnFPSInit(&xnFPS, 180);
CHECK_RC(_status, "AS3OpenNI :: FPS Init");
//----------------------------------------------------------------------//
//------------------------- SETUP DISPLAY SUPPORT ---------------------//
//--------------------------------------------------------------------//
// Setup depth and image data.
_depth.GetMetaData(_depthData);
_image.GetMetaData(_imageData);
// Hybrid mode isn't supported in this sample
if (_imageData.FullXRes() != _depthData.FullXRes() || _imageData.FullYRes() != _depthData.FullYRes())
{
printf ("AS3OpenNI :: The device depth and image resolution must be equal!\n");
return 1;
}
// RGB is the only image format supported.
if (_imageData.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
{
printf("AS3OpenNI :: The device image format must be RGB24\n");
return 1;
}
// Setup the view points to match between the depth and image maps.
if(_snapPixels) _depth.GetAlternativeViewPointCap().SetViewPoint(_image);
//-------------------------------------------------------------//
//------------------------- MAIN LOOP ------------------------//
//-----------------------------------------------------------//
// Setup the capture socket server for PC.
#if (XN_PLATFORM == XN_PLATFORM_WIN32)
if(_featureDepthMapCapture || _featureRGBCapture || _featureUserTracking)
{
if(_useSockets)
{
g_AS3Network = network();
g_AS3Network.init(setupServer);
}
}
#endif
// Main loop
while ((!_kbhit()) && (!_quit))
{
xnFPSMarkFrame(&xnFPS);
_context.WaitAndUpdateAll();
_sessionManager->Update(&_context);
if(_featureDepthMapCapture) captureDepthMap(g_ucDepthBuffer);
if(_featureRGBCapture) captureRGB(g_ucImageBuffer);
#if (XN_PLATFORM == XN_PLATFORM_WIN32)
if(_featureUserTracking) getPlayers();
#else
if(_featureUserTracking) renderSkeleton();
#endif
}
CleanupExit();
}
int main()
{
XnStatus nRetVal = XN_STATUS_OK;
Context context;
ScriptNode scriptNode;
EnumerationErrors errors;
XnUInt32 min_z1, min_z2, min_z3, maxGrad, distVal;
const char *fn = NULL;
if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
else {
printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
return XN_STATUS_ERROR;
}
printf("Reading config from: '%s'\n", fn);
nRetVal = context.InitFromXmlFile(fn, scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
DepthGenerator depth;
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
CHECK_RC(nRetVal, "Find depth generator");
XnFPSData xnFPS;
nRetVal = xnFPSInit(&xnFPS, 180);
CHECK_RC(nRetVal, "FPS Init");
DepthMetaData depthMD;
//Initialize WiringPi
if(wiringPiSetup() == -1)
exit(1);
//Enable SoftPWM on pin 1,2 and 3
softPwmCreate(1, 0, RANGE);
softPwmCreate(2, 0, RANGE);
softPwmCreate(3, 0, RANGE);
while (!xnOSWasKeyboardHit())
{
nRetVal = context.WaitOneUpdateAll(depth);
if (nRetVal != XN_STATUS_OK)
{
printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
continue;
}
xnFPSMarkFrame(&xnFPS);
depth.GetMetaData(depthMD);
const XnDepthPixel* pDepthMap = depthMD.Data();
int XRes = depthMD.XRes();
int YRes = depthMD.YRes();
//To find closest pixel value in Zone 1, Zone 2 and Zone 3
min_z1 = getClosestPixel( 0 , 0, (XRes / 2) , YRes, depthMD);
min_z2 = getClosestPixel( (XRes / 4), 0, (3 * XRes / 4), YRes, depthMD);
min_z3 = getClosestPixel( (XRes / 2), 0, XRes , YRes, depthMD);
double in_low = 600;
double in_high = 2000;
double in_diff = in_high - in_low;
double out_low = 51;
double out_high = 973;
double out_diff = out_high - out_low;
distVal = min_z1;
XnUInt32 pwm_val1 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;
distVal = min_z2;
XnUInt32 pwm_val2 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;
distVal = min_z3;
XnUInt32 pwm_val3 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;
// Zone 1 - Left side (pin )
if (pwm_val1 < out_low)
pwm_val1 = 0; // if object too far, set DUTY CYCLE to 0
if (min_z1 == 9000.0)
pwm_val1 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
if (min_z1 < 600)
pwm_val1 = out_high;
// Zone 2 - Center (pin )
if (pwm_val2 < out_low)
pwm_val2 = 0; // if object too far, set DUTY CYCLE to 0
if (min_z2 == 9000.0)
pwm_val2 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
if (min_z2 < 600)
pwm_val2 = out_high;
// Zone 3 - Right side (pin )
if (pwm_val3 < out_low)
pwm_val3 = 0; // if object too far, set DUTY CYCLE to 0
if (min_z3 == 9000.0)
pwm_val3 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
if (min_z3 < 600)
pwm_val3 = out_high;
pwm_val1 = ((pwm_val1 - out_low) / (1.0 * out_diff)) * 100.0;
pwm_val2 = ((pwm_val2 - out_low) / (1.0 * out_diff)) * 100.0;
pwm_val3 = ((pwm_val3 - out_low) / (1.0 * out_diff)) * 100.0;
softPwmWrite(1,(int)pwm_val1);
softPwmWrite(2,(int)pwm_val2);
softPwmWrite(3,(int)pwm_val3);
if ( (depthMD.FrameID() % 30) == 0)
{
printf("Frame %d", depthMD.FrameID());
printf("\n");
printf("Zone 1 value is %u \t", pwm_val1);
printf("Zone 2 value is %u \t", pwm_val2);
printf("Zone 3 value is %u \n", pwm_val3);
printf("Zone1 min_dis %u \t", min_z1);
printf("Zone2 min_dis %u \t", min_z2);
printf("Zone3 min_dis %u \n", min_z3);
//To find a gradient value for the floor
//maxGrad = getGradient( 5, 0, (YRes/2) + 1, depthMD.XRes(), depthMD.YRes(), depthMD);
//printf("Frame %d max gradient for Floor is: %u. FPS: %f\n\n", depthMD.FrameID(), maxGrad, xnFPSCalc(&xnFPS));
}
}
softPwmWrite(1,0);
softPwmWrite(2,0);
softPwmWrite(3,0);
//release the nodes
depth.Release();
scriptNode.Release();
context.Release();
return 0;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
mxArray *tmpContext;
mxArray *tmpIR, *tmpIRMD;
mxArray *tmpDepth, *tmpDepthMD;
mxArray *context_initialised_array;
mxArray *has_ir_node_array;
mxArray *has_depth_node_array;
Context* context;
IRGenerator* ir;
IRMetaData* irMD;
DepthGenerator* depth;
DepthMetaData* depthMD;
unsigned short* output_ir = 0;
unsigned short* output_depth = 0;
mwSize dims2_ir[2];
mwSize dims2_depth[2];
bool context_initialised = false;
bool has_ir_node = false;
bool has_depth_node = false;
//---------------------------------
// Read input variables
//---------------------------------
// ni_context_obj
tmpContext = mxGetField(prhs[0], 0, "ni_context_obj");
memcpy((void*)&context, mxGetPr(tmpContext), sizeof(Context*));
// ir_obj
tmpIR = mxGetField(prhs[0], 0, "ir_obj");
memcpy((void*)&ir, mxGetPr(tmpIR), sizeof(IRGenerator*));
// irMD_obj
tmpIRMD = mxGetField(prhs[0], 0, "irMD_obj");
memcpy((void*)&irMD, mxGetPr(tmpIRMD), sizeof(IRMetaData*));
// depth_obj
tmpDepth = mxGetField(prhs[0], 0, "depth_obj");
memcpy((void*)&depth, mxGetPr(tmpDepth), sizeof(DepthGenerator*));
// depthMD_obj
tmpDepthMD = mxGetField(prhs[0], 0, "depthMD_obj");
memcpy((void*)&depthMD, mxGetPr(tmpDepthMD), sizeof(DepthMetaData*));
// context_initialised
context_initialised_array = mxGetField(prhs[0], 0, "context_initialised");
context_initialised = mxGetScalar(context_initialised_array);
// has_ir_node
has_ir_node_array = mxGetField(prhs[0], 0, "has_ir_node");
has_ir_node = mxGetScalar(has_ir_node_array);
// has_depth_node
has_depth_node_array = mxGetField(prhs[0], 0, "has_depth_node");
has_depth_node = mxGetScalar(has_depth_node_array);
//-------------------------------
// Create output variables
//-------------------------------
bool initialised = context_initialised;
int ir_width = 640;
int ir_height = 480;
int depth_width = 640;
int depth_height = 480;
if(initialised){
XnStatus rc;
if(has_ir_node){
ir->GetMetaData(*irMD);
ir_width = irMD->XRes();
ir_height = irMD->YRes();
}
if(has_depth_node){
depth->GetMetaData(*depthMD);
depth_width = depthMD->XRes();
depth_height = depthMD->YRes();
}
}
dims2_ir[0] = ir_height;
dims2_ir[1] = ir_width;
plhs[0] = mxCreateNumericArray(2, dims2_ir, mxUINT16_CLASS, mxREAL);
output_ir = (unsigned short*)mxGetPr(plhs[0]);
if(nlhs >= 2){
dims2_depth[0] = depth_height;
dims2_depth[1] = depth_width;
plhs[1] = mxCreateNumericArray(2, dims2_depth, mxUINT16_CLASS, mxREAL);
output_depth = (unsigned short*)mxGetPr(plhs[1]);
}
if(!initialised) return;
if(has_ir_node && output_ir != 0){
const XnIRPixel* pIR = irMD->Data();
int ir_image_size = ir_width*ir_height;
for(int i=0;i<ir_height;i++){
for(int j = 0;j < ir_width;j++){
output_ir[j*ir_height+i] = pIR[i*ir_width+j];
}
}
}
if(has_depth_node && output_depth != 0){
const XnDepthPixel* pDepth = depthMD->Data();
int depth_image_size = depth_width*depth_height;
for(int i=0;i<depth_height;i++){
for(int j = 0;j < depth_width;j++){
output_depth[j*depth_height+i] = pDepth[i*depth_width+j];
}
}
}
}
int main()
{
XnStatus nRetVal = XN_STATUS_OK;
Context context;
ScriptNode scriptNode;
EnumerationErrors errors;
const char *fn = NULL;
if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
else {
printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
return XN_STATUS_ERROR;
}
printf("Reading config from: '%s'\n", fn);
nRetVal = context.InitFromXmlFile(fn, scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
DepthGenerator depth;
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
CHECK_RC(nRetVal, "Find depth generator");
XnFPSData xnFPS;
nRetVal = xnFPSInit(&xnFPS, 180);
CHECK_RC(nRetVal, "FPS Init");
DepthMetaData depthMD;
while (!xnOSWasKeyboardHit())
{
nRetVal = context.WaitOneUpdateAll(depth);
if (nRetVal != XN_STATUS_OK)
{
printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
continue;
}
xnFPSMarkFrame(&xnFPS);
depth.GetMetaData(depthMD);
printf("Frame %d Middle point is: %u. FPS: %f\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2), xnFPSCalc(&xnFPS));
}
depth.Release();
scriptNode.Release();
context.Release();
return 0;
}
int Init()
{
XnStatus rc;
//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));
// Load OpenNI xml settings
char filePath[255];
int length = Util::Helpers::GetExeDirectory(filePath, sizeof(filePath));
filePath[length] = '\\';
strcpy(&filePath[length+1], SAMPLE_XML_PATH);
EnumerationErrors errors;
rc = deviceContext.InitFromXmlFile(filePath, &errors);
if (rc == XN_STATUS_NO_NODE_PRESENT)
{
//One reason would be if Microsoft SDK is installed beside PrimeSense. Device manager should say PrimeSense instead of Microsoft Kinect.
//XnChar strError[1024];
//errors.ToString(strError, 1024);
//LOGE("%s\n", strError);
return -1;
}
else if (rc != XN_STATUS_OK)
{
fprintf(stderr, "%s\n", xnGetStatusString(rc));
/*LOGE("Open failed: %s\n", xnGetStatusString(rc));*/
return (rc);
}
// Retrieve colour and depth nodes
rc = deviceContext.FindExistingNode(XN_NODE_TYPE_IMAGE, colorImageGenerator);
rc = deviceContext.FindExistingNode(XN_NODE_TYPE_DEPTH, depthImageGenerator);
// Set mirror mode to off
SetMirrorMode(false);
// Get a frame to perform checks on it
ImageMetaData colorImageMetaData;
DepthMetaData depthImageMetaData;
depthImageGenerator.GetMetaData(depthImageMetaData);
colorImageGenerator.GetMetaData(colorImageMetaData);
// Hybrid mode isn't supported in this sample
if (colorImageMetaData.FullXRes() != depthImageMetaData.FullXRes() || colorImageMetaData.FullYRes() != depthImageMetaData.FullYRes())
{
/*LOGE("The device depth and image resolution must be equal!\n");*/
return 1;
}
// RGB is the only image format supported.
if (colorImageMetaData.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
{
/*LOGE("The device image format must be RGB24\n");*/
return 1;
}
// Need to make sure the automatic alignment of colour and depth images is supported.
XnBool isSupported = depthImageGenerator.IsCapabilitySupported("AlternativeViewPoint");
if(!isSupported)
{
/*LOGE("Cannot set AlternativeViewPoint!\n");*/
return 1;
}
// Set it to VGA maps at 30 FPS
/*XnMapOutputMode mapMode;
mapMode.nXRes = XN_VGA_X_RES;
mapMode.nYRes = XN_VGA_Y_RES;
mapMode.nFPS = 60;
rc = g_depth.SetMapOutputMode(mapMode);
if(rc)
{
LOGE("Failed to set depth map mode: %s\n", xnGetStatusString(rc));
return 1;
}
mapMode.nFPS = 30;
rc = g_image.SetMapOutputMode(mapMode);
if(rc)
{
LOGE("Failed to set image map mode: %s\n", xnGetStatusString(rc));
return 1;
}*/
// Set automatic alignment of the colour and depth images.
rc = depthImageGenerator.GetAlternativeViewPointCap().SetViewPoint(colorImageGenerator);
if(rc)
{
/*LOGE("Failed to set depth map mode: %s\n", xnGetStatusString(rc));*/
return 1;
}
return XN_STATUS_OK;
}