void SceneDrawer::updateScene(const xn::DepthMetaData &dmd,
const xn::SceneMetaData &smd)
{
if (!initialized)
init(dmd.XRes(), dmd.YRes(), dmd.ZRes());
unsigned int val = 0, numOfPoints = 0;
unsigned int xRes = dmd.XRes(), yRes = dmd.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
// calculate the accumulative depth histogram
memset(pDepthHist, 0, zRes*sizeof(float));
int numOfIterations = xRes * yRes;
for (int i = 0; i < numOfIterations; ++i, ++pDepth){
val = *pDepth;
if (val != 0){
pDepthHist[val]++;
numOfPoints++;
}
}
for (int i = 1; i < zRes; ++i)
pDepthHist[i] += pDepthHist[i-1];
if (numOfPoints > 0){
for (int i = 0; i < zRes; ++i)
pDepthHist[i] = floor(256.0f*(1.0f-pDepthHist[i]/(float)numOfPoints));
}
// turn depth map to a colored texture image
pDepth = dmd.Data();
XnUInt32 ci;
XnLabel label;
unsigned int histVal;
for (int i = 0; i < numOfIterations;
++i, ++pDepth, ++pLabels, pDestImage += 3){
val = *pDepth;
label = *pLabels;
if (label != 0)
ci = label % nColors;
else
ci = nColors;
if (val != 0){
histVal = pDepthHist[val];
pDestImage[0] = histVal * colors[ci][0];
pDestImage[1] = histVal * colors[ci][1];
pDestImage[2] = histVal * colors[ci][2];
}
else
pDestImage[0] = pDestImage[1] = pDestImage[2] = 0;
}
glBindTexture(GL_TEXTURE_2D, depthTexID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB,
GL_UNSIGNED_BYTE, pDepthTexBuf);
}
bool OpenNIVideo::update(osg::NodeVisitor* nv) {
//this is the main function of your video plugin
//you can either retrieve images from your video stream/camera/file
//or communicate with a thread to synchronize and get the data out
//the most important is to synchronize your data
//and copy the result to the VideoImageSteam used in this plugin
//
//0. you can collect some stats, for that you can use a timer
osg::Timer t;
{
//1. mutex lock access to the image video stream
OpenThreads::ScopedLock<OpenThreads::Mutex> _lock(this->getMutex());
osg::notify(osg::DEBUG_INFO)<<"osgART::OpenNIVideo::update() get new image.."<<std::endl;
XnStatus nRetVal = XN_STATUS_OK;
nRetVal=context.WaitAndUpdateAll();
CHECK_RC(nRetVal, "Update Data");
xnFPSMarkFrame(&xnFPS);
depth_generator.GetMetaData(depthMD);
const XnDepthPixel* pDepthMap = depthMD.Data();
//depth pixel floating point depth map.
image_generator.GetMetaData(imageMD);
const XnUInt8* pImageMap = imageMD.Data();
// Hybrid mode isn't supported in this sample
if (imageMD.FullXRes() != depthMD.FullXRes() || imageMD.FullYRes() != depthMD.FullYRes())
{
std::cerr<<"The device depth and image resolution must be equal!"<<std::endl;
exit(1);
}
// RGB is the only image format supported.
if (imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
{
std::cerr<<"The device image format must be RGB24"<<std::endl;
exit(1);
}
const XnDepthPixel* pDepth=pDepthMap;
const XnUInt8* pImage=pImageMap;
XnDepthPixel zMax = depthMD.ZRes();
//convert float buffer to unsigned short
for ( unsigned int i=0; i<(depthMD.XRes() * depthMD.YRes()); ++i )
{
*(_depthBufferByte + i) = 255 * (float(*(pDepth + i)) / float(zMax));
}
memcpy(_videoStreamList[0]->data(),pImage, _videoStreamList[0]->getImageSizeInBytes());
memcpy(_videoStreamList[1]->data(),_depthBufferByte, _videoStreamList[1]->getImageSizeInBytes());
//3. don't forget to call this to notify the rest of the application
//that you have a new video image
_videoStreamList[0]->dirty();
_videoStreamList[1]->dirty();
}
//4. hopefully report some interesting data
if (nv) {
const osg::FrameStamp *framestamp = nv->getFrameStamp();
if (framestamp && _stats.valid())
{
_stats->setAttribute(framestamp->getFrameNumber(),
"Capture time taken", t.time_m());
}
}
// Increase modified count every X ms to ensure tracker updates
if (updateTimer.time_m() > 50) {
_videoStreamList[0]->dirty();
_videoStreamList[1]->dirty();
updateTimer.setStartTick();
}
return true;
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static bool bInitialized = false;
static GLuint depthTexID;
static unsigned char* pDepthTexBuf;
static int texWidth, texHeight;
float topLeftX;
float topLeftY;
float bottomRightY;
float bottomRightX;
float texXpos;
float texYpos;
if(!bInitialized)
{
texWidth = getClosestPowerOfTwo(dmd.XRes());
texHeight = getClosestPowerOfTwo(dmd.YRes());
// printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
// printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
bInitialized = true;
topLeftX = dmd.XRes();
topLeftY = 0;
bottomRightY = dmd.YRes();
bottomRightX = 0;
texXpos =(float)dmd.XRes()/texWidth;
texYpos =(float)dmd.YRes()/texHeight;
memset(texcoords, 0, 8*sizeof(float));
texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
}
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();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
static unsigned int nZRes = dmd.ZRes();
static float* pDepthHist = (float*)malloc(nZRes* sizeof(float));
// Calculate the accumulative histogram
memset(pDepthHist, 0, nZRes*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<nZRes; nIndex++)
{
pDepthHist[nIndex] += pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<nZRes; nIndex++)
{
pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (g_bDrawPixels)
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (g_bDrawBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
else
{
xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
}
glBindTexture(GL_TEXTURE_2D, depthTexID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
// Display the OpenGL texture map
glColor4f(0.75,0.75,0.75,1);
glEnable(GL_TEXTURE_2D);
DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
glDisable(GL_TEXTURE_2D);
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
#ifndef USE_GLES
if (g_bPrintID)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
XnUInt32 nDummy = 0;
xnOSMemSet(strLabel, 0, sizeof(strLabel));
if (!g_bPrintState)
{
// Tracking
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// Tracking
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Tracking", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
}
else
{
// Nothing
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second));
}
glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
glRasterPos2i(com.X, com.Y);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
#endif
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
// Draw Joints
if (g_bMarkJoints)
{
// Try to draw all joints
DrawJoint(aUsers[i], XN_SKEL_HEAD);
DrawJoint(aUsers[i], XN_SKEL_NECK);
DrawJoint(aUsers[i], XN_SKEL_TORSO);
DrawJoint(aUsers[i], XN_SKEL_WAIST);
DrawJoint(aUsers[i], XN_SKEL_LEFT_COLLAR);
DrawJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER);
DrawJoint(aUsers[i], XN_SKEL_LEFT_ELBOW);
DrawJoint(aUsers[i], XN_SKEL_LEFT_WRIST);
DrawJoint(aUsers[i], XN_SKEL_LEFT_HAND);
DrawJoint(aUsers[i], XN_SKEL_LEFT_FINGERTIP);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_COLLAR);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_WRIST);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_HAND);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_FINGERTIP);
DrawJoint(aUsers[i], XN_SKEL_LEFT_HIP);
DrawJoint(aUsers[i], XN_SKEL_LEFT_KNEE);
DrawJoint(aUsers[i], XN_SKEL_LEFT_ANKLE);
DrawJoint(aUsers[i], XN_SKEL_LEFT_FOOT);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_HIP);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_KNEE);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_ANKLE);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_FOOT);
}
#ifndef USE_GLES
glBegin(GL_LINES);
#endif
// Draw Limbs
DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
if (!DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_WRIST))
{
DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
}
else
{
DrawLimb(aUsers[i], XN_SKEL_LEFT_WRIST, XN_SKEL_LEFT_HAND);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HAND, XN_SKEL_LEFT_FINGERTIP);
}
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
if (!DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_WRIST))
{
DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
}
else
{
DrawLimb(aUsers[i], XN_SKEL_RIGHT_WRIST, XN_SKEL_RIGHT_HAND);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HAND, XN_SKEL_RIGHT_FINGERTIP);
}
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);
#ifndef USE_GLES
glEnd();
#endif
}
}
if (g_bPrintFrameID)
{
static XnChar strFrameID[80];
xnOSMemSet(strFrameID, 0, 80);
XnUInt32 nDummy = 0;
xnOSStrFormat(strFrameID, sizeof(strFrameID), &nDummy, "%d", dmd.FrameID());
glColor4f(1, 0, 0, 1);
glRasterPos2i(10, 10);
glPrintString(GLUT_BITMAP_HELVETICA_18, strFrameID);
}
}
void SceneDrawer::depthMapCreating(unsigned char *pDestImage, const xn::DepthMetaData &dmd, const xn::SceneMetaData &smd)
{
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
unsigned int nValue = 0;
static unsigned int nZRes = dmd.ZRes();
static float* pDepthHist = (float*)malloc(nZRes* sizeof(float));
// Calculate the accumulative histogram
memset(pDepthHist, 0, nZRes*sizeof(float));
for (int nY=0; nY<g_nYRes; nY++)
for (int nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
for (int i=1; i<nZRes; i++)
pDepthHist[i] += pDepthHist[i-1];
if (nNumberOfPoints)
for (int i=1; i<nZRes; i++)
pDepthHist[i] = (unsigned int)(256 * (1.0f - (pDepthHist[i] / nNumberOfPoints)));
pDepth = dmd.Data();
// Prepare the texture map
for (int nY=0; nY<g_nYRes; nY++)
{
for (int nX=0; nX < g_nXRes; nX++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (drawBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
nColorID = nColors;
if (nValue != 0)
{
pDestImage[0] = pDepthHist[nValue] * Colors[nColorID][0];
pDestImage[1] = pDepthHist[nValue] * Colors[nColorID][1];
pDestImage[2] = pDepthHist[nValue] * Colors[nColorID][2];
}
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (Width - g_nXRes) *3;
}
}
