您的位置: 首页  >  文章  >  关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)

关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)

分类: 文章 • 2024-10-31 22:03:52

原文地址http://blog.****.net/aptx704610875/article/details/51490201    支持原创，感谢半闲居士和易科实验室大牛


最近写完了windows上的实时rgbd_slam后，读了些论文，想着怎么改进程序，想在闭环检测的方面尝试一下。最近很火的ORB_SLAM2使用了DBoW2（ORB词袋）的方法，极大的提高了速度和匹配准确度，windows版的orb_slam2还没跑成功（一部分库的编译出现了问题，不过等研究做完了，会继续跑windows版本的），这几天一直在尝试ubuntu版的orb_slam的实时重建，今天终于成功了！～（感谢高博士为我们提供了加了3D建图模块的libORB_SLAM2.so（高博的博客：半闲居士））

首先orb_slam2的话，github下载源码编译很容易，按照官方github下面的教程走就行。晒几张TUM数据集的结果：

desk：

关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)

room：

关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)

效果很棒，模拟轨迹和groundtruth的绝对误差真的和论文上说的一样小。我觉得ORB_SLAM2真的是现在视觉SLAM里最优秀的一版，考虑的非常全面。

那么如果我们要用到自己的项目中，该怎么调用呢？特别棒的一点是，原作者提供了libORB_SLAM2.so给我们，加上头文件System.h，我们就可以把ORB_SLAM作为一个整体加到我们的项目中。但是源码中并没有3D建图的模块，需要做相应改变，高博士为我们提供了加了3D建图模块的libORB_SLAM2.so，这时我们就可以根据自己的需求（kinect，xtion或其他可以获得点云的sensors）。高博的博客中有一篇是用的kinect2，在ROS运行的orb_slam2，今天我们来试一试不用ROS，通过OpenNI2直接调用xtion获取rgb数据和depth数据来重建环境（之前有windows上运行openni2_xtion的经验）。我的建议，要么xtion要么kinect2, 因为kinect很鸡肋，xtion比它轻巧，kinect2比它分辨率高。(源码下载****  源码下载github)（词袋文件太大，各位可以从官方github下载）

扯了这么多，现在拉回主线。在ORB_SLAM2/Examples/RGB-D/中，创建rgbd_xtion_cc.cpp：

