在gazebo环境中使用ORB_SLAM2跑单目、双目、深度相机

电脑环境：ubuntu16.04 + kinetic(源码安装)

ORB_SLAM2在gazebo中跑RGBD

参考博客https://blog.csdn.net/li528405176/article/details/81164637搭建的ROS-Academy-for-Beginners的gazebo仿真环境，但opencv使用的自带版本2.4.9.1，编译运行orb都没问题，就没按上面的方法安装opencv3.2了（主要是电脑上装了cuda10，貌似不支持opencv3.2版本，反正不影响orb的编译与运行，索性不重装opencv了）

roslaunch robot_sim_demo robot_spawn.launch
roslaunch orbslam2_demo ros_orbslam2.launch
rosrun robot_sim_demo robot_keyboard_teleop.py

博客里面把depth的topic名字在ros_rgbd.cc中给改了，然后运行的，其实原因是ros_orbslam2.launch文件写的有问题，remap没生效，如果不想修改ros_rgbd.cc重新编译，那就用以下办法：

把原文ros_orbslam2.launch中的：

<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"/>

修改为：

<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"/>
    node>

这样remap就生效了。实验下来，建图效果还可以，看着还像个方形的房子，只是在机器人运动过程中相机跟踪也有丢失的情况，转速慢一点，会好很多。

ORB_SLAM2在gazebo中跑mono

参考博客https://www.jianshu.com/p/811e24e8965a,文中一直处于尝试初始化，我的仿真结果也一样，一直处于尝试初始化中，然后修改了相机参数如下（最好将ROS-Academy-for-Beginners包里面的xbot-u.gazebo复制一个为xbot-u_mono.gazebo来修改参数，注意修改robot.xacro使用xbot-u_mono.gazebo）：

将xbot-u.gazebo原文中的：

 
    <gazebo reference="camera_link">  
      <sensor type="depth" name="camera">
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>cameracameraName>
          <alwaysOn>truealwaysOn>
          <updateRate>10updateRate>
          <imageTopicName>rgb/image_rawimageTopicName>
          <depthImageTopicName>depth/image_rawdepthImageTopicName>
          <pointCloudTopicName>depth/pointspointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_infocameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_infodepthImageCameraInfoTopicName>
          <frameName>camera_link_opticalframeName>
          <baseline>0.1baseline>
          <distortion_k1>0.0distortion_k1>
          <distortion_k2>0.0distortion_k2>
          <distortion_k3>0.0distortion_k3>
          <distortion_t1>0.0distortion_t1>
          <distortion_t2>0.0distortion_t2>
          <pointCloudCutoff>0.4pointCloudCutoff>
        plugin>
      sensor>
    gazebo>

再复制的xbot-u_mono.gazebo中对应位置修改如下：

  <gazebo reference="camera_link">
    <sensor type="camera" name="camera1">
      <update_rate>50.0update_rate>
      <camera name="head">
        <horizontal_fov>1.3962634horizontal_fov>
        <image>
          <width>320width>
          <height>240height>
          <format>R8G8B8format>
        image>
        <clip>
          <near>0.02near>
          <far>300far>
        clip>
        <noise>
          <type>gaussiantype>
          <mean>0.0mean>
          <stddev>0.007stddev>
        noise>
      camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>truealwaysOn>
        <updateRate>0.0updateRate>
        <cameraName>cameracameraName>
        <imageTopicName>image_rawimageTopicName>
        <cameraInfoTopicName>camera_infocameraInfoTopicName>
        <frameName>camera_linkframeName>
        <hackBaseline>0.07hackBaseline>
        <distortionK1>0.0distortionK1>
        <distortionK2>0.0distortionK2>
        <distortionK3>0.0distortionK3>
        <distortionT1>0.0distortionT1>
        <distortionT2>0.0distortionT2>
      plugin>
    sensor>
  gazebo>

上面的修改跟参考博客的区别是修改了相机的宽、高、帧率。

其次修改PATH_TO_SETTINGS_FILE，博客中直接使用了Monocular文件夹中的TUM3.yaml,这里复制TUM3.yaml为TUM4.yaml，然后：

原文TUM4.yaml（复制的TUM3.yaml）中的下面部分：

%YAML:1.0

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

# 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

修改成：

%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.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

同样主要是修改了相机的宽、高、帧率，其中相机参数沿用跑RGBD时的参数。

因为改成了新文件，所以原先的orblsam2_demo中的ros_orbslam2.launch文件也要修改一下，如下：

<launch>
    <arg name="PATH_TO_VOCABULARY" value="$(find ORB_SLAM2)/../../../Vocabulary/ORBvoc.txt"/>
    <arg name="PATH_TO_SETTINGS_FILE" value="$(find ORB_SLAM2)/../../Monocular/TUM4.yaml "/>
    
    <node name="Mono" pkg="ORB_SLAM2" type="Mono" args="$(arg PATH_TO_VOCABULARY) $(arg PATH_TO_SETTINGS_FILE)"/>
launch>

运行结果如下：

实验下来，很快就初始化成功了，除了比RBGD更容易丢失外，图的效果跟RGBD建的差的太远了，感觉是乱的，不知道是不是仿真机器人跑的快，还是参数不对，有知道的麻烦告知（下方评论一下）。

ORB_SLAM2在gazebo中跑stereo

参考博客https://www.jianshu.com/p/811e24e8965a,文中gazebo文件里面的参数，我按博主写的Stereo_setting.yaml文档里面的参数做了以下修改：

  <gazebo reference="camera_link">
      <sensor type="multicamera" name="stereo_camera">
        <update_rate>20.0update_rate>
        <camera name="left">
          <horizontal_fov>1.3962634horizontal_fov>
          <image>
            <width>752width>
            <height>480height>
            <format>R8G8B8format>
          image>
          <clip>
            <near>0.02near>
            <far>300far>
          clip>
          <noise>
            <type>gaussiantype>
            <mean>0.0mean>
            <stddev>0.007stddev>
          noise>
        camera>
        <camera name="right">
          <pose>0 -0.07 0 0 0 0pose>
          <horizontal_fov>1.3962634horizontal_fov>
          <image>
            <width>752width>
            <height>480height>
            <format>R8G8B8format>
          image>
          <clip>
            <near>0.02near>
            <far>300far>
          clip>
          <noise>
            <type>gaussiantype>
            <mean>0.0mean>
            <stddev>0.007stddev>
          noise>
        camera>
        <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
          <alwaysOn>truealwaysOn>
          <updateRate>0.0updateRate>
          <cameraName>cameracameraName>
          <imageTopicName>image_rawimageTopicName>
          <cameraInfoTopicName>camera_infocameraInfoTopicName>
          <frameName>camera_linkframeName>
          <hackBaseline>0.07hackBaseline>
          <distortionK1>0.0distortionK1>
          <distortionK2>0.0distortionK2>
          <distortionK3>0.0distortionK3>
          <distortionT1>0.0distortionT1>
          <distortionT2>0.0distortionT2>
        plugin>
      sensor>
    gazebo>

实验结果，特征点很多，基本没有发现跟踪丢失的情况，但建的图，看不出来是个方形的房子，我反复跑了好几圈，也是一样，不知道问题出哪里了。