1、ORB_SLAM2利用笔记本摄像头跑单目

1、下载usb_cam源码并配置环境：

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

注意这里我的catkin_ws是之前编译过的，如果使用未编译的文件夹首次编译后需要添加source路径，在前面很多篇中都提到，不赘述。

2、测试摄像头

roslaunch usb_cam usb_cam-test.launch

正常情况下是能够显示摄像头图像的，如果没有的话要思考前面的编译过程是否有问题。应该来说没什么问题。

3、用ORB-SLAM2实时跑数据

$ rosrun ORB_SLAM2 Mono /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM1.yaml

（即 rosrun ORB-SLAN2 Mono ORBvoc.txt路径 TUM1.yaml路径）
在这里插入图片描述
这里的

/home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM1.yaml

似乎应该是相机内参，在：

/home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml

路径下也有一个对应的可运行的参数文件，所以这里如果执行：

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

同样也是可以运行的。
在这里插入图片描述

另外这里还有一个小问题：

如果运行ORB_SLAM2之后图像显示不出来，可能是因为topic的问题，这时候可以查看发布的topic以及订阅的topic是否相同。gazebo插件中发布的topic可以使用：

rostopic list

命令查看，一般usb摄像头发布的图像格式应该是：usb_cam/image_raw
而我们订阅的程序在：catkin/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src路径下的ros_mono.cc文件中，可以看到它的ros::subscriber一般初始化为：camera/image_raw。这里将其修改为上面的topic即可。

2、ORB_SLAM2利用gazebo仿真环境跑单目仿真

关于仿真环境的建立，具体可以参考之前写的：ROS学习总结十：Gazebo物理仿真环境搭建，这里就不赘述了。

例如下面的是我随手建立的一个gazebo仿真环境（中间蓝色的是我的机器人）：
在这里插入图片描述
然后使用三个终端分别打开下面的命令：

$ roscore
$ roslaunch mbot_gazebo view_mbot_with_camera_gazebo.launch
$ rosrun ORB_SLAM2 Mono /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM1.yaml

注意这里中间的：

$ roslaunch mbot_gazebo view_mbot_with_camera_gazebo.launch

为我的gazebo启动程序，不一定相同，根据自己的路径设定。

另外注意image的rostopic与ORB_SLAM2中订阅的topic问题。

正常来说这时候就可以运行了，我们看一下效果：

在这里插入图片描述
嗯。。。效果不怎么样，发现在gazebo仿真中的特征点非常少，虽然我给了很多的特征了但是不知道为什么并没有被识别。

于是我又改用了另一个参数运行：

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

得到新的效果：

在这里插入图片描述
虽然效果比第一个好，但是特征跟使用笔记本摄像头相比差的真不是一点半点。

原本以为是gazebo仿真环境的问题，但是后来参考了一篇博客才发现应该还是参数的问题，在该博客中给出了另一种参数配置：

https://www.freesion.com/article/6807208904/

就是将原来TUM3.yaml中的参数：

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 535.4
Camera.fy: 539.2
Camera.cx: 320.1
Camera.cy: 247.6

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

# 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

改成新的：

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 277.19135641132203
Camera.fy: 277.19135641132203
Camera.cx: 160.5
Camera.cy: 120.5

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera.width: 320
Camera.height: 240

# Camera frames per second 
Camera.fps: 50.0

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

为了保险起见最好重命名一个TUM4.yaml。

重新运行以后特征提取上确实好了很多：
在这里插入图片描述
但是似乎特征点数量还是不太够，最终我绕着跑了一圈都没有初始化成功：
在这里插入图片描述
于是我使用了ROS-Academy-for-Beginners功能包，这个可以直接在GitHub上下载编译，编译过程与其他ROS功能包相同，中间可能会提示缺少几个依赖项，使用sudo apt-get install 安装即可

编译后通过运行：

$ roslaunch robot_sim_demo robot_spawn.launch

打开gazebo环境，然后运行：

$ rosrun ORB_SLAM2 Mono /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM4.yaml

可以看到新运行的环境比之前的环境中多了很多的特征点。

启动键盘节点运动小车初始化相机：

$ roslaunch mbot_teleop mbot_teleop.launch