[cpp] view plain copy
 print?
  1. #include <iostream>  
  2. #include <algorithm>  
  3. #include <fstream>  
  4. #include <chrono>  
  5. #include <OpenNI.h>  
  6. #include <opencv2/core/core.hpp>  
  7. #include <opencv2/highgui/highgui.hpp>  
  8. #include <opencv2/imgproc/imgproc.hpp>  
  9.   
  10. #include <System.h>   // orb_slam2  
  11.   
  12. using namespace std;  
  13. using namespace openni;  
  14. using namespace cv;  
  15.   
  16. void showdevice(){  
  17.     // 获取设备信息    
  18.     Array<DeviceInfo> aDeviceList;  
  19.     OpenNI::enumerateDevices(&aDeviceList);  
  20.   
  21.     cout << "电脑上连接着 " << aDeviceList.getSize() << " 个体感设备." << endl;  
  22.   
  23.     for (int i = 0; i < aDeviceList.getSize(); ++i)  
  24.     {  
  25.         cout << "设备 " << i << endl;  
  26.         const DeviceInfo& rDevInfo = aDeviceList[i];  
  27.         cout << "设备名： " << rDevInfo.getName() << endl;  
  28.         cout << "设备Id： " << rDevInfo.getUsbProductId() << endl;  
  29.         cout << "供应商名： " << rDevInfo.getVendor() << endl;  
  30.         cout << "供应商Id: " << rDevInfo.getUsbVendorId() << endl;  
  31.         cout << "设备URI: " << rDevInfo.getUri() << endl;  
  32.   
  33.     }  
  34. }  
  35.   
  36. Status initstream(Status& rc, Device& xtion, VideoStream& streamDepth, VideoStream& streamColor)  
  37. {  
  38.     rc = STATUS_OK;  
  39.   
  40.     // 创建深度数据流  
  41.     rc = streamDepth.create(xtion, SENSOR_DEPTH);  
  42.     if (rc == STATUS_OK)  
  43.     {  
  44.         // 设置深度图像视频模式  
  45.         VideoMode mModeDepth;  
  46.         // 分辨率大小  
  47.         mModeDepth.setResolution(640, 480);  
  48.         // 每秒30帧  
  49.         mModeDepth.setFps(30);  
  50.         // 像素格式  
  51.         mModeDepth.setPixelFormat(PIXEL_FORMAT_DEPTH_1_MM);  
  52.   
  53.         streamDepth.setVideoMode(mModeDepth);  
  54.         streamDepth.setMirroringEnabled(false);      //镜像  
  55.   
  56.         // 打开深度数据流  
  57.         rc = streamDepth.start();  
  58.         if (rc != STATUS_OK)  
  59.         {  
  60.             cerr << "无法打开深度数据流：" << OpenNI::getExtendedError() << endl;  
  61.             streamDepth.destroy();  
  62.         }  
  63.     }  
  64.     else  
  65.     {  
  66.         cerr << "无法创建深度数据流：" << OpenNI::getExtendedError() << endl;  
  67.     }  
  68.   
  69.     // 创建彩色图像数据流  
  70.     rc = streamColor.create(xtion, SENSOR_COLOR);  
  71.     if (rc == STATUS_OK)  
  72.     {  
  73.         // 同样的设置彩色图像视频模式  
  74.         VideoMode mModeColor;  
  75.         mModeColor.setResolution(640, 480);  
  76.         mModeColor.setFps(30);  
  77.         mModeColor.setPixelFormat(PIXEL_FORMAT_RGB888);  
  78.   
  79.         streamColor.setVideoMode(mModeColor);  
  80.         streamColor.setMirroringEnabled(false);   //镜像  
  81.         // 打开彩色图像数据流  
  82.         rc = streamColor.start();  
  83.         if (rc != STATUS_OK)  
  84.         {  
  85.             cerr << "无法打开彩色图像数据流：" << OpenNI::getExtendedError() << endl;  
  86.             streamColor.destroy();  
  87.         }  
  88.     }  
  89.     else  
  90.     {  
  91.         cerr << "无法创建彩色图像数据流：" << OpenNI::getExtendedError() << endl;  
  92.     }  
  93.   
  94.     if (!streamColor.isValid() || !streamDepth.isValid())  
  95.     {  
  96.         cerr << "彩色或深度数据流不合法" << endl;  
  97.         OpenNI::shutdown();  
  98.         rc = STATUS_ERROR;  
  99.         return rc;  
  100.     }  
  101.   
  102.     // 图像模式注册,彩色图与深度图对齐  
  103.     if (xtion.isImageRegistrationModeSupported(  
  104.         IMAGE_REGISTRATION_DEPTH_TO_COLOR))  
  105.     {  
  106.         xtion.setImageRegistrationMode(IMAGE_REGISTRATION_DEPTH_TO_COLOR);  
  107.     }  
  108.   
  109.     return rc;  
  110. }  
  111.   
  112. int main(int argc, char **argv)  
  113. {  
  114.     if(argc != 3)  
  115.     {  
  116.         cerr << endl << "Usage: ./rgbd_cc path_to_vocabulary path_to_settings" << endl;  
  117.         return 1;  
  118.     }  
  119.   
  120.     // 创建ORB_SLAM系统. (参数1：ORB词袋文件  参数2：xtion参数文件)  
  121.     ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);  
  122.   
  123.     cout << endl << "-------" << endl;  
  124.     cout << "Openning Xtion ..." << endl;  
  125.   
  126.     Status rc = STATUS_OK;  
  127.     // 初始化OpenNI环境  
  128.     OpenNI::initialize();  
  129.     showdevice();  
  130.     // 声明并打开Device设备。  
  131.     Device xtion;  
  132.     const char * deviceURL = openni::ANY_DEVICE;  //设备名  
  133.     rc = xtion.open(deviceURL);  
  134.       
  135.     VideoStream streamDepth;  
  136.     VideoStream streamColor;  
  137.     if(initstream(rc, xtion, streamDepth, streamColor) == STATUS_OK)     // 初始化数据流  
  138.         cout << "Open Xtion Successfully!"<<endl;  
  139.     else  
  140.     {  
  141.         cout << "Open Xtion Failed!"<<endl;  
  142.         return 0;  
  143.     }  
  144.   
  145.     // Main loop  
  146.     cv::Mat imRGB, imD;  
  147.     bool continueornot = true;  
  148.     // 循环读取数据流信息并保存在VideoFrameRef中  
  149.     VideoFrameRef  frameDepth;  
  150.     VideoFrameRef  frameColor;  
  151.     namedWindow("RGB Image", CV_WINDOW_AUTOSIZE);  
  152.     for (double index = 1.0; continueornot; index+=1.0)  
  153.     {  
  154.         rc = streamDepth.readFrame(&frameDepth);  
  155.         if (rc == STATUS_OK)  
  156.         {  
  157.             imD = cv::Mat(frameDepth.getHeight(), frameDepth.getWidth(), CV_16UC1, (void*)frameDepth.getData());   //获取深度图  
  158.         }  
  159.         rc = streamColor.readFrame(&frameColor);  
  160.         if (rc == STATUS_OK)  
  161.         {  
  162.             const Mat tImageRGB(frameColor.getHeight(), frameColor.getWidth(), CV_8UC3, (void*)frameColor.getData());   //获取彩色图  
  163.             cvtColor(tImageRGB, imRGB, CV_RGB2BGR);  
  164.             imshow("RGB Image",imRGB);  
  165.         }  
  166.         SLAM.TrackRGBD( imRGB, imD,  index);   // ORB_SLAM处理深度图和彩色图  
  167.         char c  = cv::waitKey(5);  
  168.         switch(c)  
  169.         {  
  170.         case 'q':  
  171.         case 27:         //退出  
  172.             continueornot = false;  
  173.             break;  
  174.         case 'p':         //暂停  
  175.             cv::waitKey(0);  
  176.             break;  
  177.         default:  
  178.             break;  
  179.         }  
  180.     }  
  181.     // Stop all threads  
  182.     SLAM.Shutdown();  
  183.     SLAM.SaveTrajectoryTUM("trajectory.txt");  
  184.     cv::destroyAllWindows();  
  185.     return 0;  
  186. }  
