Kinect Azure DK编程实战:用OpenCV、PCL操作和可视化Kinect数据

青亭网( ID:qingtinwang )--链接科技前沿,服务商业创新

本文来自: 无雨森  投稿

Kinect Azure DK 是微软今年推出的新款深度摄像头,从数据上全面领先上一代Kinect。但是,由于是新一代Kinect,围绕它的生态系统还非常的贫瘠。只有微软官方在Github上开源的SDK。

Kinect Azure DK SDK    https://github.com/microsoft/Azure-Kinect-Sensor-SDK 。在该SDK中,微软把最重要的生成深度图的代码闭源了。没有源码,只有微软提供的depthEngine.dll。

Azure_Kinect_04-1000x667

我们只能把SDK通过CMake编译,编译得到k4a.lib,k4a.dll。depthEngine.dll只能通过微软提供的Azure Kinect SDK 1.1.0.msi安装后得到。

不过比较重要的是,我们可以通过微软开放的samples去学习怎么用代码操作kinect并可视化 RGB、Depth、IR、点云。

至该文章发出之日,网上还没有关于Kinect的OpenCV、PCL应用的教程。您看到的是全网第一篇用 OpenCV 和 PCL操作Kinect 的 raw data 的文章。

OpenCV是应用最广泛的图像处理库。我们可以使用OpenCV去处理Kinect输出的 RGB + Depth 图像。另外,我们也可以通过OpenCV可视化 RGB、Depth、IR 图像。

PCL是应用最广泛的点云处理库。我们可以使用PCL去处理由 Depth 图输出计算得到的点云。我们可以通过基于VTK的可视化模块,来可视化点云。其中VTK是一个通用的可视化库,我们可以使用VTK可视化点云、3D模型,2D图片等等。

这篇文章,我将列出如何使用OpenCV,PCL来可视化Kinect的数据。

首先,我们看如何用OpenCV读取Kinect的RGB图像,IR图像,Depth图像。

微信图片_20190808133134

如上Gif动图。我实现的是一个在同一台电脑上打开两台Kinect的输出结果。但是下面的代码只能看一个Kinect。我用这个动图做演示的意思是,我写的OpenCV代码可以输出至少两个Kinect的数据。

我们可以看到三种图,其中 左上角彩色图:Kinect的 Depth 图。

Kinect上有一个深度摄像头。深度摄像头由 两个红外投影仪(IR projector) 和 一个红外接收器(IR sensor)。如下图所示。

微信图片_20190808133155

左边圆形的摄像头是"IR sensor",右边一高一低两个方形镜头是两个"IR projector",只不过一个是宽视角,一个是窄视角。宽视角的"IR projector"可以获取更大视角的场景的深度图。窄视角的"IR projector"可以获取更近更远可视范围的场景的深度图。

这一代Kinect深度摄像头用的是 TOF 原理,全称"time of flight",译为“飞行时间”。

微信图片_20190808133201

"IR projector" 发出红外光(红外光实际上就是 一面红外点集 ),"IR sensor" 接收红外光,然后计算 发出 和 接收 的时间差,就可以得到每束红外光点投射到的空间点到Kinect的距离。

所以用这个深度摄像头,我们可以测量该摄像头可视范围内的空间点到Kinect的距离。所以Depth图的每个像素表示的是“该像素对应的空间三维点与该Kinect之间的距离”。

然后根据距离值,分配不同的颜色。就形成了彩色的 Depth 图。其中“绿色”表示“近”,“红色”表示“远”。

右上角黑白图:Kinect的 IR(红外光) 图。由于原始的 IR 图的每个像素表示的是 “该像素对应的红外点投射到的空间点的亮度值”。

下面彩色图:Kinect的 RGB 图。Kinect上有一个高清的摄像头,这个 RGB 图就是该摄像头获取的数据。

微信图片_20190808133212

然后,我们使用 PCL 接收Kinect的 RGB+Depth 图,输出实时的彩色点云(三维点集合)。

微信图片_20190808133217

其中,RGB 图提供颜色信息,Depth 图提供深度信息。Depth 图上的每一个像素都对应 RGB 图上的一个像素。

每个 Depth 图的像素有一个值——深度值,该像素对应的 RGB 图上的那个像素可以提供三个值——Red + Green + Blue值,表示彩色信息。

微信图片_20190808133220