在这里遇到了单目调试过程中最大的一个坑：
每次我初始化结束后，下面map viewer上有位姿之后程序立马闪退：
在这里插入图片描述
就到这里然后报错：
在这里插入图片描述
吐槽一下：

segmentation fault（core dumped）真的是难搞

花了三天各种找原因，试了一万种方法，差点崩溃，最后在这里：

https://blog.csdn.net/qq_43525260/article/details/104378643?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.channel_param&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.channel_param

找到了解决方法。

只是cmake.list文件中多出的一个-march=native导致了程序各种bomb。关于-march=native，百度了一个解释是：-march=[native]选项：gcc/g++编译器通过-march指定cpu架构，指定该选项之后编译器将不会生成兼容的指令集，而是该架构支持的特定指令集，可以取得一部分优化的效果。特殊地，-march=native选项让编译器获取当前机器的cpu架构，并生成该架构的最优指令，达到优化指令集的目的。不是很能理解。

最终我通过将：

ORB_SLAM2/Thirdparty/DBoW2/CMakeLists.txt
ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt
ORB_SLAM2/CMakeLists.txt
ORB_SLAM2/Thirdparty/g2o/CMakeLists.txt

中的-march=native全部删除后，按照之前的编译方式重新全部编译一次以后，终于可以正常运行了。

注意不只是要运行：

./build_ros.sh

需要把前面的g2o以及DBoW2等包都要重新编译一遍，具体的翻看上一篇博客的编译过程。不赘述。

然后看一下效果：

在这里插入图片描述
总体来说其实还行，但是似乎对于自身的定位是有点差的：

例如当我绕场景跑完一圈以后得到的图像：

在这里插入图片描述
这起点与终点差的不是一点半点。

不过幸好似乎有个回环检测，总算把自己给圆回去了：
在这里插入图片描述
这张看起来还有点样子，不过单纯从自定位的角度来说单目的效果真的不太好。

3、ORB_SLAM2利用gazebo仿真环境跑双目仿真

双目仿真使用的是stereo插件，相比于单目要复杂一点，主要参考于：

https://www.jianshu.com/p/811e24e8965a

首先需要修改相机参数，双目相机参数如下：

<!-- stereo camera -->
  <gazebo reference="camera_link">
      <sensor type="multicamera" name="stereo_camera">
        <update_rate>30.0</update_rate>
        <camera name="left">
          <horizontal_fov>1.3962634</horizontal_fov>
          <image>
            <width>800</width>
            <height>800</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.02</near>
            <far>300</far>
          </clip>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.007</stddev>
          </noise>
        </camera>
        <camera name="right">
          <pose>0 -0.07 0 0 0 0</pose>
          <horizontal_fov>1.3962634</horizontal_fov>
          <image>
            <width>800</width>
            <height>800</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.02</near>
            <far>300</far>
          </clip>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.007</stddev>
          </noise>
        </camera>
        <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>0.0</updateRate>
          <cameraName>camera</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>camera_link</frameName>
          <hackBaseline>0.07</hackBaseline>
          <distortionK1>0.0</distortionK1>
          <distortionK2>0.0</distortionK2>
          <distortionK3>0.0</distortionK3>
          <distortionT1>0.0</distortionT1>
          <distortionT2>0.0</distortionT2>
        </plugin>
      </sensor>
    </gazebo>

建议新建一个模型，复制原来的参数过来以后替换掉相机的部分，这样在保留原来的模型的基础上修改更少。

然后再给一个相机配置文件：

%YAML:1.0

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

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 277.19135641132203
Camera.fy: 277.19135641132203
Camera.cx: 160.5
Camera.cy: 120.5

Camera.k1: 0.0
Camera.k2: 0.0
Camera.k3: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera.width: 752
Camera.height: 480

# Camera frames per second 
Camera.fps: 20.0

# stereo baseline times fx
Camera.bf: 47.90639384423901

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

#--------------------------------------------------------------------------------------------
# Stereo Rectification. Only if you need to pre-rectify the images.
# Camera.fx, .fy, etc must be the same as in LEFT.P
#--------------------------------------------------------------------------------------------
LEFT.height: 480
LEFT.width: 752
LEFT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [435.2046959714599, 0, 367.4517211914062, 0,  0, 435.2046959714599, 252.2008514404297, 0,  0, 0, 1, 0]