思路很简单，首先创建orb_slam系统，传入词袋和xtion/orb参数; 然后从xtion得到彩色图和深度图，调用slam的tracking线程处理得到位姿（当然也有loop线程的闭环检测和g2o下线程的优化），融合点云到同一个坐标下并显示（pointcloudmapping.h / cc里有声明和定义）。

然后在ORB_SLAM2/CMakeLists中添加：

[plain] view plain copy
 print?
  1. find_package(OpenNI2 REQUIRED)  
  2. include_directories("/usr/include/openni2/")  
  3. LINK_LIBRARIES( ${OpenNI2_LIBRARY} )  
  4. target_link_libraries(${PROJECT_NAME}${OpenNI2_LIBRARY})  
  5. add_executable(rgbd_xtion_cc  
  6. Examples/RGB-D/rgbd_xtion_cc.cpp)  
  7. target_link_libraries(rgbd_xtion_cc ${PROJECT_NAME})  

然后就可以在ORB_SLAM2/build/里cmake .. 和 make了。完成后可以看到ORB_SLAM2/Examples/RGB-D/里有可执行文件rgbd_xtion_cc。（rgbd_tum是跑TUM数据集的， rgbd_cc是跑自己的数据集的，这两个都是预先采集好彩色图和深度图）

最后在ORB_SLAM2/Examples/RGB-D/里创建xtion的参数文件xtion.yaml （包含了ORB参数信息），大家根据标定（OpenCV，ROS，MATLAB等）结果自行修改内参（rgb内参和畸变）：

