标签：
实战系列很久没有更新了。近期拿到了一台不错的Thinkpad和Kinect v2，前两天orbslam2又放出，于是想要在kinect2下尝试一下orb slam。整个过程没有特别多的技术含量，读者可以把它当成一个实验步骤的总结。
orb-slam是15年出的一个单目SLAM，也可以说是单目中做的非常好的一个实现。另一方面，他的代码也极其清爽，编译十分贴心，十分注重我等程序员的用户体验，受到了广大欢迎。前几天，orb-slam作者推出了orb-slam2，在原来的单目基础上增加了双目和RGBD的接口，尽管地图还是单目常见的稀疏特征点图。于是我们就能通过各种传感器来玩orb-slam啦！这里正巧我手上有一个Kinectv2，咱们就拿它做个实验吧！
博主的系统就不多说了，ubuntu14.04, Thinkpad T450。Kinect2 for xbox.
要在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的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的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的标定，否则很可能出错。
视觉SLAM实战（二）：ORB-SLAM2 with Kinect2
标签：
原文地址：http://www.cnblogs.com/gaoxiang12/p/5161223.html
