Kinect采集深度和彩色图像并同时保存

#include <windows.h> #include <iostream> #include <NuiApi.h> #include <opencv2/opencv.hpp> #include <opencv2/highgui/highgui.hpp> using namespace std; using namespace cv; bool tracked[NUI_SKELETON_COUNT]={FALSE}; //CvPoint skeletonPoint[NUI_SKELETON_COUNT][NUI_SKELETON_POSITION_COUNT]={cvPoint(0,0)}; //CvPoint colorPoint[NUI_SKELETON_COUNT][NUI_SKELETON_POSITION_COUNT]={cvPoint(0,0)}; void getColorImage(HANDLE &colorEvent, HANDLE &colorStreamHandle, Mat &colorImage); void getDepthImage(HANDLE &depthEvent, HANDLE &depthStreamHandle, Mat &depthImage); //interface INuiSensor *pNuiSensor; //INuiFrameTexture* pColorToDepthTexture; INuiCoordinateMapper *pMapper; int i; long* X; long* Y; int main(int argc, char *argv[]) { Mat colorImage; colorImage.create(480,640, CV_8UC3); Mat depthImage; depthImage.create(60, 80, CV_8UC3); HANDLE colorEvent = CreateEvent( NULL, TRUE, FALSE, NULL ); HANDLE depthEvent = CreateEvent( NULL, TRUE, FALSE, NULL ); HANDLE colorStreamHandle = NULL; HANDLE depthStreamHandle = NULL; HRESULT hr; int iSensorCount=0; int ImgNum=0; char ImagesName[50]; // IplImage* ipl_img; hr=NuiGetSensorCount(&iSensorCount); if(FAILED(hr)) { return hr; } for(int i=0;i<iSensorCount;i++) { hr=NuiCreateSensorByIndex(i,&pNuiSensor); if(FAILED(hr)) { return hr; } hr=pNuiSensor->NuiStatus(); if(hr==S_OK){break;} pNuiSensor->Release(); } //HRESULT hr = NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX); hr = NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX); if( hr != S_OK ) { cout<<"NuiInitialize failed"<<endl; return hr; } /*if(ImgNum==0) { hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, NULL, 4, colorEvent, &colorStreamHandle); if( hr != S_OK ) { cout<<"Open the color Stream failed"<<endl; NuiShutdown(); return hr; } hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_320x240, NULL, 2, depthEvent, &depthStreamHandle); if( hr != S_OK ) { cout<<"Open the depth Stream failed"<<endl; NuiShutdown(); return hr; } } if(ImgNum==0) { hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, NULL, 4, colorEvent, &colorStreamHandle); if( hr != S_OK ) { cout<<"Open the color Stream failed"<<endl; NuiShutdown(); return hr; } hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_640x480, NULL, 2, depthEvent, &depthStreamHandle); if( hr != S_OK ) { cout<<"Open the depth Stream failed"<<endl; NuiShutdown(); return hr; } } if(ImgNum==2) { hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, NULL, 4, colorEvent, &colorStreamHandle); if( hr != S_OK ) { cout<<"Open the color Stream failed"<<endl; NuiShutdown(); return hr; } hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_80x60, NULL, 2, depthEvent, &depthStreamHandle); if( hr != S_OK ) { cout<<"Open the depth Stream failed"<<endl; NuiShutdown(); return hr; } }*/ if(ImgNum==0) { hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, NULL, 2, colorEvent, &colorStreamHandle); if( hr != S_OK ) { cout<<"Open the color Stream failed"<<endl; NuiShutdown(); return hr; } hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_80x60, NULL, 2, depthEvent, &depthStreamHandle); if( hr != S_OK ) { cout<<"Open the depth Stream failed"<<endl; NuiShutdown(); return hr; } } if(ImgNum>=4) { hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_1280x960, NULL, 4, colorEvent, &colorStreamHandle); if( hr != S_OK ) { cout<<"Open the color Stream failed"<<endl; NuiShutdown(); return hr; } hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_320x240, NULL, 2, depthEvent, &depthStreamHandle); if( hr != S_OK ) { cout<<"Open the depth Stream failed"<<endl; NuiShutdown(); return hr; } } namedWindow("colorImage", CV_WINDOW_AUTOSIZE); namedWindow("depthImage", CV_WINDOW_AUTOSIZE); while (1) { if(WaitForSingleObject(colorEvent, 0)==0) getColorImage(colorEvent, colorStreamHandle, colorImage); if(WaitForSingleObject(depthEvent, 0)==0) getDepthImage(depthEvent, depthStreamHandle, depthImage); /*if(WaitForSingleObject(colorEvent, 0)==0) getColorImage(colorEvent, colorStreamHandle, colorImage);*/ imshow("colorImage", colorImage); imshow("depthImage", depthImage); int c=cvWaitKey(1); if(c==27) break; if(c=='s') //char filename[100]; { sprintf(ImagesName, "cImage%.3d.bmp", ImgNum); //sprintf(filename,"%d.bmp",i); //IplImage*ipl_img=colorImage; imwrite(ImagesName,colorImage); sprintf(ImagesName, "dImage%.3d.bmp", ImgNum); imwrite(ImagesName,depthImage); ImgNum++; //cvCvtColor(colorImage,"1.bmp",0); //IplImage ipl_img=colorImage; //const CvArr* s=(CvArr*)&colorImage; //cvSaveImage(filename,colorImage); //cvReleaseImage(ipl_img); //cvDestroyWindow("colorImage"); //imsave(filename,colorImage); } } NuiShutdown(); return 0; } void getColorImage(HANDLE &colorEvent, HANDLE &colorStreamHandle, Mat &colorImage) { const NUI_IMAGE_FRAME *colorFrame = NULL; NuiImageStreamGetNextFrame(colorStreamHandle, 0, &colorFrame); INuiFrameTexture *pTexture = colorFrame->pFrameTexture; NUI_LOCKED_RECT LockedRect; pTexture->LockRect(0, &LockedRect, NULL, 0); /*int KINECT_DEPTH_W=640; int KINECT_DEPTH_H=480; uchar *pBuffer = (uchar*)(LockedRect.pBits) + i * LockedRect.Pitch; USHORT *pBufferRun = (USHORT*) pBuffer; for(int i=0; i<KINECT_DEPTH_W*KINECT_DEPTH_H; ++i) { // 真实深度 int RealDepth = (*pBufferRun & 0xfff8) >> 3; // RGB和深度对齐，跳过无效点 if (RealDepth==0) continue; // 有效点转换坐标 NuiImageGetColorPixelCoordinatesFromDepthPixel( KINECT_DEPTH_W==320 ? NUI_IMAGE_RESOLUTION_320x240 : NUI_IMAGE_RESOLUTION_640x480, NULL, i % KINECT_DEPTH_W, i / KINECT_DEPTH_W, *pBufferRun, X, Y ); }*/ if( LockedRect.Pitch != 0 ) { for (int i=0; i<colorImage.rows; i++) { uchar *ptr = colorImage.ptr<uchar>(i); uchar *pBuffer = (uchar*)(LockedRect.pBit