gazebo小车使用stereo相机ORB_SLAM2

gazebo小车使用stereo相机运行ORB_SLAM2

修改urdf文件

在工作空间中找到urdf模型，我的是

urdf/mybot.gazebo.xacro

将stereo相机添加进去（替换原来的相机）
ORB_SLAM2下载安装方法见我之前的博客

<!-- stereo camera -->
    <gazebo reference="base_camera_link">  
      <sensor type="multicamera" name="stereocamera">
        <always_on>true</always_on>
        <update_rate>10</update_rate>
        <visualize>false</visualize>
        <camera name="left">
          <pose>0 0 0 0 0 0</pose>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>320</width>
            <height>240</height>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
        </camera>
        <camera name="right">
          <pose>0 -0.07 0 0 0 0</pose>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>320</width>
            <height>240</height>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
        </camera>        
        <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
          <cameraName>stereocamera</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <cameraName>stereocamera</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>camera_link_optical</frameName>
          <baseline>0.07</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>
        </plugin>
      </sensor>
    </gazebo>

编译工作空间、修改topic

将下载好的ORB_slam2放到工作空间里，进行编译

cd ORB_SLAM2
cmake ..
./build_ros.sh
cd ..
cd ..
(回到工作空间路径下）
catkin_make

查看之前修改的机器人urdf模型文件得到机器人的stereo左右相机topic,修改ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc文件下订阅的摄像头信息

cd ORB_SLAM2 
./build_ros.sh

编译功能包

运行demo

运行gazebo
运行slam2-stereo相机

rosrun ORB_SLAM2 Stereo src/ORB_SLAM2/Vocabulary/ORBvoc.txt src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Stereo_setting.yaml false

运行键盘控制控制机器人运动

rosrun robotcar teleop_robot_key

通过键盘控制小车运动，可以看到有两个窗口。CURRENT Frame和Map viewer。

运行结果

在CURRENT Frame窗口中看到摄像机实时检测到的画面和特征点，在Map viewer窗口中摄像机（即机器人）运行的轨迹和特征点的点云（3维）信息。
现在通过键盘控制机器人遍历地图。


在运行过程中特征点检测停止并且Map viwer停止更新。但是图像传输没有断。
可以看到current frame中显示track lost 轨迹丢失，因为是仿真环境，特征点较少，经历了一段没有特征点的情况会导致机器人无法获知轨迹的状态。如果是在现实环境中做ORB_SLAM2就可以有效避免这种情况。