[plain] view plain copy
 print?
  1. %YAML:1.0  
  2.   
  3. #--------------------------------------------------------------------------------------------  
  4. # Camera Parameters. xtion 640*480  
  5. #--------------------------------------------------------------------------------------------  
  6.   
  7. # Camera calibration and distortion parameters (OpenCV)   
  8. Camera.fx: 558.341390  
  9. Camera.fy: 558.387543  
  10. Camera.cx: 314.763671  
  11. Camera.cy: 240.992295  
  12.   
  13. Camera.k1: 0.062568  
  14. Camera.k2: -0.096148  
  15. Camera.p1: 0.000140  
  16. Camera.p2: -0.006248  
  17. Camera.k3: 0.000000  
  18.   
  19. Camera.width: 640  
  20. Camera.height: 480  
  21.   
  22. # Camera frames per second   
  23. Camera.fps: 30.0  
  24.   
  25. # IR projector baseline times fx (aprox.)  
  26. Camera.bf: 40.0  
  27.   
  28. # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)  
  29. Camera.RGB: 0  
  30.   
  31. # Close/Far threshold. Baseline times.  
  32. ThDepth: 50.0  
  33.   
  34. # Deptmap values factor   
  35. DepthMapFactor: 1000.0  
  36.   
  37. #--------------------------------------------------------------------------------------------  
  38. # ORB Parameters  
  39. #--------------------------------------------------------------------------------------------  
  40.   
  41. # ORB Extractor: Number of features per image  
  42. ORBextractor.nFeatures: 1000  
  43.   
  44. # ORB Extractor: Scale factor between levels in the scale pyramid     
  45. ORBextractor.scaleFactor: 1.2  
  46.   
  47. # ORB Extractor: Number of levels in the scale pyramid    
  48. ORBextractor.nLevels: 8  
  49.   
  50. # ORB Extractor: Fast threshold  
  51. # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.  
  52. # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST  
  53. # You can lower these values if your images have low contrast             
  54. ORBextractor.iniThFAST: 20  
  55. ORBextractor.minThFAST: 7  
  56.   
  57. #--------------------------------------------------------------------------------------------  
  58. # Viewer Parameters  
  59. #--------------------------------------------------------------------------------------------  
  60. Viewer.KeyFrameSize: 0.05  
  61. Viewer.KeyFrameLineWidth: 1  
  62. Viewer.GraphLineWidth: 0.9  
  63. Viewer.PointSize:2  
  64. Viewer.CameraSize: 0.08  
  65. Viewer.CameraLineWidth: 3  
  66. Viewer.ViewpointX: 0  
  67. Viewer.ViewpointY: -0.7  
  68. Viewer.ViewpointZ: -1.8  
  69. Viewer.ViewpointF: 500  
  70.   
  71. #--------------------------------------------------------------------------------------------  
  72. # PointCloud Mapping  
  73. #--------------------------------------------------------------------------------------------  
  74. PointCloudMapping.Resolution: 0.01  

现在来运行吧～ 在ORB_SLAM2/下打开终端，输入 ./Examples/RGB-D/rgbd_xtion_cc Vocabulary/ORBvoc.txt Examples/RGB-D/xtion.yaml    系统加载ORB词袋，然后打开xtion设备，采集图像处理，显示角点，轨迹和点云：

关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)

按‘q’或esc程序退出，自动保存估计的轨迹和点云pcd文件到ORB_SLAM2/下（帧数较多时如3000帧，保存时间较长20s左右）。运行pcl_viewer xx.pcd 即可查看。保存优化后的点云的代码在pointcloudmapping.cc里:

