void multipleKinects_ImageNodes()
{
NodeInfoList image_node_info_list;
IplImage* kinectFirstRGBImage;
IplImage* kinectSecondRGBImage;
XnStatus status;
Context context1, context2;
int c;
bool bShouldrun = true;
context1.Init();
context2.Init();
Query query;
status = query.SetVendor("PrimeSense");
status = context1.EnumerateProductionTrees(XN_NODE_TYPE_IMAGE, &query, image_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 = image_node_info_list.Begin();
NodeInfo& firstNode = *nodeIt;
nodeIt++;
NodeInfo& secondNode = *nodeIt;
ImageGenerator rgb1, rgb2;
status = firstNode.GetInstance(rgb1);
status = secondNode.GetInstance(rgb2);
status = context1.CreateProductionTree(firstNode);
status = context2.CreateProductionTree(secondNode);
status = rgb1.Create(context1);
status = rgb2.Create(context2);
status = context1.StartGeneratingAll();
status = context2.StartGeneratingAll();
cvNamedWindow("RGB1", 1);
cvNamedWindow("RGB2", 1);
while (bShouldrun)
{
kinectFirstRGBImage = cvCreateImage(cvSize(640,480),8,3);
kinectSecondRGBImage = cvCreateImage(cvSize(640,480),8,3);
// Wait for new data to be available
status = context2.WaitOneUpdateAll(rgb2);
status = context1.WaitOneUpdateAll(rgb1);
if (status != XN_STATUS_OK)
{
printf("Failed updating data: %s\n");
xnGetStatusString(status);
continue;
}
// Take current rgb map
const XnRGB24Pixel* pFirstImageMap = rgb1.GetRGB24ImageMap();
const XnRGB24Pixel* pSecondImageMap = rgb2.GetRGB24ImageMap();
for (int y=0; y<XN_VGA_Y_RES; y++)
{
uchar *ptr1 = (uchar*)kinectFirstRGBImage->imageData + y*kinectFirstRGBImage->widthStep;
uchar *ptr2 = (uchar*)kinectSecondRGBImage->imageData + y*kinectSecondRGBImage->widthStep;
for (int x=0; x<XN_VGA_X_RES; x++)
{
ptr1[3*x] = pFirstImageMap->nBlue;
ptr1[3*x + 1] = pFirstImageMap->nGreen;
ptr1[3*x + 2] = pFirstImageMap->nRed;
pFirstImageMap++;
ptr2[3*x] = pSecondImageMap->nBlue;
ptr2[3*x + 1] = pSecondImageMap->nGreen;
ptr2[3*x + 2] = pSecondImageMap->nRed;
pSecondImageMap++;
}
}
cvShowImage("RGB1", kinectFirstRGBImage);
cvShowImage("RGB2", kinectSecondRGBImage);
cvReleaseImageHeader(&kinectFirstRGBImage);
cvReleaseImageHeader(&kinectSecondRGBImage);
c = cvWaitKey(1);
if (c == 27)
bShouldrun = false;
}
}
// Clean-up
context1.Shutdown();
context2.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 multipleKinects_DepthNodes()
{
NodeInfoList depth_node_info_list;
XnStatus status;
Context context1, context2;
int c;
bool bShouldrun = true;
context1.Init();
context2.Init();
Query query;
status = query.SetVendor("PrimeSense");
status = context1.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& firstNode = *nodeIt;
nodeIt++;
NodeInfo& secondNode = *nodeIt;
DepthGenerator depth1, depth2;
status = firstNode.GetInstance(depth1);
status = secondNode.GetInstance(depth2);
status = context1.CreateProductionTree(firstNode);
status = context2.CreateProductionTree(secondNode);
status = depth1.Create(context1);
status = depth2.Create(context2);
status = context1.StartGeneratingAll();
status = context2.StartGeneratingAll();
cvNamedWindow("Depth1", 1);
cvNamedWindow("Depth2", 1);
// Create an OpenCv matrix
CvMat* firstDepthMetersMat = cvCreateMat(480, 640, CV_16UC1);
CvMat* seocndDepthMetersMat = cvCreateMat(480, 640, CV_16UC1);
IplImage *kinectFirstDepthImage;
IplImage *kinectSecondDepthImage;
while (bShouldrun)
{
status = context1.WaitOneUpdateAll(depth1);
status = context2.WaitOneUpdateAll(depth2);
if (status)
{
printf("Error: %s", xnGetStatusString(status));
continue;
}
//Take current depth map
const XnDepthPixel* pFirstDepthMap = depth1.GetDepthMap();
const XnDepthPixel* pSecondDepthMap = depth2.GetDepthMap();
for (int y=0; y<XN_VGA_Y_RES; y++)
{
for (int x=0; x<XN_VGA_X_RES; x++)
{
firstDepthMetersMat->data.s[y*XN_VGA_X_RES+x]=10*pFirstDepthMap[y*XN_VGA_X_RES+x];
seocndDepthMetersMat->data.s[y*XN_VGA_X_RES+x]=10*pSecondDepthMap[y*XN_VGA_X_RES+x];
}
}
kinectFirstDepthImage = cvCreateImage(cvSize(640,480),8,1);
kinectSecondDepthImage = cvCreateImage(cvSize(640,480),8,1);
cvGetImage(firstDepthMetersMat, kinectFirstDepthImage);
cvGetImage(seocndDepthMetersMat, kinectSecondDepthImage);
cvShowImage("Depth1", kinectFirstDepthImage);
cvShowImage("Depth2", kinectSecondDepthImage);
cvReleaseImageHeader(&kinectFirstDepthImage);
cvReleaseImageHeader(&kinectSecondDepthImage);
c = cvWaitKey(1);
if (c == 27)
bShouldrun = false;
}
}
// Clean-up
context1.Shutdown();
context2.Shutdown();
}
void KinectSensor::initDevice(int id, int idRefC, bool aligned, char* path)
{
idCam = id;
idRefCam = idRefC;
// Initialise sensors
context.Init();
if (path != NULL)
{
context.OpenFileRecording(path);
context.FindExistingNode(XN_NODE_TYPE_DEPTH, depthNode);
context.FindExistingNode(XN_NODE_TYPE_IMAGE, rgbNode);
//Player xPlayer;
//context.OpenFileRecording(path, xPlayer);
//xPlayer.SetRepeat(false);
//context.FindExistingNode(XN_NODE_TYPE_DEPTH, depthNode);
//context.FindExistingNode(XN_NODE_TYPE_IMAGE, rgbNode);
//xPlayer.GetNumFrames(depthNode.GetName(), uFrames);
}
else
{
NodeInfoList depth_node_info_list, image_node_info_list;
Query query;
query.SetVendor("PrimeSense");
context.EnumerateProductionTrees(XN_NODE_TYPE_DEPTH, &query, depth_node_info_list, NULL);
context.EnumerateProductionTrees(XN_NODE_TYPE_IMAGE, &query, image_node_info_list, NULL);
//depth & RGB node instantiation
NodeInfoList::Iterator dNodeIt = depth_node_info_list.Begin();
NodeInfoList::Iterator rgbNodeIt = image_node_info_list.Begin();
int contNodes = 0;
NodeInfo& depth_Node = *dNodeIt;
NodeInfo& rgb_Node = *rgbNodeIt;
while (contNodes <= idCam)
{
depth_Node = *dNodeIt++;
rgb_Node = *rgbNodeIt++;
contNodes++;
}
depth_Node.GetInstance(depthNode);
context.CreateProductionTree(depth_Node);
depthNode.Create(context);
rgb_Node.GetInstance(rgbNode);
context.CreateProductionTree(rgb_Node);
rgbNode.Create(context);
}
if (aligned && depthNode.IsCapabilitySupported("AlternativeViewPoint"))
depthNode.GetAlternativeViewPointCap().SetViewPoint(rgbNode);
// Initialise Camera with Intrinsics
depthNode.GetRealProperty ("ZPPS", pSize);
pSize *= 2.0; //the resolution is reduced due to the bandwidth
depthNode.GetIntProperty ("ZPD",focalLength);
ox = XN_VGA_X_RES/2;
oy = XN_VGA_Y_RES/2;
double depth_focal_length_VGA_ = (double)focalLength/pSize;
XnUInt64 zeroPlaneD;
depthNode.GetIntProperty("ZPD", zeroPlaneD);
XnDouble baseline;
depthNode.GetRealProperty ("LDDIS", baseline);
// Initialise Camera with Extrinsics
initExtrinsics(id, idRefC);
}