内容字号:默认大号超大号

段落设置:段首缩进取消段首缩进

字体设置:切换到微软雅黑切换到宋体

视觉SLAM实战(二):ORB-SLAM2 with Kinect2

2016-02-03 17:31 出处:清屏网 人气: 评论(0

前言

实战系列很久没有更新了。近期拿到了一台不错的Thinkpad和Kinect v2,前两天orbslam2又放出,于是想要在kinect2下尝试一下orb slam。整个过程没有特别多的技术含量,读者可以把它当成一个实验步骤的总结。

ORB-slam

orb-slam是15年出的一个单目SLAM,也可以说是单目中做的非常好的一个实现。另一方面,他的代码也极其清爽,编译十分贴心,十分注重我等程序员的用户体验,受到了广大欢迎。前几天,orb-slam作者推出了orb-slam2,在原来的单目基础上增加了双目和RGBD的接口,尽管地图还是单目常见的稀疏特征点图。于是我们就能通过各种传感器来玩orb-slam啦!这里正巧我手上有一个Kinectv2,咱们就拿它做个实验吧!

博主的系统就不多说了,ubuntu14.04, Thinkpad T450。Kinect2 for xbox.

编译Kinect v2

要在ubuntu下使用Kinect V2,需要做两件事。一是编译Kinectv2的开源驱动libfreenect2,二是使用kinect2_bridge在ROS下采集它的图像。这两者在hitcm的博客中已经说的很清楚了,咱就不罗嗦了。

请参照hitcm的博客安装好libfreenect2和iai_kinect2系列软件:

http://www.cnblogs.com/hitcm/p/5118196.html

iai_kinect2中含有四个模块。我们主要用它的bridge进行图像间的转换。此外,还要使用kinect2_registration进行标定。没标定过的kinect2,深度图和彩色图之间是不保证一一对应的,在做slam时就会出错。所以请务必做好它的标定。好在作者十分良心地写了标定的详细过程,即使像博主这样的小白也能顺利完成标定啦!

kinect2_calibration模块,有详细的标定过程解释:

https://github.com/code-iai/iai_kinect2/tree/master/kinect2_calibration

标定之后呢,会得到kinect2彩色头、深度头、红外头的内参和外参,它们都以(万恶的)yaml模式存储在你的机器内。kinect2_bridge会自动检测你的标定文件,对深度图进行校正。之后slam过程主要使用彩色头的内参哦!同时,你也可以使用kinect2_viewer模块来看获取的点云和kinect2的图像哦!

编译orb-slam2

orb-slam2的github位于:https://github.com/raulmur/ORB_SLAM2 直接clone到本地即可。

这版的orb-slam可以脱离ros编译,不需要事先安装ros。(但是由于我们要用kinect2还是装了ros)它的主要依赖项是opencv2.4, eigen3, dbow2和g2o,另外还有一些我没怎么听说过的UI库:pangolin。其中Dbow2和g2o已经包含在它的Thirdparty文件夹中,不需要另外下载啦!opencv的安装方式参见一起做第一篇。(g2o版本问题一直是个坑) 所以,只需要去下载pangolin即可。

pangolin的github: https://github.com/stevenlovegrove/Pangolin 它是一个cmake工程,没什么特别的依赖项,直接下载编译安装即可。

随后,进入orb-slam2的文件夹。作者很贴心的为我们准备了 build.sh 文件,直接运行这个文件即可完成编译。

但愿你也顺利编译成功了。orb-slam作者为我们提供了几个example,包括kitti的双目和tum的单目/rgbd。我们可以参照着它去写自己的输入。如果你只想把orb-slam2作为一个整体的模块,可以直接调用include/System.h文件里定义的SLAM System哦。现在我们就把Kinect2丢给它试试。

在Kinect2上运行orb-slam2

Kinect2的topic一共有三种,含不同的分辨率。其中hd是1920的,qhd是四分之一的960的,而sd是最小的。博主发现sd的效果不理想,而hd的图像又太大了,建议大家使用qhd的920大小啦!在调用orb-slam时,需要把相机参数通过一个yaml来告诉它,所以需要简单写一下你的kinect参数喽。比如像这样:

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 529.97
Camera.fy: 526.97
Camera.cx: 477.44
Camera.cy: 261.87

Camera.k1: 0.05627
Camera.k2: -0.0742
Camera.p1: 0.00142
Camera.p2: -0.00169
Camera.k3: 0.0241

Camera.width: 960
Camera.height: 540

# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
Camera.bf: 40.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
ThDepth: 50.0

# Deptmap values factor 
DepthMapFactor: 1000.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

ORB部分的参数我们就不用动啦。然后,对kinect2_viewer进行一定程度的改写,加入ORBSLAM,就可以跑起来喽:

  1 #include <stdlib.h>
  2 #include <stdio.h>
  3 #include <iostream>
  4 #include <sstream>
  5 #include <string>
  6 #include <vector>
  7 #include <cmath>
  8 #include <mutex>
  9 #include <thread>
 10 #include <chrono>
 11 
 12 #include <ros/ros.h>
 13 #include <ros/spinner.h>
 14 #include <sensor_msgs/CameraInfo.h>
 15 #include <sensor_msgs/Image.h>
 16 
 17 #include <cv_bridge/cv_bridge.h>
 18 
 19 #include <image_transport/image_transport.h>
 20 #include <image_transport/subscriber_filter.h>
 21 
 22 #include <message_filters/subscriber.h>
 23 #include <message_filters/synchronizer.h>
 24 #include <message_filters/sync_policies/exact_time.h>
 25 #include <message_filters/sync_policies/approximate_time.h>
 26 
 27 #include <kinect2_bridge/kinect2_definitions.h>
 28 
 29 #include "orbslam2/System.h"
 30 
 31 class Receiver
 32 {
 33 public:
 34   enum Mode
 35   {
 36     IMAGE = 0,
 37     CLOUD,
 38     BOTH
 39   };
 40 
 41 private:
 42   std::mutex lock;
 43 
 44   const std::string topicColor, topicDepth;
 45   const bool useExact, useCompressed;
 46 
 47   bool updateImage, updateCloud;
 48   bool save;
 49   bool running;
 50   size_t frame;
 51   const size_t queueSize;
 52 
 53   cv::Mat color, depth;
 54   cv::Mat cameraMatrixColor, cameraMatrixDepth;
 55   cv::Mat lookupX, lookupY;
 56 
 57   typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ExactSyncPolicy;
 58   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximateSyncPolicy;
 59 
 60   ros::NodeHandle nh;
 61   ros::AsyncSpinner spinner;
 62   image_transport::ImageTransport it;
 63   image_transport::SubscriberFilter *subImageColor, *subImageDepth;
 64   message_filters::Subscriber<sensor_msgs::CameraInfo> *subCameraInfoColor, *subCameraInfoDepth;
 65 
 66   message_filters::Synchronizer<ExactSyncPolicy> *syncExact;
 67   message_filters::Synchronizer<ApproximateSyncPolicy> *syncApproximate;
 68 
 69   std::thread imageViewerThread;
 70   Mode mode;
 71 
 72   std::ostringstream oss;
 73   std::vector<int> params;
 74 
 75   //RGBDSLAM  slam; //the slam object
 76   ORB_SLAM2::System* orbslam    =nullptr;
 77 
 78 public:
 79   Receiver(const std::string &topicColor, const std::string &topicDepth, const bool useExact, const bool useCompressed)
 80     : topicColor(topicColor), topicDepth(topicDepth), useExact(useExact), useCompressed(useCompressed),
 81       updateImage(false), updateCloud(false), save(false), running(false), frame(0), queueSize(5),
 82       nh("~"), spinner(0), it(nh), mode(CLOUD)
 83   {
 84     cameraMatrixColor = cv::Mat::zeros(3, 3, CV_64F);
 85     cameraMatrixDepth = cv::Mat::zeros(3, 3, CV_64F);
 86     params.push_back(cv::IMWRITE_JPEG_QUALITY);
 87     params.push_back(100);
 88     params.push_back(cv::IMWRITE_PNG_COMPRESSION);
 89     params.push_back(1);
 90     params.push_back(cv::IMWRITE_PNG_STRATEGY);
 91     params.push_back(cv::IMWRITE_PNG_STRATEGY_RLE);
 92     params.push_back(0);
 93 
 94     string orbVocFile = "/home/xiang/catkin_ws/src/walle/config/ORBvoc.txt";
 95     string orbSetiingsFile = "/home/xiang/catkin_ws/src/walle/config/kinect2_sd.yaml";
 96 
 97     orbslam = new ORB_SLAM2::System( orbVocFile, orbSetiingsFile ,ORB_SLAM2::System::RGBD, true );
 98   }
 99 
100   ~Receiver()
101   {
102       if (orbslam)
103       {
104           orbslam->Shutdown();
105           delete orbslam;
106       }
107   }
108 
109   void run(const Mode mode)
110   {
111     start(mode);
112     stop();
113   }
114 
115   void finish() 
116   {
117   }
118 private:
119   void start(const Mode mode)
120   {
121     this->mode = mode;
122     running = true;
123 
124     std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info";
125     std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info";
126 
127     image_transport::TransportHints hints(useCompressed ? "compressed" : "raw");
128     subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints);
129     subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints);
130     subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize);
131     subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize);
132 
133     if(useExact)
134     {
135       syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
136       syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
137     }
138     else
139     {
140       syncApproximate = new message_filters::Synchronizer<ApproximateSyncPolicy>(ApproximateSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
141       syncApproximate->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
142     }
143 
144     spinner.start();
145 
146     std::chrono::milliseconds duration(1);
147     while(!updateImage || !updateCloud)
148     {
149       if(!ros::ok())
150       {
151         return;
152       }
153       std::this_thread::sleep_for(duration);
154     }
155     createLookup(this->color.cols, this->color.rows);
156 
157     switch(mode)
158     {
159     case IMAGE:
160       imageViewer();
161       break;
162     case BOTH:
163       imageViewerThread = std::thread(&Receiver::imageViewer, this);
164       break;
165     }
166   }
167 
168   void stop()
169   {
170     spinner.stop();
171 
172     if(useExact)
173     {
174       delete syncExact;
175     }
176     else
177     {
178       delete syncApproximate;
179     }
180 
181     delete subImageColor;
182     delete subImageDepth;
183     delete subCameraInfoColor;
184     delete subCameraInfoDepth;
185 
186     running = false;
187     if(mode == BOTH)
188     {
189       imageViewerThread.join();
190     }
191   }
192 
193   void callback(const sensor_msgs::Image::ConstPtr imageColor, const sensor_msgs::Image::ConstPtr imageDepth,
194                 const sensor_msgs::CameraInfo::ConstPtr cameraInfoColor, const sensor_msgs::CameraInfo::ConstPtr cameraInfoDepth)
195   {
196     cv::Mat color, depth;
197 
198     readCameraInfo(cameraInfoColor, cameraMatrixColor);
199     readCameraInfo(cameraInfoDepth, cameraMatrixDepth);
200     readImage(imageColor, color);
201     readImage(imageDepth, depth);
202 
203     // IR image input
204     if(color.type() == CV_16U)
205     {
206       cv::Mat tmp;
207       color.convertTo(tmp, CV_8U, 0.02);
208       cv::cvtColor(tmp, color, CV_GRAY2BGR);
209     }
210 
211     lock.lock();
212     this->color = color;
213     this->depth = depth;
214     updateImage = true;
215     updateCloud = true;
216     lock.unlock();
217   }
218 
219   void imageViewer()
220   {
221     cv::Mat color, depth;
222     for(; running && ros::ok();)
223     {
224       if(updateImage)
225       {
226         lock.lock();
227         color = this->color;
228         depth = this->depth;
229         updateImage = false;
230         lock.unlock();
231 
232         if (orbslam)
233         {
234             orbslam->TrackRGBD( color, depth, ros::Time::now().toSec() );
235         }
236       }
237 
238     }
239 
240     cv::destroyAllWindows();
241     cv::waitKey(100);
242   }
243 
244   void readImage(const sensor_msgs::Image::ConstPtr msgImage, cv::Mat &image) const
245   {
246     cv_bridge::CvImageConstPtr pCvImage;
247     pCvImage = cv_bridge::toCvShare(msgImage, msgImage->encoding);
248     pCvImage->image.copyTo(image);
249   }
250 
251   void readCameraInfo(const sensor_msgs::CameraInfo::ConstPtr cameraInfo, cv::Mat &cameraMatrix) const
252   {
253     double *itC = cameraMatrix.ptr<double>(0, 0);
254     for(size_t i = 0; i < 9; ++i, ++itC)
255     {
256       *itC = cameraInfo->K[i];
257     }
258   }
259 
260   void dispDepth(const cv::Mat &in, cv::Mat &out, const float maxValue)
261   {
262     cv::Mat tmp = cv::Mat(in.rows, in.cols, CV_8U);
263     const uint32_t maxInt = 255;
264 
265     #pragma omp parallel for
266     for(int r = 0; r < in.rows; ++r)
267     {
268       const uint16_t *itI = in.ptr<uint16_t>(r);
269       uint8_t *itO = tmp.ptr<uint8_t>(r);
270 
271       for(int c = 0; c < in.cols; ++c, ++itI, ++itO)
272       {
273         *itO = (uint8_t)std::min((*itI * maxInt / maxValue), 255.0f);
274       }
275     }
276 
277     cv::applyColorMap(tmp, out, cv::COLORMAP_JET);
278   }
279 
280   void combine(const cv::Mat &inC, const cv::Mat &inD, cv::Mat &out)
281   {
282     out = cv::Mat(inC.rows, inC.cols, CV_8UC3);
283 
284     #pragma omp parallel for
285     for(int r = 0; r < inC.rows; ++r)
286     {
287       const cv::Vec3b
288       *itC = inC.ptr<cv::Vec3b>(r),
289        *itD = inD.ptr<cv::Vec3b>(r);
290       cv::Vec3b *itO = out.ptr<cv::Vec3b>(r);
291 
292       for(int c = 0; c < inC.cols; ++c, ++itC, ++itD, ++itO)
293       {
294         itO->val[0] = (itC->val[0] + itD->val[0]) >> 1;
295         itO->val[1] = (itC->val[1] + itD->val[1]) >> 1;
296         itO->val[2] = (itC->val[2] + itD->val[2]) >> 1;
297       }
298     }
299   }
300 
301 
302   void createLookup(size_t width, size_t height)
303   {
304     const float fx = 1.0f / cameraMatrixColor.at<double>(0, 0);
305     const float fy = 1.0f / cameraMatrixColor.at<double>(1, 1);
306     const float cx = cameraMatrixColor.at<double>(0, 2);
307     const float cy = cameraMatrixColor.at<double>(1, 2);
308     float *it;
309 
310     lookupY = cv::Mat(1, height, CV_32F);
311     it = lookupY.ptr<float>();
312     for(size_t r = 0; r < height; ++r, ++it)
313     {
314       *it = (r - cy) * fy;
315     }
316 
317     lookupX = cv::Mat(1, width, CV_32F);
318     it = lookupX.ptr<float>();
319     for(size_t c = 0; c < width; ++c, ++it)
320     {
321       *it = (c - cx) * fx;
322     }
323   }
324 };
325 
326 int main(int argc, char **argv)
327 {
328   ros::init(argc, argv, "kinect2_slam", ros::init_options::AnonymousName);
329 
330   if(!ros::ok())
331   {
332     return 0;
333   }
334   std::string topicColor = "/kinect2/sd/image_color_rect";
335   std::string topicDepth = "/kinect2/sd/image_depth_rect";
336   bool useExact = true;
337   bool useCompressed = false;
338   Receiver::Mode mode = Receiver::IMAGE;
339   // 初始化receiver
340   Receiver receiver(topicColor, topicDepth, useExact, useCompressed);
341 
342   //OUT_INFO("starting receiver...");
343   receiver.run(mode);
344 
345   receiver.finish();
346 
347   ros::shutdown();
348 
349   return 0;
350 }

编译方面,只要在CMakeLists.txt中加入orb-slam的头文件和库,告诉cmake你想链接它即可。甚至你可以把整个orb-slam放到你的代码目录中一块儿编译,不过我还是简单地把liborb_slam2.so文件和头文件拷了过来而已。

实际的手持kinect2运行效果(由于博客园无法传视频,暂时把百度云当播放器使一使):http://pan.baidu.com/s/1eRcyW1s (感谢也冬同学友情出演……)

一起做rgbd slam的数据集上效果:http://pan.baidu.com/s/1bocx5s

大体上还是挺理想的。

小结

本文主要展现了orbslam2在Kinect2下的表现,大致是令人满意的。读者在使用时,请务必注意kinect2的标定,否则很可能出错。


分享给小伙伴们:
本文标签: 视觉SLAMORB-SLAM2Kinect2

相关文章

发表评论愿您的每句评论,都能给大家的生活添色彩,带来共鸣,带来思索,带来快乐。

CopyRight © 2015-2016 QingPingShan.com , All Rights Reserved.

清屏网 版权所有 豫ICP备15026204号