RGBD-SLAM实战

本文采用高博大佬的代码来跑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行的指针                    //每个字节代表一个颜色信息，直接使用ucharuchar *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。

RGBD-SLAM实战相关推荐

从零手写RGBD SLAM
刚学习完ORB-SLAM2框架,但苦于没有实战项目,总感觉心里没底.偶然间发现了高翔博士的一起做RGBD SLAM博客,简单看了一点就感觉对自己大有帮助,hh大佬就是大佬,完全理解我们这群小菜鸡的现状 ...
BAD SLAM：捆绑束调整直接RGB-D SLAM
BAD SLAM:捆绑束调整直接RGB-D SLAM BAD SLAM: Bundle Adjusted Direct RGB-D SLAM 论文地址: http://openaccess.thecv ...
Towards Real-time Semantic RGB-D SLAM in Dynamic Environments（动态语义SLAM）
动态环境下的实时语义SLAM 简介摘要系统流程实验结果总结简介在ICRA 2021上看到这样一篇论文:Towards Real-time Semantic RGB-D SLAM in Dy ...
结构化场景中的RGB-D SLAM
点云PCL免费知识星球,点云论文速读. 文章:RGB-D SLAM with Structural Regularities 作者:Yanyan Li , Raza Yunus , Nikolas B ...
面向动态环境基于面元的RGB-D SLAM系统
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:robot L https://zhuanlan.zhihu.com/p/142175916 本 ...
Improving RGB-D SLAM in dynamic environments: A motion removal approach
一.贡献 (1)提出一种针对RGB-D的新的运动分割算法 (2)运动分割采用矢量量化深度图像 (3)数据集测试,并建立RGB-D SLAM系统二.Related work [1]R.K. Namde ...
3D Mapping with an RGB-D Camera（RGBD SLAM V2 ）论文笔记
这篇文章即是Felix Endres等人12年完成的RGB-D SLAM V2,是最早的为kinect风格传感器设计的SLAM系统之一没看到有相关的论文解析在Github上可找到开源代码,工程配置 ...
论文笔记_S2D.77_2013_TOR_使用RGBD相机的3D建图(RGBD SLAM V2)
目录基本情况摘要介绍系统流程特征提取运动估计 EMM:Environment Measurement Model 回环检测图优化建图OctoMap 参考基本情况出处:Endres ...
一起做RGB-D SLAM(3)
第三讲特征提取与配准师兄:同学们大家好,又到我们每周一次的"一起做RGB-D SLAM"时间啦!大家还记得上周我们讲了什么东西吗? 小萝卜:等一下师兄!我们的博客什么时候变成每 ...
调试笔记——《一起做RGB-D SLAM 》
花了前后大概两周的时间,好好整理了一遍高博的<一起做RGB-D SLAM >系列,调试过程中发现,在安装g2o这个包时遇到了很多问题,下面把解决问题的过程记录一下. 起初安装g2o这个包时 ...

RGBD-SLAM实战

RGBD-SLAM实战相关推荐

最新文章

热门文章