ORB_SLAM2实战-（ORB_SLAM2-B）

https://www.jianshu.com/p/c3e8c88edb64

ORB_SLAM安装

源文件目录https://github.com/raulmur/ORB_SLAM2

安装orb_slam首先需要安装依赖库

1. 安装Pangolin

• sudo apt-get install libglew-dev
• sudo apt-get install cmake
• sudo apt-get install libpython2.7-dev
• sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev
• git clone https://github.com/stevenlovegrove/Pangolin.git

sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev

cd Pangolin
mkdir build
cd build
cmake ..
make -j7

1  安装依赖

（1）Pangolin：https://github.com/stevenlovegrove/Pangolin

安装依赖：

sudo apt-get install libglew-dev

sudo apt-get install cmake

sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev

sudo apt-get install libpython2.7-dev

安装Pangolin:

git clone https://github.com/stevenlovegrove/Pangolin.git

cd Pangolin

mkdir build

cd build

cmake -DCPP11_NO_BOOSR=1 ..

make -j

2. 安装OpenCV------（2）OpenCV：http://opencv.org/（我使用的是OpenCV 2.4.10）

   安装步骤：参考：http://blog.csdn.net/csqingchen/article/details/43968925

   ros中自带

3. 安装Eigen3
   后续安装----

   （3）Eigen3：http://eigen.tuxfamily.org/

   直接在终端：sudo apt-get install libeigen3-dev

4. 安装BLAS and LAPACK

sudo apt-get install libblas-dev
sudo apt-get install liblapack-dev

5. 安装DBoW2 和 g2o
   包含在orb_slam的第三方库中，后续一起安装

编译g2o

1. cd ~/catkin_ws/ORB_SLAM2/Thirdparty/g2o/
2. mkdir build
3. cmake ..
4. make

• 1
• 2
• 3
• 4

编译DBoW2

1. cd ~/catkin_ws/ORB_SLAM2/Thridparty/DBoW2
2. mkdir build
3. cmake ..
4. make

安装orb_slam

1. 下载软件包

cd catkin_ws/src
git clone https://github.com/raulmur/ORB_SLAM2.git

2. 编译第三方库

cd ORB_SLAM2
chmod +x build.sh
./build.sh

    碰到问题：(没有遇到)

找不到，但是eigen3已经安装，

解决——运行指令：sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen

方向1--data

 下载并运行TUM（慕尼黑工业大学）的数据集

目前为止，我实际跑了单目、RGB-D的一个数据集作为调试用。

下载TUM的数据集：http://vision.in.tum.de/rgbd/dataset/freiburg1/rgbd_dataset_freiburg1_xyz.tgz

解压放到~/ORB_SLAM2/Examples下

运行单目的指令：Monocular Examples TUM Dataset

cd ~/ORB_SLAM2/

　　./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml Examples/rgbd_dataset_freiburg1_xyz

运行RGB-D的指令： RGB-D Example TUM Dataset

      cd ~/ORB_SLAM2/

./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml Examples/rgbd_dataset_freiburg1_xyz Examples/RGB-D/associations/fr1_xyz.txt

以上均不需要ROS，在ubuntu 14.04下就能实现。

 

继续跑TUM（慕尼黑工业大学）的数据集：

初始化失败的数据集：(不完全统计)

rgbd_dataset_freiburd1_floor

rgbd_dataset_freiburd2_pioneer_slam

rgbd_dataset_freiburd3_nostructure_notexture_near_withloop

方向2--camera

在ROS下跑ORB-SLAM2，使用外接摄像机

 

（1）安装ROS indigo，

 ubuntu14上可以安装安装ROS indigo；ubuntu 16.04下安裝和配置ROs-kinetic

参考

1）ubuntu 16.04下安裝和配置ros（ORB-SLAM-A）

2）ubuntu 16.04下安裝和配置ros

https://blog.csdn.net/jinking01/article/details/79387639
https://blog.csdn.net/tq08g2z/article/details/79209435

