void calculateHistogram()
{
DepthGenerator* pDepthGen = getDepthGenerator();
if (pDepthGen == NULL)
return;
XnUInt32 nZRes = pDepthGen->GetDeviceMaxDepth() + 1;
if (g_pDepthHist == NULL)
{
g_pDepthHist = new float[nZRes];
}
xnOSMemSet(g_pDepthHist, 0, nZRes*sizeof(float));
int nNumberOfPoints = 0;
XnDepthPixel nValue;
const XnDepthPixel* pDepth = pDepthGen->GetDepthMap();
const XnDepthPixel* pDepthEnd = pDepth + (pDepthGen->GetDataSize() / sizeof(XnDepthPixel));
while (pDepth != pDepthEnd)
{
nValue = *pDepth;
XN_ASSERT(nValue <= nZRes);
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
XnUInt32 nIndex;
for (nIndex = 1; nIndex < nZRes; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
for (nIndex = 1; nIndex < nZRes; nIndex++)
{
if (g_pDepthHist[nIndex] != 0)
{
g_pDepthHist[nIndex] = (nNumberOfPoints-g_pDepthHist[nIndex]) / nNumberOfPoints;
}
}
}
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();
}
void mixRGB_Depth()
{
bool bShouldRun = true;
int c;
XnStatus nRetVal = XN_STATUS_OK;
Context context;
// Initialize context object
nRetVal = context.Init();
// Check error code
if (nRetVal)
printf("Error: %s", xnGetStatusString(nRetVal));
context.SetGlobalMirror(true);
//Create Depth generator node
DepthGenerator depth;
nRetVal = depth.Create(context);
// Check error code
if (nRetVal)
printf("Error: %s", xnGetStatusString(nRetVal));
// Create an ImageGenetor node
ImageGenerator image;
nRetVal = image.Create(context);
if (nRetVal)
printf("Error: %s", xnGetStatusString(nRetVal));
// Sync the DepthGenerator with the ImageGenerator
nRetVal = depth.GetAlternativeViewPointCap().SetViewPoint(image);
if (nRetVal)
printf("Error: %s", xnGetStatusString(nRetVal));
//Set it to VGA maps at 30 fps
XnMapOutputMode mapMode;
mapMode.nXRes = XN_VGA_X_RES;
mapMode.nYRes = XN_VGA_Y_RES;
mapMode.nFPS = 30;
nRetVal = depth.SetMapOutputMode(mapMode);
// Make it start generating data
nRetVal = context.StartGeneratingAll();
if (nRetVal)
printf("Error: %s", xnGetStatusString(nRetVal));
// Create an OpenCv matrix
CvMat* depthMetersMat = cvCreateMat(480, 640, CV_16UC1);
IplImage *kinectDepthImage;
kinectDepthImage = cvCreateImage(cvSize(640,480), 16, 1);
IplImage *rgbimg = cvCreateImageHeader(cvSize(640,480), 8,3);
// Main loop
while (bShouldRun)
{
//wait for new data to be available
nRetVal = context.WaitOneUpdateAll(depth);
if (nRetVal)
{
printf("Error: %s", xnGetStatusString(nRetVal));
continue;
}
//Take current depth map
const XnDepthPixel* pDepthMap = depth.GetDepthMap();
for (int y=0; y<XN_VGA_Y_RES; y++)
{
for (int x=0; x<XN_VGA_X_RES; x++)
{
depthMetersMat->data.s[y*XN_VGA_X_RES+x]=10*pDepthMap[y*XN_VGA_X_RES+x];
}
}
cvGetImage(depthMetersMat, kinectDepthImage);
//take current image
const XnRGB24Pixel* pImage = image.GetRGB24ImageMap();
//process image data
XnRGB24Pixel* ucpImage = const_cast<XnRGB24Pixel*>(pImage);
cvSetData(rgbimg, ucpImage, 640*3);
cvShowImage("RGB", kinectDepthImage);
c = cvWaitKey(1);
if (c == 27)
bShouldRun = false;
}
cvReleaseImageHeader(&kinectDepthImage);
context.Shutdown();
}
void OneKinect(int type)
{
NodeInfoList image_node_info_list;
NodeInfoList depth_node_info_list;
XnStatus status;
Context context;
int c;
IplImage* kinectRGBImage;
bool bShouldrun = true;
context.Init();
// status = context.InitFromXmlFile("D:\\initXml.xml");
Query query;
switch (type) {
case 0:
status = query.SetVendor("PrimeSense");
status = context.EnumerateProductionTrees(XN_NODE_TYPE_DEPTH, &query, depth_node_info_list, NULL);
//status = context.EnumerateProductionTrees(XN_NODE_TYPE_DEPTH, NULL, depth_node_info_list, NULL);
if (status != XN_STATUS_OK)
printf("Enumerating devices failed. Reason: %s", xnGetStatusString(status));
else
{
NodeInfoList::Iterator nodeIt = depth_node_info_list.Begin();
// NodeInfo& selectedNode = *depth_node_info_list.Begin();
// nodeIt++;
NodeInfo& selectedNode = *nodeIt;
printf("instance %s\n", selectedNode.GetInstanceName());
DepthGenerator depth;
status = selectedNode.GetInstance(depth);
status = context.CreateProductionTree(selectedNode);
status = depth.Create(context);
status = context.StartGeneratingAll();
cvNamedWindow("Depth", 1);
// Create an OpenCv matrix
CvMat* depthMetersMat = cvCreateMat(480, 640, CV_16UC1);
IplImage *kinectDepthImage;
while (bShouldrun)
{
status = context.WaitOneUpdateAll(depth);
if (status)
{
printf("Error: %s", xnGetStatusString(status));
continue;
}
//Take current depth map
const XnDepthPixel* pDepthMap = depth.GetDepthMap();
for (int y=0; y<XN_VGA_Y_RES; y++)
{
for (int x=0; x<XN_VGA_X_RES; x++)
{
depthMetersMat->data.s[y*XN_VGA_X_RES+x]=10*pDepthMap[y*XN_VGA_X_RES+x];
}
}
kinectDepthImage = cvCreateImage(cvSize(640,480),8,1);
cvGetImage(depthMetersMat, kinectDepthImage);
cvShowImage("Depth", kinectDepthImage);
cvReleaseImageHeader(&kinectDepthImage);
c = cvWaitKey(1);
if (c == 27)
bShouldrun = false;
}
}
break;
case 1:
status = context.EnumerateProductionTrees(XN_NODE_TYPE_IMAGE, NULL, image_node_info_list, NULL);
if (status != XN_STATUS_OK)
printf("Enumerating devices failed. Reason: %s", xnGetStatusString(status));
else
{
NodeInfo& selectedNode = *image_node_info_list.Begin();
xn::ImageGenerator rgb;
status = selectedNode.GetInstance(rgb);
status = context.CreateProductionTree(selectedNode);
status = rgb.Create(context);
status = context.StartGeneratingAll();
cvNamedWindow("RGB", 1);
while (bShouldrun)
{
kinectRGBImage = cvCreateImage(cvSize(640,480),8,3);
// Wait for new data to be available
status = context.WaitOneUpdateAll(rgb);
if (status != XN_STATUS_OK)
{
printf("Failed updating data: %s\n");
xnGetStatusString(status);
continue;
}
// Take current rgb map
const XnRGB24Pixel* pImageMap = rgb.GetRGB24ImageMap();
for (int y=0; y<XN_VGA_Y_RES; y++)
{
uchar *ptr = (uchar*)kinectRGBImage->imageData + y*kinectRGBImage->widthStep;
for (int x=0; x<XN_VGA_X_RES; x++)
{
ptr[3*x] = pImageMap->nBlue;
ptr[3*x + 1] = pImageMap->nGreen;
ptr[3*x + 2] = pImageMap->nRed;
pImageMap++;
}
}
cvShowImage("RGB", kinectRGBImage);
cvReleaseImageHeader(&kinectRGBImage);
c = cvWaitKey(1);
if (c == 27)
bShouldrun = false;
}
}
break;
default:
cout << "Incorrect number" << endl;
} // end switch
// Clean-up
context.Shutdown();
}
void main()
{
//---------------------------------------------------
bool bShouldRun = true;
int c ;
XnStatus nRetVal = XN_STATUS_OK;
Context context;
// Initialize context object
nRetVal = context.Init();
// check error code
if(nRetVal)
printf("Error : %s", xnGetStatusString(nRetVal));
context.SetGlobalMirror(true); //mirror image
// Create a DepthGenerator node
DepthGenerator depth;
nRetVal = depth.Create(context);
// check error code
if(nRetVal)
printf("Failed to create depth generator: %s\n", xnGetStatusString(nRetVal));
/// Create an ImageGenerator node
ImageGenerator image;
nRetVal = image.Create(context);
if(nRetVal)
printf("Failed to create image generator: %s\n", xnGetStatusString(nRetVal));
if(nRetVal)
printf("Failed to match Depth and RGB points of view: %s\n",xnGetStatusString(nRetVal));
// Set it to VGA maps at 30 FPS
XnMapOutputMode mapMode;
mapMode.nXRes = XN_VGA_X_RES;
mapMode.nYRes = XN_VGA_Y_RES;
mapMode.nFPS = 30;
nRetVal = depth.SetMapOutputMode(mapMode);
// Make it start generating data
nRetVal = context.StartGeneratingAll();
// check error code
if(nRetVal)
printf("Error : %s", xnGetStatusString(nRetVal));
//create a OpenCv matrix
CvMat* depthMetersMat = cvCreateMat(480, 640, CV_16UC1);
IplImage *kinectDepthImage;
kinectDepthImage = cvCreateImage( cvSize(640,480),16,1);
IplImage *kinectDepthImage_raw= cvCreateImage( cvSize(640,480),16,1);
IplImage rgbimg;
XnPoint3D* pDepthPointSet = new XnPoint3D[ 640*480 ];
// Main loop
while (bShouldRun)
{
// Wait for new data to be available
nRetVal = context.WaitOneUpdateAll(depth);
if (nRetVal != XN_STATUS_OK)
{
printf("Failed updating data: %s\n", xnGetStatusString(nRetVal));
continue;
}
// Take current depth map
const XnDepthPixel* pDepthMap = depth.GetDepthMap();
xn::DepthGenerator rDepth;
//Copy the depth values
for (int y=0; y<XN_VGA_Y_RES; y++)
for(int x=0;x<XN_VGA_X_RES;x++)
{
depthMetersMat->data.s[y * XN_VGA_X_RES + x ] = 20*pDepthMap[y * XN_VGA_X_RES + x];
// Convert the coordinates in the camera coordinate system
pDepthPointSet[y * XN_VGA_X_RES + x].X = (XnFloat) x;
pDepthPointSet[y * XN_VGA_X_RES + x].Y = (XnFloat) y;
pDepthPointSet[y * XN_VGA_X_RES + x].Z = pDepthMap[y * XN_VGA_X_RES + x];
}
cvGetImage(depthMetersMat,kinectDepthImage_raw);
cvShowImage("Depth stream", kinectDepthImage_raw);
unsigned char* picture_RGB = new unsigned char[XN_VGA_X_RES * XN_VGA_Y_RES * 3];
//initialization with the retrieved data
memcpy(picture_RGB, (unsigned char*)image.GetRGB24ImageMap(),XN_VGA_Y_RES * XN_VGA_X_RES * 3);
//From BGR to RGB
for(int i = 0 ; i < XN_VGA_X_RES * XN_VGA_Y_RES ; i++)
{
unsigned char temp = picture_RGB[i*3];
picture_RGB[i*3] = picture_RGB[i*3+2];
picture_RGB[i*3+2] = temp;
}
cv::Mat colorMatRes(XN_VGA_Y_RES, XN_VGA_X_RES, CV_8UC3, picture_RGB);
rgbimg=colorMatRes; //Conversion from cv::mat to IplImage format
cvShowImage("Color stream",&rgbimg); //Display the RGB stream
// free memory
delete picture_RGB;
c = cvWaitKey(1);
if (c == 27)
bShouldRun = false; //exit main loop
}
// Clean-up
cvDestroyWindow("Color stream");
cvDestroyWindow("Depth stream");
cvReleaseImageHeader(&kinectDepthImage);
delete pDepthPointSet;
}
