// デプスのヒストグラムを作成
depth_hist getDepthHistgram(const xn::DepthGenerator& depth,
const xn::DepthMetaData& depthMD)
{
// デプスの傾向を計算する(アルゴリズムはNiSimpleViewer.cppを利用)
const int MAX_DEPTH = depth.GetDeviceMaxDepth();
depth_hist depthHist(MAX_DEPTH);
unsigned int points = 0;
const XnDepthPixel* pDepth = depthMD.Data();
for (XnUInt y = 0; y < depthMD.YRes(); ++y) {
for (XnUInt x = 0; x < depthMD.XRes(); ++x, ++pDepth) {
if (*pDepth != 0) {
depthHist[*pDepth]++;
points++;
}
}
}
for (int i = 1; i < MAX_DEPTH; ++i) {
depthHist[i] += depthHist[i-1];
}
if ( points != 0) {
for (int i = 1; i < MAX_DEPTH; ++i) {
depthHist[i] =
(unsigned int)(256 * (1.0f - (depthHist[i] / points)));
}
}
return depthHist;
}
int GenerateMinecraftCharacter(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, const XnRGB24Pixel* image)
{
int ret = 0;
int xRes = dmd.XRes();
int yRes = dmd.YRes();
cv::Mat inputImage = cv::Mat(yRes, xRes, CV_8UC3);
cv::Mat skin = cv::Mat::zeros(cv::Size(64,32), CV_8UC3);
XnToCV(image,&inputImage);
cv::cvtColor(inputImage,inputImage,CV_RGB2BGR);
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
int i = 0;
for (i = 0; i < nUsers; ++i) {
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) break;
}
// No users being tracked
if (i == nUsers) return -1;
ret = GenerateSkin(aUsers[i], &inputImage, &skin);
printf("GenerateSkin returned %d on user %d\n",ret,(int)aUsers[i]);
cv::imwrite("skin.png",skin);
SegmentUser(aUsers[i], &inputImage, smd);
DrawDebugPoints(aUsers[i], &inputImage);
cv::imwrite("blah.png",inputImage);
sync();
system("convert skin.png -transparent black skin.png && composite -geometry +32+0 hardhat.png skin.png skin.png");
sync();
return ret;
}
std::string getDepthMap(bool br) {
{
ReadLock r_lock(myLock);
stringstream emitter;
emitter << "{DepthMap," << g_depthMD.XRes() << ","<< g_depthMD.YRes() << "}[";
if(g_depthMD.XRes() != 0){
for(int i = 0; i < g_depthMD.XRes(); i++) {
if(i>0 && br == true) emitter << "<BR>";
for(int j = 0; j < g_depthMD.YRes(); j++) {
emitter << g_depthMD(i, j);
emitter << ",";
}
}
}
emitter << "]";
return emitter.str();
}
}
inline void getDepthMapFromMetaData( const xn::DepthMetaData& depthMetaData, cv::Mat& depthMap, XnUInt64 noSampleValue, XnUInt64 shadowValue )
{
int cols = depthMetaData.XRes();
int rows = depthMetaData.YRes();
depthMap.create( rows, cols, CV_16UC1 );
const XnDepthPixel* pDepthMap = depthMetaData.Data();
// CV_Assert( sizeof(unsigned short) == sizeof(XnDepthPixel) );
memcpy( depthMap.data, pDepthMap, cols*rows*sizeof(XnDepthPixel) );
cv::Mat badMask = (depthMap == (double)noSampleValue) | (depthMap == (double)shadowValue) | (depthMap == 0);
// mask the pixels with invalid depth
depthMap.setTo( cv::Scalar::all( CvCapture_OpenNI::INVALID_PIXEL_VAL ), badMask );
}
IplImage* CvCapture_OpenNI::retrievePointCloudMap()
{
if( !depthMetaData.Data() )
return 0;
cv::Mat depth;
getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue );
const int badPoint = INVALID_PIXEL_VAL;
const float badCoord = INVALID_COORDINATE_VAL;
int cols = depthMetaData.XRes(), rows = depthMetaData.YRes();
cv::Mat pointCloud_XYZ( rows, cols, CV_32FC3, cv::Scalar::all(badPoint) );
cv::Ptr<XnPoint3D> proj = new XnPoint3D[cols*rows];
cv::Ptr<XnPoint3D> real = new XnPoint3D[cols*rows];
for( int y = 0; y < rows; y++ )
{
for( int x = 0; x < cols; x++ )
{
int ind = y*cols+x;
proj[ind].X = (float)x;
proj[ind].Y = (float)y;
proj[ind].Z = depth.at<unsigned short>(y, x);
}
}
depthGenerator.ConvertProjectiveToRealWorld(cols*rows, proj, real);
for( int y = 0; y < rows; y++ )
{
for( int x = 0; x < cols; x++ )
{
// Check for invalid measurements
if( depth.at<unsigned short>(y, x) == badPoint ) // not valid
pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( badCoord, badCoord, badCoord );
else
{
int ind = y*cols+x;
pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( real[ind].X*0.001f, real[ind].Y*0.001f, real[ind].Z*0.001f); // from mm to meters
}
}
}
outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].mat = pointCloud_XYZ;
return outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].getIplImagePtr();
}
//- - - - - - - - - - - - - - - - - -
//
void C_TrackWindow::initTexs( const xn::DepthMetaData& depthMD ) {
struct getClosestPowerOfTwo {
inline unsigned int operator()( unsigned int n) {
unsigned int m = 2;
while(m < n) m<<=1;
return m;
};
};
GLuint texID = 0;
glGenTextures( 1, &depthTexID_ );
texWidth_ = getClosestPowerOfTwo()( depthMD.XRes() );
texHeight_ = getClosestPowerOfTwo()( depthMD.YRes() );
pDepthTexBuf_ = new unsigned char[ texWidth_ * texHeight_ * 4 ];
glBindTexture( GL_TEXTURE_2D, depthTexID_ );
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
}
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 not initialized
set parameters and reserve memory space
*/
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();
// get the depth resolution
static unsigned int nZRes = dmd.ZRes();
static float* pDepthHist = (float*)malloc(nZRes* sizeof(float));
// Calculate the accumulative histogram
memset(pDepthHist, 0, nZRes*sizeof(float));
// count the number of pixels of every possible depth value
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];
}
// calculate percentage for every depth value
// the larger the value is, the darker the pixel should be
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::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);
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, ros::Publisher pub_body, edwin::SceneAnalysis scene, edwin::People person)
{
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();
// 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)));
}
}
XnPoint3D coms[20];
XnUInt32 labels[20] = {0};
for (int i = 0; i < 20; ++i)
{
coms[i] = xnCreatePoint3D(0,0,0);
}
pDepth = dmd.Data();
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
if (label < 20 && label > 0)
{
coms[label].X += nX;
coms[label].Y += nY;
coms[label].Z += *pDepth;
labels[label]++;
}
}
else
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
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[3] = "";
//Initialize the scene array
// scene.crowd = [];
for (int i = 0; i < 20; ++i)
{
if (labels[i] == 0)
continue;
coms[i].X /= labels[i];
coms[i].Y /= labels[i];
coms[i].Z /= labels[i];
sprintf(strLabel, "%d", i);
glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
glRasterPos2i(coms[i].X, coms[i].Y);
// glRasterPos2i(320, 240); //testing for midpoint of Kinect
person.ID = i;
person.xpos = coms[i].X;
person.ypos = coms[i].Y;
person.zpos = coms[i].Z;
scene.crowd[i] = person;
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
pub_body.publish(scene);
for(int i = 0; i < 20; ++i)
{
scene.crowd[i].ID = 0;
}
}
void SimKinect::DrawDepthMapWithUsers(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static bool bInitialized = false;
//image used in opencv;
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());
pDepthTexBuf = (unsigned char*)malloc(dmd.XRes()*dmd.YRes()*3*sizeof(unsigned char));
// printf("Initializing 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;
}
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 nXRes = dmd.XRes();
XnUInt16 nYRes = dmd.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
int* p_depth_map = depth_map;
//Calculate the accumulative histogram
memset(pDepthHist, 0, MAX_DEPTH*sizeof(float));
for (nY=0; nY<nYRes; nY++)
{
for (nX=0; nX<nXRes; nX++)
{
nValue = *pDepth;
*p_depth_map++ = nValue;
if (nValue != 0)
{
pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
pDepthHist[nIndex] += pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (bDrawPixels)
{
// Prepare the texture map
for (nY=0; nY<nYRes; nY++)
{
for (nX=0; nX < nXRes; nX++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (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;
}
}
}
else
{
xnOSMemSet(pDepthTexBuf, 0, 3*nXRes*nYRes);
}
//Draw depth image
unsigned char* pix_ptr = pDepthTexBuf;
for ( int i=0 ; i<video_size_height ; i++ ) {
unsigned char* row_pointer = depth_frame+i*video_size_width*3;
for ( int j=0 ; j<video_size_width*3; j+=3 ) {
row_pointer[j] = *pix_ptr++;
row_pointer[j+1]=*pix_ptr++;
row_pointer[j+2]=*pix_ptr++;
}
}
}
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();
// 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 (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 = g_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);
xnOSMemSet(strLabel, 0, sizeof(strLabel));
if (!g_bPrintState)
{
// Tracking
sprintf(strLabel, "%d", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// Tracking
sprintf(strLabel, "%d - Tracking", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
}
else
{
// Nothing
sprintf(strLabel, "%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]))
{
#ifndef USE_GLES
glBegin(GL_LINES);
#endif
glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
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);
DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
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);
//---------------- get skeleton data ----------------------
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_HEAD, head);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_NECK, neck);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_TORSO, torso);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_SHOULDER, leftShoulder);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_ELBOW, leftElbow);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_HAND, leftHand);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_SHOULDER, rightShoulder);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_ELBOW, rightElbow);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_HAND, rightHand);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_HIP, leftHip);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_KNEE, leftKnee);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_FOOT, leftFoot);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_HIP, rightHip);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_KNEE, rightKnee);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_FOOT, rightFoot);
//-------------- store frame data in frameData ------------
char headX[30], headY[30], headZ[30], headC[30];
strcpy(frameData, "");
char space[2] = " ";
sprintf(headX, "%f", head.position.X);
sprintf(headY, "%f", head.position.Y);
sprintf(headZ, "%f", head.position.Z);
sprintf(headC, "%f", head.fConfidence);
strcat(frameData, headX);
strcat(frameData, space);
strcat(frameData, headY);
strcat(frameData, space);
strcat(frameData, headZ);
strcat(frameData, space);
strcat(frameData, headC);
strcat(frameData, space);
//std::cout << "Frame data: " << frameData << std::endl;
std::cout << "frameData to be sent: " << frameData << std::endl;
HandleConnection(frameData);
////------- print to console -------
//xnOSGetTimeStamp(&nNow);
//nNow /= 1000;
//std::cout << "-------------time: " << nNow << "---------------\n"
// << "User " << aUsers[i] <<std::endl
// << " Head: " << head.position.X << " "
// << head.position.Y << " " << head.position.Z
// << head.fConfidence << std::endl;
#ifndef USE_GLES
glEnd();
#endif
}
}
}
void DrawDepthMap(const xn::DepthMetaData& dm)
{
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)
{
XnUInt16 nXRes = dm.XRes();
XnUInt16 nYRes = dm.YRes();
texWidth = getClosestPowerOfTwo(nXRes);
texHeight = getClosestPowerOfTwo(nYRes);
depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
bInitialized = true;
topLeftX = nXRes;
topLeftY = 0;
bottomRightY = nYRes;
bottomRightX = 0;
texXpos =(float)nXRes/texWidth;
texYpos =(float)nYRes/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 = dm.XRes();
XnUInt16 g_nYRes = dm.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnUInt16* pDepth = dm.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 = dm.Data();
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
nValue = *pDepth;
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
pDestImage[0] = nHistValue;
pDestImage[1] = nHistValue;
pDestImage[2] = nHistValue;
}
else
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
}
pDepth++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
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.5,0.5,0.5,1);
glEnable(GL_TEXTURE_2D);
DrawTexture(dm.XRes(),dm.YRes(),0,0);
glDisable(GL_TEXTURE_2D);
}
int main ( int argc, char ** argv )
{
//
// Initializing Calibration Related
//
// ARTagHelper artagHelper ( colorImgWidth, colorImgHeight, ARTAG_CONFIG_FILE, ARTAG_POS_FILE );
ARTagHelper artagHelper ( colorImgWidth, colorImgHeight, ARTAG_CONFIG_A3_FILE, ARTAG_POS_A3_FILE );
ExtrCalibrator extrCalibrator ( 6, KINECT_INTR_FILE, KINECT_DIST_FILE );
// unsigned char * kinectImgBuf = new unsigned char[colorImgWidth * colorImgHeight * 3];
//
// Initializing OpenNI Settings
//
int ctlWndKey = -1;
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
//
// Initialize Context Object
//
nRetVal = g_Context.InitFromXmlFile ( CONFIG_XML_PATH, g_ScriptNode, &errors );
if ( nRetVal == XN_STATUS_NO_NODE_PRESENT )
{
XnChar strError[1024];
errors.ToString ( strError, 1024 );
printf ( "XN_STATUS_NO_NODE_PRESENT:\n%s\n", strError );
system ( "pause" );
return ( nRetVal );
}
else if ( nRetVal != XN_STATUS_OK )
{
printf ( "Open FAILED:\n%s\n", xnGetStatusString ( nRetVal ) );
system ( "pause" );
return ( nRetVal );
}
//
// Handle the Depth Generator Node.
//
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_DEPTH, g_DepthGen );
if ( nRetVal != XN_STATUS_OK )
{
printf ( "No Depth Node Exists! Please Check your XML.\n" );
return ( nRetVal );
}
//
// Handle the Image Generator node
//
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_IMAGE, g_ImageGen );
if ( nRetVal != XN_STATUS_OK )
{
printf ( "No Image Node Exists! Please Check your XML.\n" );
return ( nRetVal );
}
// g_DepthGen.GetAlternativeViewPointCap().SetViewPoint( g_ImageGen );
g_DepthGen.GetMetaData ( g_DepthMD );
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_ImageMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24 );
assert ( g_DepthMD.PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT );
//
// Create OpenCV Showing Window and Related Data Structures
//
cv::namedWindow ( IMAGE_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::namedWindow ( DEPTH_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::Mat depthImgMat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_16UC1 );
cv::Mat depthImgShow ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_8UC3 );
cv::Mat colorImgMat ( g_ImageMD.YRes(), g_ImageMD.XRes(), CV_8UC3 );
#define ARTAG_DEBUG
#ifdef ARTAG_DEBUG
cv::setMouseCallback ( IMAGE_WIN_NAME, ClickOnMouse, 0 );
#endif
bool flipColor = true;
//
// Start to Loop
//
while ( ctlWndKey != ESC_KEY_VALUE )
{
//
// Try to Get New Frame From Kinect
//
nRetVal = g_Context.WaitAnyUpdateAll ();
g_DepthGen.GetMetaData ( g_DepthMD );
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_DepthMD.FullXRes() == g_DepthMD.XRes() && g_DepthMD.FullYRes() == g_DepthMD.YRes() );
assert ( g_ImageMD.FullXRes() == g_ImageMD.XRes() && g_ImageMD.FullYRes() == g_ImageMD.YRes() );
GlobalUtility::CopyColorRawBufToCvMat8uc3 ( (const XnRGB24Pixel *)(g_ImageMD.Data()), colorImgMat );
#ifdef SHOW_DEPTH_WINDOW
GlobalUtility::CopyDepthRawBufToCvMat16u ( (const XnDepthPixel *)(g_DepthMD.Data()), depthImgMat );
// GlobalUtility::ConvertDepthCvMat16uToYellowCvMat ( depthImgMat, depthImgShow );
GlobalUtility::ConvertDepthCvMat16uToGrayCvMat ( depthImgMat, depthImgShow );
cv::imshow ( DEPTH_WIN_NAME, depthImgShow );
#endif
ctlWndKey = cvWaitKey ( 15 );
if ( ctlWndKey == 'f' || ctlWndKey == 'F' )
{
artagHelper.Clear();
artagHelper.FindMarkerCorners ( (unsigned char *)(g_ImageMD.Data()) );
artagHelper.PrintMarkerCornersPos2dInCam ();
extrCalibrator.ExtrCalib ( artagHelper );
std::cout << "\nKinect Extr Matrix:" << std::endl;
extrCalibrator.PrintMatrix ( extrCalibrator.GetMatrix ( ExtrCalibrator::EXTR ) );
std::cout << "Reprojection ERROR = "
<< extrCalibrator.ComputeReprojectionErr ( artagHelper ) << std::endl
// << extrCalibrator.ComputeReprojectionErr ( artagHelper.m_MarkerCornerPosCam2d, artagHelper.m_MarkerCornerPos3d, 24 ) << std::endl
<< "Valid Marker Number = " << artagHelper.GetValidMarkerNumber() << std::endl
<< std::endl;
extrCalibrator.SaveMatrix ( ExtrCalibrator::EXTR, KINECT_EXTR_FILE );
}
if ( ctlWndKey == 's' || ctlWndKey == 'S' )
{
flipColor = !flipColor;
}
if ( flipColor ) {
cv::cvtColor ( colorImgMat, colorImgMat, CV_RGB2BGR );
}
artagHelper.DrawMarkersInCameraImage ( colorImgMat );
cv::imshow ( IMAGE_WIN_NAME, colorImgMat );
}
g_Context.Release ();
system ( "pause" );
exit ( EXIT_SUCCESS );
}
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, XnUserID player, xn::ImageMetaData& imd)
{
texWidth = 640;
texHeight = 480;
LEFT = 0; RIGHT = 640;
TOP = 0; BOTTOM = 480;
nValue = 0;
nIndex = 0;
nX = 0; nY = 0;
nNumberOfPoints = 0;
g_nXRes = dmd.XRes();
g_nYRes = dmd.YRes();
pDestImage = pDepthTexBuf;
pDepth = dmd.Data();
pixel = imd.RGB24Data();
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)));
}
}
// printf("Debug: %i\n",focus);
pDepth = (short unsigned int*)dmd.Data();
///{
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++)
{
nValue = *pDepth;
if(nX == (int)centerScreen[0] && nY == (int)centerScreen[1]){
if (calibrationMode){
depthVal = nValue;
// printf("depthVal: %i\n",depthVal);
}
}
//printf("Depth: %i \n",nValue);
label = *pLabels;
// XnUInt32 nColorID = label % nColors;
if (label != focus)
{
if(calibrationMode){
pDestImage[0] = pixel->nRed;
pDestImage[1] = pixel->nGreen;
pDestImage[2] = pixel->nBlue;
pDestImage[3] = 255;
} else {
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
pDestImage[3] = 0;
}
} else {
pDestImage[0] = pixel->nRed;
pDestImage[1] = pixel->nGreen;
pDestImage[2] = pixel->nBlue;
pDestImage[3] = 255;
//find max/min values for width and height boundaries
if (nX > (unsigned int)LEFT) {
LEFT = nX;
}
if (nX < (unsigned int)RIGHT) {
RIGHT = nX;
}
if (nY > (unsigned int)TOP) {
TOP = nY;
}
if (nY < (unsigned int)BOTTOM) {
BOTTOM = nY;
}
}
pixel++;
pDepth++;
pLabels++;
pDestImage+=4;
}
pDestImage += (texWidth - g_nXRes) *4;
}
//}*/
// Display the OpenGL texture map
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, texWidth, texHeight, 0, GL_RGBA, GL_UNSIGNED_BYTE, pDepthTexBuf);
nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
g_UserGenerator.GetCoM(aUsers[0], com);
CENTER = com.X;
//glDisable(GL_BLEND);
//glDisable(GL_TEXTURE_2D);
/*
char strLabel[20] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
if (aUsers[i] == player)
sprintf(strLabel, "%d (Player)", aUsers[i]);
else
sprintf(strLabel, "%d", aUsers[i]);
//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); */
// }
// Draw skeleton of user
// glBegin(GL_LINES);
// //glColor4f(1,0,0,1.0f);
// glVertex3f(-1, 1, 0);
// glVertex3f(1, -1, 0);
// //glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1);
// DrawLimb(1, XN_SKEL_HEAD, XN_SKEL_NECK);
//
// DrawLimb(1, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
// DrawLimb(1, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
// DrawLimb(1, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
//
// DrawLimb(1, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
// DrawLimb(1, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
// DrawLimb(1, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
//
// DrawLimb(1, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
// DrawLimb(1, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
//
// DrawLimb(1, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
// DrawLimb(1, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
// DrawLimb(1, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
//
// DrawLimb(1, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
// DrawLimb(1, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
// DrawLimb(1, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
// glEnd();
//XnSkeletonJoint eJoint1 = XN_SKEL_RIGHT_HAND;
}
void DepthMapLogger::DumpDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static char name_str[20], comment_str[255];
// Don't do anything if the h5 file is not open
if(!p_h5_file_ || !p_frames_group_) { return; }
// References to various bits of the HDF5 output
H5File &file(*p_h5_file_);
Group &frames_group(*p_frames_group_);
// This frame's index is the number of frames we've previously saved
hsize_t this_frame_idx = frames_group.getNumObjs();
// Create this frame's group
snprintf(name_str, 20, "frame_%06lld", this_frame_idx);
snprintf(comment_str, 255, "Data for frame %lld", this_frame_idx);
Group this_frame_group(frames_group.createGroup(name_str));
this_frame_group.setComment(".", comment_str);
// Create attributes for this group
Attribute idx_attr = this_frame_group.createAttribute("idx", PredType::NATIVE_HSIZE, DataSpace());
idx_attr.write(PredType::NATIVE_HSIZE, &this_frame_idx);
// Create this frame's datasets
DSetCreatPropList creat_props;
uint16_t fill_value(0);
creat_props.setFillValue(PredType::NATIVE_UINT16, &fill_value);
hsize_t rows(static_cast<hsize_t>(dmd.YRes())), cols(static_cast<hsize_t>(dmd.XRes()));
hsize_t creation_dims[2] = { rows, cols };
hsize_t max_dims[2] = { rows, cols };
DataSpace mem_space(2, creation_dims, max_dims);
DataSet depth_ds(this_frame_group.createDataSet(
"depth", PredType::NATIVE_UINT16, mem_space, creat_props));
DataSet label_ds(this_frame_group.createDataSet(
"label", PredType::NATIVE_UINT16, mem_space, creat_props));
// Get depth and label buffers
const uint16_t *p_depths = dmd.Data();
const uint16_t *p_labels = smd.Data();
// Write depth data
depth_ds.write(p_depths, PredType::NATIVE_UINT16);
// Write label data
label_ds.write(p_labels, PredType::NATIVE_UINT16);
// Convert non-zero depth values into 3D point positions
XnPoint3D *pts = new XnPoint3D[rows*cols];
uint16_t *pt_labels = new uint16_t[rows*cols];
size_t n_pts(0);
for(size_t depth_idx(0); depth_idx < rows*cols; ++depth_idx) {
// Skip zero depth values
if(p_depths[depth_idx] == 0) {
continue;
}
// Store projective-values
pts[n_pts].X = depth_idx % cols;
pts[n_pts].Y = depth_idx / cols;
pts[n_pts].Z = p_depths[depth_idx];
pt_labels[n_pts] = p_labels[depth_idx];
++n_pts;
}
g_DepthGenerator.ConvertProjectiveToRealWorld(n_pts, pts, pts);
if (n_pts > 0)
{
// Create points dataset
hsize_t pts_creation_dims[2] = { n_pts, 3 };
hsize_t pts_max_dims[2] = { n_pts, 3 };
DataSpace pts_mem_space(2, pts_creation_dims, pts_max_dims);
DataSet pts_ds(this_frame_group.createDataSet(
"points", PredType::NATIVE_FLOAT, pts_mem_space, creat_props));
hsize_t pt_labels_creation_dims[1] = { n_pts };
hsize_t pt_labels_max_dims[1] = { n_pts };
DataSpace pt_labels_mem_space(1, pt_labels_creation_dims, pt_labels_max_dims);
DataSet pt_labels_ds(this_frame_group.createDataSet(
"point_labels", PredType::NATIVE_UINT16, pt_labels_mem_space, creat_props));
// Write points data
pts_ds.write(pts, PredType::NATIVE_FLOAT);
pt_labels_ds.write(pt_labels, PredType::NATIVE_UINT16);
}
// Create groups to store detected users
Group users_group(this_frame_group.createGroup("users"));
// Dump each user in turn
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
// Create a group for this user
snprintf(name_str, 20, "user_%02d", aUsers[i]);
Group this_user_group(users_group.createGroup(name_str));
// Create attributes for this group
Attribute user_idx_attr = this_user_group.createAttribute(
"idx", PredType::NATIVE_UINT16, DataSpace());
uint16_t this_user_idx(aUsers[i]);
user_idx_attr.write(PredType::NATIVE_UINT16, &this_user_idx);
// Write state (if any)
H5std_string strwritebuf;
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
strwritebuf = "tracking";
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
strwritebuf = "calibrating";
}
else
{
// Nothing
strwritebuf = "looking";
}
StrType strdatatype(PredType::C_S1, strwritebuf.size()); // of length 256 characters
Attribute user_state_attr = this_user_group.createAttribute(
"state", strdatatype, DataSpace());
user_state_attr.write(strdatatype, strwritebuf);
static Joint joints[g_NumJointTypes];
int n_joints_found = 0;
// Try to dump all joints
for(int jt_idx=0; jt_idx < g_NumJointTypes; ++jt_idx)
{
if(DumpJoint(aUsers[i], g_JointTypes[jt_idx], joints[n_joints_found]))
{
++n_joints_found;
}
}
if (n_joints_found > 0)
{
// Create joints dataset
hsize_t joints_dim[] = { n_joints_found };
DataSpace joints_space(1, joints_dim);
DataSet joints_ds(this_user_group.createDataSet("joints", joint_dt_, joints_space));
joints_ds.write(joints, joint_dt_);
}
}
}
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;
}
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player)
{
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();
// 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();
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
else
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
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[20] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
if (aUsers[i] == player)
sprintf(strLabel, "%d (Player)", aUsers[i]);
else
sprintf(strLabel, "%d", aUsers[i]);
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);
}
// Draw skeleton of user
if (player != 0)
{
glBegin(GL_LINES);
glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1);
// gesture
static int gesture = 0;
static XnPoint3D previousLeftHandPt;
XnPoint3D newLeftHandPt;
newLeftHandPt = getJointPoint(player, XN_SKEL_LEFT_HAND);
if(previousLeftHandPt.X > 0 && previousLeftHandPt.X < 640)
if(previousLeftHandPt.X - newLeftHandPt.X > 60)
gesture = 1;
else if(previousLeftHandPt.X - newLeftHandPt.X < -60)
gesture = 2;
else if(previousLeftHandPt.Y - newLeftHandPt.Y > 60)
gesture = 3;
else if(previousLeftHandPt.Y - newLeftHandPt.Y < -60)
gesture = 4;
else
gesture = 0;
if(gesture != 0)
printf("gesture: %d\n", gesture);
previousLeftHandPt = newLeftHandPt;
// head
XnPoint3D pt = getJointPoint(player, XN_SKEL_HEAD);
// save
saveLocation(pt, newLeftHandPt, gesture);
DrawLimb(player, XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(player, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
DrawLimb(player, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
DrawLimb(player, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
DrawLimb(player, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(player, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(player, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(player, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(player, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
glEnd();
}
}
/***********************************************************************************
* This routine will draw the scene. This is the OpenGL display function / idle
function callback routine. There have been additions to this routine (from the depth
map sample).
a. Along with updating the context, we also update the session manager.
b. We draw the hand point as well. The hand point is represented using a white
square.
***********************************************************************************/
void draw(){
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //clear the gl buffers
rc = cxt.WaitAnyUpdateAll(); //first update the context - refresh the depth/image data coming from the sensor
if(rc != XN_STATUS_OK){ //if the update failed, i.e. couldn't be read
printf("\nERROR:Read failed... Quitting!\n"); //print error message
exit(0); //exit the program
}
else if(inSession){
sessionMgr->Update(&cxt); //NEW ADDITION - update the session manager
depthGen.GetMetaData(depthMapMetaData); //grab the depth map meta data
long xSize = depthMapMetaData.XRes(); //this meta data will give us the true width
long ySize = depthMapMetaData.YRes(); //and height of the scene
long totalSize = xSize * ySize; //total size of the depth map is gotten this way
const XnDepthPixel* depthMapData; //this array will contain the value of depth of each pixel in depth map
depthMapData = depthMapMetaData.Data(); //this will grab the depth values and store it into the array
int i, j,colorToSet; //color to set will be calculated for the pixel based on its depth
int depth; //to store the depth at a point
glLoadIdentity(); //set the matrix to an identity matrix
glOrtho(0, xSize, ySize, 0, -1, 1); //set the orthogonal view. -1 and 1 define the near and far clip off points along z axis
/************************************** DRAWS THE DEPTH MAP *************************************/
glBegin(GL_POINTS); //start the drawing of points
/*
Note: the depthMapData is a pointer to an array that is linear. It contains integer values of depth in millimeters. This linear array must be imagined
as a matrix of dimensions --> xSize,ySize
*/
for(i = 0;i<xSize;i++) //this loop will run through width of the depth map
{
for(j=0;j<ySize;j++) //this will run through the height
{
depth = depthMapMetaData(i,j); //get the depth info from the depthMapData. lvalue is (i*Xmax + j) in order to translate matrix coords to linear ones
colorToSet = MAX_COLOR - depth/COLOR_ZONES;
if((depth < DEPTH_THRESH_FAR) && (depth > DEPTH_THRESH_NEAR)) { //if the current pixel depth is within the threshold limits (acceptable range in millimeters)
glColor3ub(0,colorToSet,0); //then paint this pixel
glVertex3f(i,j,0);
}
}
}
glEnd(); //end drawing sequence
glBegin(GL_POINTS);
pixelmap[4][4].g=255;
for(i = 0;i<xSize;i++) //this loop will run through width of the depth map
{
for(j=0;j<ySize;j++) //this will run through the height
{
if (pixelmap[i][j].r > 0) {
glColor3ub(pixelmap[i][j].r,pixelmap[i][j].g, pixelmap[i][j].b); //then paint this pixel
glVertex3f(i,j,0);
}
}
}
glEnd();
/********************************* DRAWS THE HAND POINT ***************************************/
if(isHandPointNull() == false){
glColor3f(255,255,255); //set the color to white
glBegin(GL_POLYGON); //start drawing the polygon
int xCo = handPointCoords.X; //xCoord of the hand point
int yCo = handPointCoords.Y; //yCoord of the hand point
int zCo = handPointCoords.Z;
int size = 4; //size of this square
glVertex3f(xCo-size,yCo-size,0); //plot the top left corner
glVertex3f(xCo+size,yCo-size,0); //plot the top right corner
glVertex3f(xCo+size,yCo+size,0); //plot the bot right corner
glVertex3f(xCo-size,yCo+size,0); //plot the bot left corner
glEnd(); //stop drawing the polygon
//draws a bounding box around the hand using lines
int boxSize = 200;
int xBox = handPointCoords.X - boxSize/2;
int yBox = handPointCoords.Y - boxSize/2;
//pixelmap[-1][-1].g=233;
if (zCo < g_dist) {
glColor3f(255,0,0);
for (int i = -5; i <= 5; i++)
for (int j = -5; j <= 5; j++)
pixelmap[(xCo + i)%640][(yCo + j)%480].r = 255;
}
glBegin(GL_LINES);
glVertex3f(xBox,yBox,0); //top edge
glVertex3f(xBox+boxSize,yBox,0);
glVertex3f(xBox,yBox,0); //left edge
glVertex3f(xBox,yBox+boxSize,0);
glVertex3f(xBox+boxSize,yBox,0); //right edge
glVertex3f(xBox+boxSize,yBox+boxSize,0);
glVertex3f(xBox,yBox+boxSize,0); //bottom edge
glVertex3f(xBox+boxSize,yBox+boxSize,0);
glEnd();
}
/**********************************************************************************************/
}
glutSwapBuffers(); //for double buffering
}
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();
// 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 (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 = g_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)
{
if (g_bPrintID)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
xnOSMemSet(strLabel, 0, sizeof(strLabel));
if (!g_bPrintState)
{
// Tracking
sprintf(strLabel, "%d", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// Tracking
sprintf(strLabel, "%d - Tracking", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
}
else
{
// Nothing
sprintf(strLabel, "%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);
}
//Draw skeleton
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
glBegin(GL_LINES);
glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
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);
DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
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);
glEnd();
}
}
}
void PublishPeopleImage(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, image_transport::Publisher& pub)
{
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
unsigned int nValue = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
cv::Mat peopleSegmentation(g_nYRes, g_nXRes, CV_8UC3);
// Prepare the texture map
for (unsigned int nY=0; nY<g_nYRes; nY++)
{
uchar* pDestImage = peopleSegmentation.ptr(nY);
for (unsigned int nX=0; nX < g_nXRes; nX++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (*pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (nValue != 0)
{
pDestImage[0] = 255 * Colors[nColorID][0];
pDestImage[1] = 255 * Colors[nColorID][1];
pDestImage[2] = 255 * Colors[nColorID][2];
}
}
pDepth++;
pLabels++;
pDestImage+=3;
}
}
// todo: stop and start with respect to odometry: segmentation works best if robot is standing still
// publish
try
{
IplImage img = (IplImage)peopleSegmentation;
sensor_msgs::ImagePtr msg = (sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8"));
msg->header.stamp = ros::Time::now();
pub.publish(msg);
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("[openni_tracker] Could not convert IplImage to ROS message");
}
// cv_bridge::CvImage bridgeImage; did not work
// bridgeImage.image = peopleSegmentation.clone();
// sensor_msgs::ImagePtr msg = bridgeImage.toImageMsg();
// pub.publish(msg);
// display for checking the output
// cv::namedWindow("Test");
// imshow("Test", peopleSegmentation);
// uchar key = cv::waitKey(10);
// if (key == 'r')
// {
// g_UserGenerator.StopGenerating();
// std::cout << "stop\n";
// }
// if (key == 'c')
// {
// g_UserGenerator.StartGenerating();
// std::cout << "start\n";
// }
}
int main ( int argc, char * argv[] )
{
//
// Initialize OpenNI Settings
//
XnStatus nRetVal = XN_STATUS_OK;
xn::ScriptNode scriptNode;
xn::EnumerationErrors errors;
//
// Initialize Context Object
//
nRetVal = g_Context.InitFromXmlFile ( CONFIG_XML_PATH, scriptNode, &errors );
if ( nRetVal == XN_STATUS_NO_NODE_PRESENT ) {
XnChar strError[1024];
errors.ToString(strError, 1024);
printf ( "XN_STATUS_NO_NODE_PRESENT:\n%s\n", strError );
system ( "pause" );
return ( nRetVal );
}
else if ( nRetVal != XN_STATUS_OK ) {
printf ( "Open failed: %s\n", xnGetStatusString(nRetVal) );
system ( "pause" );
return ( nRetVal );
}
//
// Handle Image & Depth Generator Node
//
bool colorFlag = true;
bool depthFlag = true;
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_DEPTH, g_DepthGen );
if ( nRetVal != XN_STATUS_OK ) {
printf("No depth node exists!\n");
depthFlag = false;
}
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_IMAGE, g_ImageGen );
if ( nRetVal != XN_STATUS_OK ) {
printf("No image node exists!\n");
colorFlag = false;
}
// g_DepthGen.GetAlternativeViewPointCap().SetViewPoint( g_ImageGen );
if ( depthFlag ) {
g_DepthGen.GetMetaData ( g_DepthMD );
assert ( g_DepthMD.PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT );
}
if ( colorFlag ) {
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_ImageMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24 );
}
g_DepthImgShow = cv::Mat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_8UC1 );
g_DepthImgMat = cv::Mat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_16UC1 );
g_ColorImgMat = cv::Mat ( g_ImageMD.YRes(), g_ImageMD.XRes(), CV_8UC3 );
//
// Start to Loop
//
bool flipColor = true;
int ctlWndKey = -1;
g_StartTickCount = GetTickCount();
g_HeadTrackingFrameCount = 0;
while ( ctlWndKey != ESC_KEY_VALUE )
{
nRetVal = g_Context.WaitOneUpdateAll ( g_DepthGen );
// nRetVal = g_Context.WaitAnyUpdateAll();
#ifdef HANDLING_IMAGE_DATA
if ( colorFlag )
{
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_ImageMD.FullXRes() == g_ImageMD.XRes() );
assert ( g_ImageMD.FullYRes() == g_ImageMD.YRes() );
GlobalUtility::CopyColorRawBufToCvMat8uc3 ( (const XnRGB24Pixel *)(g_ImageMD.Data()), g_ColorImgMat );
if ( ctlWndKey == 's' || ctlWndKey == 'S' ) { // Switch
flipColor = !flipColor;
}
if ( flipColor ) {
cv::cvtColor ( g_ColorImgMat, g_ColorImgMat, CV_RGB2BGR );
}
cv::namedWindow ( IMAGE_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::imshow ( IMAGE_WIN_NAME, g_ColorImgMat );
}
#endif
#ifdef HANDLING_DEPTH_DATA
if ( depthFlag )
{
g_DepthGen.GetMetaData(g_DepthMD);
// assert ( g_DepthMD.FullXRes() == g_DepthMD.XRes() );
// assert ( g_DepthMD.FullYRes() == g_DepthMD.YRes() );
GlobalUtility::CopyDepthRawBufToCvMat16u ( (const XnDepthPixel *)(g_DepthMD.Data()), g_DepthImgMat );
GlobalUtility::ConvertDepthCvMat16uToGrayCvMat ( g_DepthImgMat, g_DepthImgShow );
/*
cv::putText(colorImgMat,
GlobalUtility::DoubleToString(g_ImageMD.FPS()) + " FPS",
cv::Point(10, 450),
cv::FONT_ITALIC,
0.7,
cv::Scalar(255, 255, 255, 0),
2,
8,
false);
*/
cv::namedWindow ( DEPTH_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::imshow ( DEPTH_WIN_NAME, g_DepthImgShow );
}
#endif
XnFieldOfView fov;
g_DepthGen.GetFieldOfView( fov );
std::cout << "HFov = " << fov.fHFOV << std::endl
<< "VFov = " << fov.fVFOV << std::endl;
ctlWndKey = cvWaitKey ( 5 );
g_HeadTrackingFrameCount++;
g_CurrTickCount = GetTickCount();
std::cout << "FPS = "
<< 1000 / ( ( double )( g_CurrTickCount - g_StartTickCount ) / ( double )( g_HeadTrackingFrameCount ) )
<< std::endl;
}
g_Context.Release ();
exit ( EXIT_SUCCESS );
}
void Gestures::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( 1280 );
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(m_texcoords, 0, 8*sizeof(float));
m_texcoords[0] = texXpos, m_texcoords[1] = texYpos, m_texcoords[2] = texXpos, m_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();
//Calculate the accumulative histogram
memset(m_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)
{
m_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
m_pDepthHist[nIndex] += m_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
m_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (m_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (m_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 (m_bDrawBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % m_nColors;
if (label == 0)
{
nColorID = m_nColors;
}
if (nValue != 0)
{
nHistValue = m_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);
}