（2）到路径Examples/ROS/ORB_SLAM2下，执行：

mkdir build
cd build
cmake .. -DROS_BUILD_TYPE=Release
make -j

完成了ROS indigo下ORB-SLAM2的调试，使用的是索尼的PS3摄像机，效果不错。

ORB-SLAM2的ROS节点，以双目为例，在路径: path/to/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src的ros_stereo.cc中，可以看到订阅左右摄像机的Topic：

    message_filters::Subscriber left_sub(nh, "/stereo/left/image_raw", 1);
    message_filters::Subscriber right_sub(nh, "/stereo/right/image_raw", 1);

发布对应的Tocpic上，就能够运行ORB-SLAM2的Stereo版。

（3）基于VSLAM的四旋翼飞行器自主悬停控制

 

ORB_SLAM测试

安装USB_CAM 驱动

cd catkin_ws/src
git clone https://github.com/bosch-ros-pkg/usb_cam.git
cd ../
catkin_make

运行usb_cam 输出topic为/usb_cam/image_raw

打开摄像头，需要启动ros，在两个不同的终端分别执行以下命令：

roslaunch usb_cam usb_cam-test.launch

具体的

$ roscore

source ~/catkin_ws/devel/setup.bashroslaunch usb_cam usb_cam-test.launch

摄像头标定   （也可以通过MATLAB工具箱标定）

参考ROS单目标定wiki

1. 准备标准黑白棋格盘
   • 下载黑白棋盘

• 打印棋盘（可以A4纸打印，粘贴在硬纸板上）
• 测量棋盘单元边长（A4纸的在0.025 m 左右）

2. 安装测试程序（ros）

$ rosdep install camera_calibration
$ rosmake camera_calibration

3. 启动usb_cam程序

sudo apt-get install ros-indigo-usb-cam (如果没有安装usb_cam)
roslaunch usb_cam usb-cam-test.launch

4. 启动camera_calibration

rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.025 image:=/usb_cam/image_raw camera:=/usb_cam
其中 size是黑白格的横纵点数，square是黑白格边长，image是图像节点名称，camera是相机名称。

5. 实际标定

   
   
   

   标定界面
   
   

   标定界面出现后，按照x（左右）、y（上下）、size（前后）、skew（倾斜）等方式移动棋盘，知道x,y,size,skew的进度条都变成绿色位置，此时可以按下CALIBRATE按钮，等一段时间就可以完成标定。

   
   
   

   标定就绪
   
   标定完成后，点击COMMIT按钮，最终的标定结果会在终端出现，在usb_cam的终端上会有yaml文件保存地址。
   
   

   标定结果
6. 结果转换
   转换结果如下所示：
   image_width: 640
   image_height: 480
   camera_name: usb_cam
   camera_matrix:
   rows: 3
   cols: 3
   data: [914.5937398762499, 0, 313.2995046937195, 0, 906.4439566149695, 299.4151672923101, 0, 0, 1]
   distortion_model: plumb_bob
   distortion_coefficients:
   rows: 1
   cols: 5
   data: [0.09838481350767736, 0.1181389276375996, 0.01127830741538815, 0.006633733461628469, 0]
   rectification_matrix:
   rows: 3
   cols: 3
   data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
   projection_matrix:
   rows: 3
   cols: 4
   data: [933.7507934570312, 0, 315.2716618576233, 0, 0, 926.8785400390625, 300.85249210037, 0, 0, 0, 1, 0]

而在orb_slam中的格式如下，需要将上面的
camera_matrix：

camera_matrix

distortion_coefficients：

distortion_coefficients

参数值对应。其中ORB参数与Viewer参数不变。
最终的mycam.yaml文件保存在：

