gazebo仿真跑VINS-Fusion双目视觉惯性SLAM

目录

实验环境: NIVIDIA Jetson TX2 (Ubuntu 20.04 arm架构)

下载仿真环境源码

参考的这篇博客gazebo中给机器人添加16线激光雷达跑LIO-SAM.
下载github源码并创建工作空间对源码进行编译: https://github.com/linzs-online/robot_gazebo.git

由于我是双目+IMU, 因此需要删掉LIO-SAM, pcd2pgm, scout_slam_nav, velodyne_simulator这四个功能包. realsense_ros_gazebo是realsense深度相机的仿真功能包, 需要自己基于这里面的相机仿真文件编写一个双目相机仿真文件, 后面会介绍, scout_gazebo包里面就主要是机器人仿真模型和IMU的仿真包.

创建自己的仿真环境

这里可以选择使用源码里提供的仿真环境, 也可以自己创建一个适合自己的仿真环境添加到launch文件里. 由于源码里仿真环境的环境纹理太单调了, 于是我自己随便搭建了一个仿真环境.
仿真环境搭建方法: 终端启动gazebo, 在Insert里直接插入自己需要的模型就可以, 然后保存为world文件, 并放在源码功能包的world文件夹下.

最后在scout_gazebo.launch文件里修改导入的仿真环境文件夹路径

编写双目相机xacro仿真文件

这里我是综合参考: gazebo中仿真双目相机(stereo camera)和scout_gazebo功能包里urdf文件夹里的camera.xacro文件以及ealsense_ros_gazebo仿真功能包里的depthcam.xacro来编写的自己的双面相机xacro仿真文件. 内容如下

<?xml version="1.0"?>
<!-- 摄像头相关的 xacro 文件 -->
<robot xmlns:xacro="http://wiki.ros.org/xacro"> 
    <!-- 摄像头属性 -->
    <xacro:property name="camera_length" value="0.05" /> <!-- 摄像头长度(x) -->
    <xacro:property name="camera_width" value="0.08" /> <!-- 摄像头宽度(y) -->
    <xacro:property name="camera_height" value="0.08" /> <!-- 摄像头高度(z) -->
    <xacro:property name="camera_x" value="0.4" /> <!-- 摄像头安装的x坐标 -->
    <xacro:property name="camera_y" value="0.0" /> <!-- 摄像头安装的y坐标 -->
    <xacro:property name="camera_z" value="${base_z_size / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2  -->
    <xacro:property name="camera_m" value="0.01" /> <!-- 摄像头质量 -->

    <!-- 摄像头关节以及link -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:Box_inertial_matrix m="${camera_m}" l="${camera_length}" w="${camera_width}" h="${camera_height}" />
        
    </link>

    <joint name="camera2base_link" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${camera_x} ${camera_y} ${camera_z}" />
    </joint>
    
    <!-- camera left joints and links -->
    <joint name="left_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0" />
      <parent link="camera" />
      <child link="stereo_left_frame" />
    </joint>
    <link name="stereo_left_frame"/>

    <joint name="left_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
      <parent link="stereo_left_frame" />
      <child link="stereo_left_optical_frame" />
    </joint>
    <link name="stereo_left_optical_frame"/>
    
    <!-- camera right joints and links -->
    <joint name="right_joint" type="fixed">
      <origin xyz="0 -0.07 0" rpy="0 0 0" />
      <parent link="camera" />
      <child link="stereo_right_frame" />
    </joint>
    <link name="stereo_right_frame"/>

    <joint name="right_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
      <parent link="stereo_right_frame" />
      <child link="stereo_right_optical_frame" />
    </joint>
    <link name="stereo_right_optical_frame"/>
    
    <!-- stereo camera --> 
    <gazebo reference="camera">
      <sensor type="multicamera" name="stereocamera">
        <material>Gazebo/Blue</material>
        <always_on>true</always_on>
        <update_rate>30</update_rate>
        <visualize>1</visualize>
        
        <camera name="left">
          <pose>0 0 0 0 0 0</pose>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>640</width>
            <height>360</height>
            <!-- format>L_UINT8</format -->
            <format>R8G8B8</format>
          </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>640</width>
            <height>360</height>
            <!-- format>L_UINT8</format -->
            <format>R8G8B8</format>
          </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>30</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>
</robot>

将编写的stereo_camera_my.xacro文件放在urdf文件夹下

然后在base.xacro文件里导入双目相机仿真文件, 并删除激光雷达模块

这里有个问题: 我的双目相机启动后输出的话题频率一直是维持在25HZ左右, 达不到我仿真文件里写的的30HZ, 而且即使我修改了仿真文件里的相机频率, 话题输出数据的频率依旧是25左右, 好像就跟我仿真文件里的频率没有关系一样, 不知道是不是仿真文件里设置频率的地方有问题. 要是有人看到了知道怎么解决这个问题的话麻烦留言帮忙解决一下, 感谢!

启动仿真

编译好功能包后通过一下命令启动仿真环境

roslaunch scout_gazebo scout_gazebo.launch

启动仿真环境后查看当前输出的话题

获取相机内参

通过 /stereocamera/left/camera_info 和 /stereocamera/right/camera_info 话题查看相机内参, 由于是仿真环境, 所以左右目外参理论上是一样的

然后根据该参编写SLAM需要的内参文件即可

获取双目与IMU的外参

外参的通过ROS的TF坐标变换来获取
启动rviz可视化界面, Add-TF

通过以下命令查看左, 右目相机与IMU的外参

其中imu_base即imu的坐标系, stereo_left_optical_frame 和 stereo_right_optical_frame是左右目相机的相机坐标系, 而stereo_left_frame 和 stereo_right_frame 是左右目相机的安装坐标系, 相机坐标系和相机的安装坐标系是存在一个旋转关系的, 因此需要的是相机坐标系与IMU坐标系的外参, 而不是相机安装坐标系与IMU坐标系的外参.

录制rosbag包并测试VINS-Fusion算法

录制了一下四个话题的数据

其中/odom话题是机器人自身在仿真环境中的里程计数据, 我这里用这个数据来做为机器人的真实轨迹, 用来与VINS-Fusion的轨迹做对比. 下面是轨迹对比图, 效果非常好, 仿真环境下的定位精度很高了.

我录的数据路径总长度71m, evo_rpe的相对位姿误差在毫米级, 简直不可思议