[cpp] view plain copy
 print?
  1. globalMap->clear();  
  2. for(size_t i=0;i<keyframes.size();i++)                               // save the optimized pointcloud  
  3. {  
  4.     cout<<"keyframe "<<i<<" ..."<<endl;  
  5.     PointCloud::Ptr tp = generatePointCloud( keyframes[i], colorImgs[i], depthImgs[i] );  
  6.     PointCloud::Ptr tmp(new PointCloud());  
  7.     voxel.setInputCloud( tp );  
  8.     voxel.filter( *tmp );  
  9.     *globalMap += *tmp;  
  10.     viewer.showCloud( globalMap );  
  11. }  
  12. PointCloud::Ptr tmp(new PointCloud());  
  13. sor.setInputCloud(globalMap);  
  14. sor.filter(*tmp);  
  15. globalMap->swap( *tmp );           
  16. pcl::io::savePCDFileBinary ( "optimized_pointcloud.pcd", *globalMap );  
  17. cout<<"Save point cloud file successfully!"<<endl;  

局部1：

关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)

局部2：

关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)

局部3：

关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)

接下来准备改进词袋，尝试加入3d特征描述words，训练然后提高匹配精准度，拭目以待。


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

2016/6/23补充：
很多同学没有FindOpenNI2.cmake, 导致了系统找不到openni2的库
关于SLAM的那些事——实时RGBD_ORB_SLAM (Ubuntu+Xtion)
这里给大家提供了一个FindOpenNI2.cmake文件，复制内容到新的cmake文件，
保存后存到 /usr/share/cmake-2.8/Modules/中去就好了。
[plain] view plain copy
 print?
  1. #  
  2. # Try to find OPenNI2 library and include path.  
  3. # Once done this will define  
  4. #  
  5. #   
  6.   
  7. FIND_PATH( OpenNI2_INCLUDE_PATH OpenNI.h  
  8.     /usr/include  
  9.     /usr/local/include  
  10.     /sw/include  
  11.     /opt/local/include  
  12.     DOC "The directory where OpenNI.h resides")  
  13. FIND_LIBRARY( OpenNI2_LIBRARY  
  14.     NAMES OpenNI2 openni2  
  15.     PATHS  
  16.     /usr/lib64  
  17.     /usr/lib  
  18.     /usr/local/lib64  
  19.     /usr/local/lib  
  20.     /sw/lib  
  21.     /opt/local/lib  
  22.     DOC "The OpenNI2 library")  
  23.   
  24. IF (OpenNI2_INCLUDE_PATH)  
  25.     SET( OpenNI2_FOUND 1 CACHE STRING "Set to 1 if OpenNI2 is found, 0 otherwise")  
  26. ELSE (OpenNI2_INCLUDE_PATH)  
  27.     SET( OpenNI2_FOUND 0 CACHE STRING "Set to 1 if OpenNI2 is found, 0 otherwise")  
  28. ENDIF (OpenNI2_INCLUDE_PATH)  
  29.   
  30. MARK_AS_ADVANCED( OpenNI2_FOUND )  

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

2016/8/12补充：
(回答4楼的问题，由于****回复功能的渣过滤技术，只能写在这里了)

1。Camera.bf中的b指基线baseline（单位：米），f是焦距fx（x轴和y轴差距不大），bf=b*f，和ThDepth一起决定了深度点的范围：bf * ThDepth / fx即大致为b * ThDepth。 基线在双目视觉中出现的比较多，如ORB-SLAM中的双目示例中的EuRoC.yaml中的bf为47.9，ThDepth为35，fx为435.2，则有效深度为47.9*35/435.3=3.85米；KITTI.yaml中的bf为387.57，ThDepth为40，fx为721.54，则有效深度为387.57*40/721.54=21.5米。这里的xtion的IR基线（其实也可以不这么叫）bf为40，ThDepth为50，fx为558.34，则有效深度为3.58米（官方为3.5米）。


2。DepthMapFactor: 1000.0这个很好理解，depth深度图的值为真实3d点深度*1000. 例如depth值为2683，则真是世界尺度的这点的深度为2.683米。 这个值是可以人为转换的（如opencv中的convert函数，可以设置缩放因子），如TUM中的深度图的DepthMapFactor为5000，就代表深度图中的5000个单位为1米。






未来，属于一心想要改变世界的人。

-cc  

顶
3
踩
0
 
 

相关推荐