RIGHT.height: 480
RIGHT.width: 752
RIGHT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0]
RIGHT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1]
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0]

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

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

# 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

编写launch文件：
orbslam2_stereo.launch:

<launch>
    <arg name="PATH_TO_VOCABULARY" value="$(find ORB_SLAM2)/vocabulary_files/ORBvoc.txt"/>
    <arg name="PATH_TO_SETTINGS_FILE" value="$(find ORB_SLAM2)/setting_files/Stereo_setting.yaml"/>
    
    <node name="Stereo" pkg="ORB_SLAM2" type="Stereo" args="$(arg PATH_TO_VOCABULARY) $(arg PATH_TO_SETTINGS_FILE) false">
    </node>

</launch>

在这里要说明一下ORB_SLAM2的节点启动，最后有一个false，是代表是否需要双目校正，在这里填ture的话，反而会出现严重的畸变，因此推测，仿真出来的相机镜头是不需要标定和校正的。

启动命令：

roslaunch ORB_SLAM2 orbslam2_stereo.launch

这里的ORB_SLAM2是上面launch文件的一级路径，这里在我使用这个roslaunch的时候会出现参数文件找不到的问题，是因为它对应这里的ORB_SLAM2是指的catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2路径，而我们前面的参数文件是以catkin_ws/src/ORB_SLAM2作为路径的，简单点的方法就是将前面的参数以这个路径重新保存一份，只保存其中的vox.txt以及yaml文件即可。

运行结果：
在这里插入图片描述

在这里插入图片描述
可以看到在我快走完一圈的时候，整个位姿的偏差其实还是非常大的，跟单目的效果感觉差不多

最后也是靠一个回环检测拯救了过来：

在这里插入图片描述

4、ORB_SLAM2利用gazebo仿真环境跑RGBD仿真

同样的修改一下相机模型参数：

  <!-- camera -->
    <gazebo reference="camera_link">  
      <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${60.0*3.14/180.0}</horizontal_fov>
          <image>
              <format>R8G8B8</format>
              <width>640</width>
              <height>480</height>
          </image>
          <clip>
              <near>0.05</near>
              <far>8.0</far>
          </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>camera</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>camera_frame_optical</frameName>
          <baseline>0.1</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
      </sensor>
    </gazebo>

这里我重新复制了ROS-Academy-for-Beginners功能包下的ros_sim_demo/xbot-ugazebo，重命名xbot_kinect，这个里面本身使用的是双目相机，然后将相机参数部分修改掉。注意修改robot.xacro文件中的模型名字，因为launch文件调用的是robot.xacro文件。

launch文件可以不用写，因为在ROS-Academy-for-Beginners功能包下的orbslam2_demo下有一个对应的launch文件，打开看一下我们可以知道它是一个对应的RGBD相机的launch文件，可以直接使用这个launch文件。

<launch>
    <arg name="PATH_TO_VOCABULARY" value="$(find ORB_SLAM2)/../../../Vocabulary/ORBvoc.txt"/>
    <arg name="PATH_TO_SETTINGS_FILE" value="$(find orbslam2_demo)/param/Zdzn.yaml "/>
    
    <node name="RGBD" pkg="ORB_SLAM2" type="RGBD" args="$(arg PATH_TO_VOCABULARY) $(arg PATH_TO_SETTINGS_FILE)"/>
   	 <remap from="/camera/depth_registered/image_raw" to="/camera/depth/image_raw"/>
</launch>

然后我们分别运行下列命令：

$ roscore
$ roslaunch robot_sim_demo robot_spawn.launch
$ roslaunch orbslam2_demo ros_orbslam2.launch
$ roslaunch mbot_teleop mbot_teleop.launch

最终我们运行得到：

这是快走完一圈时的路径：

在这里插入图片描述
这是走完一圈回环检测后的路径：

在这里插入图片描述
只能疯狂赞扬一下ORB_SLAM2的回环检测是真的强大，每次跑到天上去都能通过回环检测圆回来是真的可以。

但是这个前端位置检测真的一言难尽，不知道是不是我相机参数的问题，有知道的小伙伴可以评论告知一下。