RGBD-SLAM实战

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/github_39611196/article/details/82917959

本文采用高博大佬的代码来跑kinect v1获取的彩色图和深度图。

首先是获取彩色图和深度图的代码：

#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);
}

彩色图示例：

深度图示例：

RGBD-SLAM运行结果：

采用pcl_viewer查看生成的result.pcd 

 

效果不怎么好，还是要继续提高姿势水平啊。

最后再次感谢高博大佬，RGB-D SLAM代码可参考我的GitHub。