rgbd slam_RGB模型

#include <windows.h>
#include <iostream> 
#include <NuiApi.h>
#include <opencv2/opencv.hpp>
#include<string.h>
using namespace std;
using namespace cv;
void getColorImage(HANDLE &colorEvent, HANDLE &colorStreamHandle, Mat &colorImage);
void getDepthImage(HANDLE &depthEvent, HANDLE &depthStreamHandle, Mat &depthImage);
int i = 0;
string rgb = "./rgb/";
string title = ".png";
string depth = "./depth/";
int main(int argc, char *argv[])
{
Mat colorImage;
colorImage.create(480, 640, CV_8UC3);
Mat depthImage;
depthImage.create(480, 640, CV_8UC1);
HANDLE colorEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
HANDLE depthEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
HANDLE colorStreamHandle = NULL;
HANDLE depthStreamHandle = NULL;
HRESULT hr = NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH);
if (hr != S_OK)
{
cout << "NuiInitialize failed" << endl;
return hr;
}
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, NUI_IMAGE_RESOLUTION_640x480, 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);
imshow("colorImage", colorImage);
imshow("depthImage", depthImage);
imwrite(rgb + to_string(i) + title, colorImage);
imwrite( depth + to_string(i) + title, depthImage);
i++;
if (cvWaitKey(1) == 27)
break;
}
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);
if (LockedRect.Pitch != 0)
{
for (int i = 0; i<colorImage.rows; i++)
{
uchar *ptr = colorImage.ptr<uchar>(i);  //第i行的指针                    
//每个字节代表一个颜色信息，直接使用uchar
uchar *pBuffer = (uchar*)(LockedRect.pBits) + i * LockedRect.Pitch;
for (int j = 0; j<colorImage.cols; j++)
{
ptr[3 * j] = pBuffer[4 * j];  //内部数据是4个字节，0-1-2是BGR，第4个现在未使用 
ptr[3 * j + 1] = pBuffer[4 * j + 1];
ptr[3 * j + 2] = pBuffer[4 * j + 2];
}
}
}
else
{
cout << "捕捉色彩图像出现错误" << endl;
}
pTexture->UnlockRect(0);
NuiImageStreamReleaseFrame(colorStreamHandle, colorFrame);
}
void getDepthImage(HANDLE &depthEvent, HANDLE &depthStreamHandle, Mat &depthImage)
{
const NUI_IMAGE_FRAME *depthFrame = NULL;
NuiImageStreamGetNextFrame(depthStreamHandle, 0, &depthFrame);
INuiFrameTexture *pTexture = depthFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect(0, &LockedRect, NULL, 0);
RGBQUAD q;
if (LockedRect.Pitch != 0)
{
//4.5、将数据转换为OpenCV的Mat格式
for (int i = 0; i < depthImage.rows; i++)
{
uchar *ptr = depthImage.ptr<uchar>(i);  //第i行的指针
//深度图像数据含有两种格式，这里像素的低12位表示一个深度值，高4位未使用；
//注意这里需要转换，因为每个数据是2个字节，存储的同上面的颜色信息不一样，
uchar *pBufferRun = (uchar*)(LockedRect.pBits) + i * LockedRect.Pitch;
USHORT * pBuffer = (USHORT*)pBufferRun;
for (int j = 0; j < depthImage.cols; j++)
{
ptr[j] = 255 - (uchar)(256 * pBuffer[j] / 0x0fff);  //直接将数据归一化处理
}
}
}
else
{
cout << "捕捉深度图像出现错误" << endl;
}
pTexture->UnlockRect(0);
NuiImageStreamReleaseFrame(depthStreamHandle, depthFrame);
}