視覺SLAM實戰（二）：ORB-SLAM2 with Kinect2

時間 2019-12-06

標籤視覺 slam 實戰 orb slam2 kinect2 kinect 欄目快樂工作简体版

原文原文鏈接

前言

　　實戰系列好久沒有更新了。近期拿到了一臺不錯的Thinkpad和Kinect v2，前兩天orbslam2又放出，因而想要在kinect2下嘗試一下orb slam。整個過程沒有特別多的技術含量，讀者能夠把它當成一個實驗步驟的總結。html

ORB-slam

　　orb-slam是15年出的一個單目SLAM，也能夠說是單目中作的很是好的一個實現。另外一方面，他的代碼也極其清爽，編譯十分貼心，十分注重我等程序員的用戶體驗，受到了廣大歡迎。前幾天，orb-slam做者推出了orb-slam2，在原來的單目基礎上增長了雙目和RGBD的接口，儘管地圖仍是單目常見的稀疏特徵點圖。因而咱們就能經過各類傳感器來玩orb-slam啦！這裏正巧我手上有一個Kinectv2，我們就拿它作個實驗吧！ios

　　博主的系統就很少說了，ubuntu14.04, Thinkpad T450。Kinect2 for xbox.git

編譯Kinect v2

　　要在ubuntu下使用Kinect V2，須要作兩件事。一是編譯Kinectv2的開源驅動libfreenect2，二是使用kinect2_bridge在ROS下采集它的圖像。這二者在hitcm的博客中已經說的很清楚了，咱就不羅嗦了。程序員

　　請參照hitcm的博客安裝好libfreenect2和iai_kinect2系列軟件：github

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

　　iai_kinect2中含有四個模塊。咱們主要用它的bridge進行圖像間的轉換。此外，還要使用kinect2_registration進行標定。沒標定過的kinect2，深度圖和彩色圖之間是不保證一一對應的，在作slam時就會出錯。因此請務必作好它的標定。好在做者十分良心地寫了標定的詳細過程，即便像博主這樣的小白也能順利完成標定啦！app

　　kinect2_calibration模塊，有詳細的標定過程解釋：ide

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

　　標定以後呢，會獲得kinect2彩色頭、深度頭、紅外頭的內參和外參，它們都以（萬惡的）yaml模式存儲在你的機器內。kinect2_bridge會自動檢測你的標定文件，對深度圖進行校訂。以後slam過程主要使用彩色頭的內參哦！同時，你也可使用kinect2_viewer模塊來看獲取的點雲和kinect2的圖像哦！this

編譯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 }