使用 PCL 来读取Kinect数据,需要继承 PCL 的 grabber 类,写一个自己的Kinect Grabber 类。如PCL中已经提供了 Real Sense 的 grabber,openni 的 grabber,openni2 的 grabber。新一代Kinect还没有被提供,我就自己写了一个 grabber。
然后配合 PCL 的 Visualization 模块来可视化实时点云。

  1. #pragma once
  2. #include <k4a/k4a.hpp>
  3. #include <pcl/io/boost.h>
  4. #include <pcl/io/grabber.h>
  5. #include <pcl/point_cloud.h>
  6. #include <pcl/point_types.h>
  7. namespace pcl
  8. {
  9. struct pcl::PointXYZ;
  10. struct pcl::PointXYZI;
  11. struct pcl::PointXYZRGB;
  12. struct pcl::PointXYZRGBA;
  13. template <typename T> class pcl::PointCloud;
  14. class KinectAzureDKGrabber : public pcl::Grabber
  15. {
  16. public:
  17. KinectAzureDKGrabber(const int &device_id_, const int &depth_mode_, const int &color_format_, const int &color_resolution_);
  18. virtual ~KinectAzureDKGrabber() throw ();
  19. virtual void start();
  20. virtual void stop();
  21. virtual bool isRunning() const;
  22. virtual std::string getName() const;
  23. virtual float getFramesPerSecond() const;
  24. typedef void (signal_KinectAzureDK_PointXYZ)(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>&);
  25. typedef void (signal_KinectAzureDK_PointXYZI)(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI>>&);
  26. typedef void (signal_KinectAzureDK_PointXYZRGB)(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGB>>&);
  27. typedef void (signal_KinectAzureDK_PointXYZRGBA)(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA>>&);
  28. protected:
  29. void setupDevice(const int &device_id_, const int &depth_mode_, const int &color_format_, const int &color_resolution_);
  30. boost::signals2::signal<signal_KinectAzureDK_PointXYZ>* signal_PointXYZ;
  31. boost::signals2::signal<signal_KinectAzureDK_PointXYZI>* signal_PointXYZI;
  32. boost::signals2::signal<signal_KinectAzureDK_PointXYZRGB>* signal_PointXYZRGB;
  33. boost::signals2::signal<signal_KinectAzureDK_PointXYZRGBA>* signal_PointXYZRGBA;
  34. pcl::PointCloud<pcl::PointXYZ>::Ptr convertDepthToPointXYZ();
  35. pcl::PointCloud<pcl::PointXYZI>::Ptr convertInfraredDepthToPointXYZI();
  36. pcl::PointCloud<pcl::PointXYZRGB>::Ptr convertRGBDepthToPointXYZRGB();
  37. pcl::PointCloud<pcl::PointXYZRGBA>::Ptr convertRGBADepthToPointXYZRGBA();
  38. boost::thread thread;
  39. mutable boost::mutex mutex;
  40. void threadFunction();
  41. bool quit;
  42. bool running;
  43. k4a_device_configuration_t config;
  44. k4a::device dev;
  45. int device_id;
  46. k4a::calibration calibration;
  47. k4a::transformation transformation;
  48. int colorWidth;
  49. int colorHeight;
  50. k4a::image colorImage;
  51. int depthWidth;
  52. int depthHeight;
  53. k4a::image depthImage;
  54. int infraredWidth;
  55. int infraredHeight;
  56. k4a::image infraredImage;
  57. };
  58. pcl::KinectAzureDKGrabber::KinectAzureDKGrabber(const int &device_id_, const int &depth_mode_, const int &color_format_, const int &color_resolution_) :
  59. config(K4A_DEVICE_CONFIG_INIT_DISABLE_ALL),
  60. dev(nullptr),
  61. colorImage(nullptr),
  62. depthImage(nullptr),
  63. infraredImage(nullptr),
  64. running(false),
  65. quit(false),
  66. signal_PointXYZ(nullptr),
  67. signal_PointXYZI(nullptr),
  68. signal_PointXYZRGB(nullptr),
  69. signal_PointXYZRGBA(nullptr)
  70. {
  71. setupDevice(device_id_, depth_mode_, color_format_, color_resolution_);
  72. signal_PointXYZ = createSignal<signal_KinectAzureDK_PointXYZ>();
  73. signal_PointXYZI = createSignal<signal_KinectAzureDK_PointXYZI>();
  74. signal_PointXYZRGB = createSignal<signal_KinectAzureDK_PointXYZRGB>();
  75. signal_PointXYZRGBA = createSignal<signal_KinectAzureDK_PointXYZRGBA>();
  76. }
  77. pcl::KinectAzureDKGrabber::~KinectAzureDKGrabber() throw()
  78. {
  79. stop();
  80. disconnect_all_slots<signal_KinectAzureDK_PointXYZ>();
  81. disconnect_all_slots<signal_KinectAzureDK_PointXYZI>();
  82. disconnect_all_slots<signal_KinectAzureDK_PointXYZRGB>();
  83. disconnect_all_slots<signal_KinectAzureDK_PointXYZRGBA>();
  84. thread.join();
  85. if (dev)
  86. {
  87. transformation.destroy();
  88. dev.close();
  89. }
  90. }
  91. void pcl::KinectAzureDKGrabber::start()
  92. {
  93. dev = k4a::device::open(device_id);
  94. dev.start_cameras(&config);
  95. calibration = dev.get_calibration(config.depth_mode, config.color_resolution);
  96. transformation = k4a::transformation(calibration);
  97. running = true;
  98. thread = boost::thread(&KinectAzureDKGrabber::threadFunction, this);
  99. }
  100. void pcl::KinectAzureDKGrabber::stop()
  101. {
  102. boost::unique_lock<boost::mutex> lock(mutex);
  103. quit = true;
  104. running = false;
  105. lock.unlock();
  106. }
  107. bool pcl::KinectAzureDKGrabber::isRunning() const
  108. {
  109. boost::unique_lock<boost::mutex> lock(mutex);
  110. return running;
  111. lock.unlock();
  112. }
  113. std::string pcl::KinectAzureDKGrabber::getName() const
  114. {
  115. return std::string("KinectAzureDKGrabber");
  116. }
  117. float pcl::KinectAzureDKGrabber::getFramesPerSecond() const
  118. {
  119. return config.camera_fps;
  120. }
  121. void pcl::KinectAzureDKGrabber::setupDevice(const int &device_id_, const int &depth_mode_, const int &color_format_, const int &color_resolution_)
  122. {
  123. device_id = device_id_;
  124. config.camera_fps = K4A_FRAMES_PER_SECOND_30;
  125. config.depth_mode = k4a_depth_mode_t(depth_mode_);//K4A_DEPTH_MODE_NFOV_UNBINNED
  126. config.color_format = k4a_image_format_t(color_format_);// K4A_IMAGE_FORMAT_COLOR_BGRA32
  127. config.color_resolution = k4a_color_resolution_t(color_resolution_);// K4A_COLOR_RESOLUTION_720P
  128. config.synchronized_images_only = true; // ensures that depth and color images are both available in the capture
  129. }
  130. void pcl::KinectAzureDKGrabber::threadFunction()
  131. {
  132. while (!quit)
  133. {
  134. boost::unique_lock<boost::mutex> lock(mutex);
  135. k4a::capture capture;
  136. if (!dev.get_capture(&capture, std::chrono::milliseconds(0)))
  137. {
  138. continue;
  139. }
  140. depthImage = capture.get_depth_image();
  141. if (depthImage == nullptr)
  142. {
  143. throw std::exception("Failed to get depth image from capture\n");
  144. }
  145. colorImage = capture.get_color_image();
  146. if (colorImage == nullptr)
  147. {
  148. throw std::exception("Failed to get color image from capture\n");
  149. }
  150. infraredImage = capture.get_ir_image();
  151. if (infraredImage == nullptr)
  152. {
  153. throw std::exception("Failed to get IR image from capture\n");
  154. }
  155. lock.unlock();
  156. if (signal_PointXYZ->num_slots() > 0)
  157. {
  158. signal_PointXYZ->operator()(convertDepthToPointXYZ());
  159. }
  160. if (signal_PointXYZI->num_slots() > 0)
  161. {
  162. signal_PointXYZI->operator()(convertInfraredDepthToPointXYZI());
  163. }
  164. if (signal_PointXYZRGB->num_slots() > 0)
  165. {
  166. signal_PointXYZRGB->operator()(convertRGBDepthToPointXYZRGB());
  167. }
  168. if (signal_PointXYZRGBA->num_slots() > 0)
  169. {
  170. signal_PointXYZRGBA->operator()(convertRGBADepthToPointXYZRGBA());
  171. }
  172. }
  173. }
  174. pcl::PointCloud<pcl::PointXYZ>::Ptr pcl::KinectAzureDKGrabber::convertDepthToPointXYZ(/*UINT16* depthBuffer*/)
  175. {
  176. PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());
  177. int depth_image_width_pixels = depthImage.get_width_pixels();
  178. int depth_image_height_pixels = depthImage.get_height_pixels();
  179. cloud->width = depth_image_width_pixels;
  180. cloud->height = depth_image_height_pixels;
  181. cloud->is_dense = false;
  182. cloud->points.resize(cloud->height * cloud->width);
  183. k4a::image transformed_color_image = NULL;
  184. transformed_color_image = k4a::image::create(K4A_IMAGE_FORMAT_COLOR_BGRA32,
  185. depth_image_width_pixels,
  186. depth_image_height_pixels,
  187. depth_image_width_pixels * 4 * (int)sizeof(uint8_t));
  188. k4a::image point_cloud_image = NULL;
  189. point_cloud_image = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
  190. depth_image_width_pixels,
  191. depth_image_height_pixels,
  192. depth_image_width_pixels * 3 * (int)sizeof(int16_t));
  193. transformation.color_image_to_depth_camera(depthImage, colorImage, &transformed_color_image);
  194. transformation.depth_image_to_point_cloud(depthImage, K4A_CALIBRATION_TYPE_DEPTH, &point_cloud_image);
  195. int width = point_cloud_image.get_width_pixels();
  196. int height = point_cloud_image.get_height_pixels();
  197. int16_t *point_cloud_image_data = (int16_t *)(void *)point_cloud_image.get_buffer();
  198. for (int i = 0; i < width * height; ++i)
  199. {
  200. PointXYZ point;
  201. point.x = point_cloud_image_data[3 * i + 0];
  202. point.y = point_cloud_image_data[3 * i + 1];
  203. point.z = point_cloud_image_data[3 * i + 2];
  204. cloud->points[i] = point;
  205. }
  206. return cloud;
  207. }
  208. pcl::PointCloud<pcl::PointXYZI>::Ptr pcl::KinectAzureDKGrabber::convertInfraredDepthToPointXYZI(/*UINT16* infraredBuffer, UINT16* depthBuffer*/)
  209. {
  210. PointCloud<PointXYZI>::Ptr cloud(new PointCloud<PointXYZI>());
  211. int depth_image_width_pixels = depthImage.get_width_pixels();
  212. int depth_image_height_pixels = depthImage.get_height_pixels();
  213. cloud->width = depth_image_width_pixels;
  214. cloud->height = depth_image_height_pixels;
  215. cloud->is_dense = false;
  216. cloud->points.resize(cloud->height * cloud->width);
  217. k4a::image transformed_color_image = NULL;
  218. transformed_color_image = k4a::image::create(K4A_IMAGE_FORMAT_COLOR_BGRA32,
  219. depth_image_width_pixels,
  220. depth_image_height_pixels,
  221. depth_image_width_pixels * 4 * (int)sizeof(uint8_t));
  222. k4a::image point_cloud_image = NULL;
  223. point_cloud_image = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
  224. depth_image_width_pixels,
  225. depth_image_height_pixels,
  226. depth_image_width_pixels * 3 * (int)sizeof(int16_t));
  227. transformation.color_image_to_depth_camera(depthImage, colorImage, &transformed_color_image);
  228. transformation.depth_image_to_point_cloud(depthImage, K4A_CALIBRATION_TYPE_DEPTH, &point_cloud_image);
  229. int width = point_cloud_image.get_width_pixels();
  230. int height = point_cloud_image.get_height_pixels();
  231. int16_t *point_cloud_image_data = (int16_t *)(void *)point_cloud_image.get_buffer();
  232. float *ir_image_data = reinterpret_cast<float *>(infraredImage.get_buffer());
  233. for (int i = 0; i < width * height; ++i)
  234. {
  235. PointXYZI point;
  236. point.x = point_cloud_image_data[3 * i + 0];
  237. point.y = point_cloud_image_data[3 * i + 1];
  238. point.z = point_cloud_image_data[3 * i + 2];
  239. point.intensity = ir_image_data[i];
  240. cloud->points[i] = point;
  241. }
  242. return cloud;
  243. }
  244. pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl::KinectAzureDKGrabber::convertRGBDepthToPointXYZRGB(/*RGBQUAD* colorBuffer, UINT16* depthBuffer*/)
  245. {
  246. PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>());
  247. int depth_image_width_pixels = depthImage.get_width_pixels();
  248. int depth_image_height_pixels = depthImage.get_height_pixels();
  249. int color_image_width_pixels = colorImage.get_width_pixels();
  250. int color_image_height_pixels = colorImage.get_height_pixels();
  251. k4a::image transformed_depth_image = NULL;
  252. transformed_depth_image = k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16,
  253. color_image_width_pixels,
  254. color_image_height_pixels,
  255. color_image_width_pixels * (int)sizeof(uint16_t));
  256. k4a::image point_cloud_image = NULL;
  257. point_cloud_image = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
  258. color_image_width_pixels,
  259. color_image_height_pixels,
  260. color_image_width_pixels * 3 * (int)sizeof(int16_t));
  261. transformation.depth_image_to_color_camera(depthImage, &transformed_depth_image);
  262. transformation.depth_image_to_point_cloud(transformed_depth_image, K4A_CALIBRATION_TYPE_COLOR, &point_cloud_image);
  263. int width = point_cloud_image.get_width_pixels();
  264. int height = colorImage.get_height_pixels();
  265. cloud->width = width;
  266. cloud->height = height;
  267. cloud->is_dense = false;
  268. cloud->points.resize(cloud->height * cloud->width);
  269. int16_t *point_cloud_image_data = (int16_t *)(void *)point_cloud_image.get_buffer();
  270. uint8_t *color_image_data = colorImage.get_buffer();
  271. for (int i = 0; i < width * height; ++i)
  272. {
  273. PointXYZRGB point;
  274. point.x = point_cloud_image_data[3 * i + 0];
  275. point.y = point_cloud_image_data[3 * i + 1];
  276. point.z = point_cloud_image_data[3 * i + 2];
  277. point.b = color_image_data[4 * i + 0];
  278. point.g = color_image_data[4 * i + 1];
  279. point.r = color_image_data[4 * i + 2];
  280. uint8_t alpha = color_image_data[4 * i + 3];
  281. cloud->points[i] = point;
  282. }
  283. return cloud;
  284. }
  285. pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcl::KinectAzureDKGrabber::convertRGBADepthToPointXYZRGBA(/*RGBQUAD* colorBuffer, UINT16* depthBuffer*/)
  286. {
  287. PointCloud<PointXYZRGBA>::Ptr cloud(new PointCloud<PointXYZRGBA>());
  288. int depth_image_width_pixels = depthImage.get_width_pixels();
  289. int depth_image_height_pixels = depthImage.get_height_pixels();
  290. int color_image_width_pixels = colorImage.get_width_pixels();
  291. int color_image_height_pixels = colorImage.get_height_pixels();
  292. k4a::image transformed_depth_image = NULL;
  293. transformed_depth_image = k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16,
  294. color_image_width_pixels,
  295. color_image_height_pixels,
  296. color_image_width_pixels * (int)sizeof(uint16_t));
  297. k4a::image point_cloud_image = NULL;
  298. point_cloud_image = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
  299. color_image_width_pixels,
  300. color_image_height_pixels,
  301. color_image_width_pixels * 3 * (int)sizeof(int16_t));
  302. transformation.depth_image_to_color_camera(depthImage, &transformed_depth_image);
  303. transformation.depth_image_to_point_cloud(transformed_depth_image, K4A_CALIBRATION_TYPE_COLOR, &point_cloud_image);
  304. int width = point_cloud_image.get_width_pixels();
  305. int height = colorImage.get_height_pixels();
  306. cloud->width = width;
  307. cloud->height = height;
  308. cloud->is_dense = false;
  309. cloud->points.resize(cloud->height * cloud->width);
  310. int16_t *point_cloud_image_data = (int16_t *)(void *)point_cloud_image.get_buffer();
  311. uint8_t *color_image_data = colorImage.get_buffer();
  312. for (int i = 0; i < width * height; ++i)
  313. {
  314. PointXYZRGBA point;
  315. point.x = point_cloud_image_data[3 * i + 0];
  316. point.y = point_cloud_image_data[3 * i + 1];
  317. point.z = point_cloud_image_data[3 * i + 2];
  318. point.b = color_image_data[4 * i + 0];
  319. point.g = color_image_data[4 * i + 1];
  320. point.r = color_image_data[4 * i + 2];
  321. point.a = color_image_data[4 * i + 3];
  322. cloud->points[i] = point;
  323. }
  324. return cloud;
  325. }
  326. }

最关键的代码是几个convert函数,输入 RGB + Depth 图,输出点云信息。

微信图片_20190808133230

在这篇文章中,我将不详细讲解上述所有代码。后面几篇文章,我将详细讲解所有代码,并将代码开源于 Coding 和 Github 上。敬请关注。

更多精彩内容,关注青亭网微信号(ID:qingtinwang),或者来微博@青亭网与我们互动!转载请注明版权和原文链接!
青亭网

微信扫码关注青亭网

青亭网

青亭 | 前沿科技交流群01

责任编辑:hi188
分享到QQ 分享到微信
切换注册

登录

忘记密码 ?

您也可以使用第三方帐号快捷登录

Q Q 登 录
微 博 登 录
切换登录

注册