src/ORB_SLAM2/Examples/Monocular/mycam.yaml

    %YAML:1.0
    #--------------------------------------------------------------------------------------------
    # Camera Parameters. Adjust them!
    #--------------------------------------------------------------------------------------------

    # Camera calibration and distortion parameters (OpenCV) 
    Camera.fx: 914.5937398762499
    Camera.fy: 906.4439566149695
    Camera.cx: 313.2995046937195
    Camera.cy: 299.4151672923101

    Camera.k1: 0.09838481350767736
    Camera.k2: 0.1181389276375996
    Camera.p1: 0.01127830741538815
    Camera.p2: 0.006633733461628469
    Camera.k3: 0

    Camera.width: 640
    Camera.height: 480

    # Camera frames per second 
    Camera.fps: 30.0

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

    #--------------------------------------------------------------------------------------------
    # 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

运行demo

1. 环境配置

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/ubuntu/catkin_ws/src/ORB_SLAM2/Examples/ROS

2. 编译

cd src/ORB_SLAM2/Examples/ROS/ORB_SLAM2
mkdir build
cd build
cmake .. -DROS_BUILD_TYPE=Release
make -j

3. 启动demo

rosrun ORB_SLAM2 Mono /home/your_path/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/your_path/catkin_ws/src/ORB_SLAM2/Examples/Monocular/mycam.yaml

rosrun ORB_SLAM2 Mono /home/ubuntu/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/ubuntu/catkin_ws/src/ORB_SLAM2/Examples/Monocular/mycam.yaml

rosrun ORB_SLAM2 Mono /home/ubuntu/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/ubuntu/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml

其中有两个参数，第一个参数为加载的字典，第二个参数为相机参数，更改为自身的相机参数即可。

/your_path/    ubuntu   

新建一个 mycam.yaml

例子1 运行

环境配置

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS

启动Mono

rosrun ORB_SLAM2 Mono /home/ubuntu/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/ubuntu/catkin_ws/src/ORB_SLAM2/Examples/Monocular/mycam.yaml

如下截图

例子2 运行

环境配置

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS

启动Mono

rosrun ORB_SLAM2 Mono /home/ubuntu/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/ubuntu/catkin_ws/src/ORB_SLAM2/Examples/ ROS/ORB_SLAM2/Asus.yaml

(打开新的终端)运行rqt_graph

运行顺序：
１．第一个终端

roscore

• 1

２．第二个终端运行usb_cam

roslaunch usb_cam usb_cam-test.launch

• 1

观察camera是否在正常输出图像，正常则继续
３．第三个终端运行

rosrun ORB_SLAM2 Mono /home/sun/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/sun/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml 

• 1

因为现在是单目SLAM,采用的mono，第二个参数是标定参数保存的路径．
读vocabulary需要一点时间，等等．．．．
正常情况下，你这个时候应该是看到这样的东西

，
４．第四个终端：
运行rqt_rqt_graphgraph
你应该可以看到这样的东西

话题没有接上，所以当然是waiting for images咯　．

关掉第三个终端，进到catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src 打开ros_mono.cc
将subscribe的话题改为/usb_cam/image_raw,将话题接上

    ros::Subscriber sub = nodeHandler.subscribe("/usb_cam/image_raw", 1, &ImageGrabber::GrabImage,&igb);

• 1
• 2

重新编译ROS的example

1. cd ~/catkin_ws/src/ORB_SLAM2/Example/ROS/ORB_SLAM2
2. mkdir build
3. cd build
4. cmake ..
5. make

• 1
• 2
• 3
• 4
• 5

再来一次：

rosrun ORB_SLAM2 Mono /home/ｘｘ/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/xx/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml 

• 1

到这里应该就可以跑起来了

总结：刚刚开始一段时间，还请帮忙及时提出改正．

linux系统IDE找不到Eigen

编译过程中遇到

/usr/local/include/vikit/math_utils.h:12:22: fatal error: Eigen/Core: No such file or directory

但是，我是确定安装了eigen的，这是怎么回事呢～

使用pkg-config 检测我的安装

pkg-config --cflags eigen
Package eigen was not found in the pkg-config search path.
Perhaps you should add the directory containing `eigen.pc'
to the PKG_CONFIG_PATH environment variable
No package 'eigen' found

再次检测

pkg-config --cflags eigen3
-I/usr/local/include/eigen3 

于是

sean@fight:/usr/local/include$ sudo ln -s ./eigen3/Eigen/   ./Eigen

再次使用IDE编译，通过～
但pkg-config --cflags eigen 还是没有检测到。

摄像机调用问题

roscore cannot run as another roscore/master is already running. Please kill other roscore/master

在终端里写下

killall -9 roscore

killall -9 rosmaster

即可解决

实时用摄像头（可笔记本自带或者外加摄像头）跑数据

（1）安装usb_cam package

   $ cd ~/catkin_ws/src  
    $ git clone https://github.com/bosch-ros-pkg/usb_cam.git  
    $ cd ~/catkin_ws  
    $ catkin_make  

笔记本自带的摄像头的设备号一般为/dev/video0    外接摄像头一般是

"video_device"value="/dev/video1"/>

（2）把ORB-SLAM2，和 usb_cam放到catkin下src目录下

     $ cd ~/catkin_ws/src  
    $ roscore    //初始化
    $ roslaunch usb_cam usb_cam-test.launch     //启动usb_cam包下的.launch文件启动摄像头。

此时证明摄像头可以正常使用

～～～使用自定义 launch 文件设置摄像头：
usb_cam 给了我们一个默认的 launch 文件在如下目录

~/catkin-ws/usb_cam/src/usb_cam/launch/usb_cam-test.launch

1	~/catkin-ws/usb_cam/src/usb_cam/launch/usb_cam-test.launch

如果想要自定义一个我们自己的launch文件，我们可以复制这个文件为一个 usb_cam.launch，然后打开这个文件：

1 2 3 4 5 6 7 8 9 10 11 12 13 14

<launch>   <nodename="usb_cam"pkg="usb_cam"type="usb_cam_node"output="screen">     <paramname="video_device"value="/dev/video0"/>     <paramname="image_width"value="640"/>     <paramname="image_height"value="480"/>     <paramname="pixel_format"value="yuyv"/>     <paramname="camera_frame_id"value="usb_cam"/>     <paramname="io_method"value="mmap"/>   </node>   <nodename="image_view"pkg="image_view"type="image_view"respawn="false"output="screen">     <remapfrom="image"to="/usb_cam/image_raw"/>     <paramname="autosize"value="true"/>   </node> </launch>

其中 /div/video0 表示是第一个摄像头，如果你有多个摄像头，可以将此改为 /div/video1 等等。想要查看当前连接设备，使用如下命令即可：

ls /dev/video*

1	ls/dev/video*

其余参数请见参考文献[1]的说明。

修改好后运行这个文件：

roslaunch usb_cam usb_cam.launch

1	roslaunchusb_camusb_cam.launch

错误提示：
1、Error: package ‘image_view’ not found
如果出现：

[rospack] Error: package 'image_view' not found

1	[rospack]Error:package'image_view'notfound

表明你的 image_view 没有安装，可以执行以下命令安装即可：

sudo apt-get install ros-indigo-image-view

1	sudo apt-getinstallros-indigo-image-view

（3）用ORB-SLAM2实时跑数据（记住:若是用外加摄像头，需要在usb_cam-test.launch和usb_cam_node.cpp把摄像头的设备号改为/dev/video1）

  $ cd ~/catkin_ws/src  
  $ rosrun ORB_SLAM2 Mono /home/ubantu/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt 
       /home/ubantu/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml    
       （即 rosrun ORB-SLAN2 Mono    ORBvoc.txt路径    Asus.yaml路径）

6.标定摄像头（为了防止镜头下的图片发生畸变）

1）摄像头标定时所处的平面位置一旦改变，一般会影响相机内参，需重新标定

2）将标定后的参数替换相机原有的内参，重新跑一遍即可。