gazebo中vins-fusion在仿真小车上的部署

软件要求：Ubuntu 20.04 ros的noetic版本，我是在虚拟机vitrualbox上运行的

这几天在学ROS，跟着赵虚左老师过了一遍之后，感觉还是有很多不懂的地方，xtdrone上仿真跟着文档走了一遍，好像没学到什么东西，所以我决定想办法自己搭建一个仿真平台，至少实现定位和路径规划的功能。

定位的话，这里我想用vins-fusion来做，奈何网上的资料太少了，完全不知道该从何下手，经过几天的查找资料，我目前算是解决了这个问题。

VINS-Fusion的安装

这里跟着这个教程走就行vins-fusion环境配置、安装与测试-CSDN博客，最后在数据集上测试通过代表安装完成，vins-fusion可以在自己的工作空间下面安装。

Gazebo的搭建

这里我们跟着赵虚左老师的视频【Autolabor初级教程】ROS机器人入门_哔哩哔哩_bilibili，最后会得到一个类似这样的机器人

我们将原camera的位置往左移一点，然后在注释掉原carema，在原camera的位置加上IMU，它会发布名称叫imu/data的消息

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro"><gazebo reference="camera"><material>Gazebo/Bule</material><gravity>true</gravity><sensor name="imu_sensor" type="imu"><always_on>true</always_on><update_rate>100</update_rate><visualize>true</visualize><topic>__default_topic__</topic><plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin"><topicName>imu/data</topicName><bodyName>imu_base</bodyName><updateRateHZ>100.0</updateRateHZ><gaussianNoise>0.01</gaussianNoise><xyzOffset>0 0 0</xyzOffset>     <rpyOffset>0 0 0</rpyOffset><frameName>imu_base</frameName>        </plugin><pose>0 0 0 0 0 0</pose></sensor></gazebo>
</robot>

由于vins-fusion除了imu还需要双目相机，所以图上那个比较大的方块就是我加上的双目相机，代码如下：

<?xml version="1.0"?>
<!-- 摄像头相关的 xacro 文件 -->
<robot xmlns:xacro="http://wiki.ros.org/xacro"> <!-- 摄像头属性 --><xacro:property name="camera_length" value="0.025" /> <!-- 摄像头长度(x) --><xacro:property name="camera_width" value="0.04" /> <!-- 摄像头宽度(y) --><xacro:property name="camera_height" value="0.04" /> <!-- 摄像头高度(z) --><xacro:property name="camera_x" value="0.06" /> <!-- 摄像头安装的x坐标 --><xacro:property name="camera_y" value="0.06" /> <!-- 摄像头安装的y坐标 --><xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2  --><xacro:property name="camera_m" value="0.01" /> <!-- 摄像头质量 --><!-- 摄像头关节以及link --><link name="double_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="double_camera2base_link" type="fixed"><parent link="base_link" /><child link="double_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="double_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="double_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="double_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>

OK,现在我们已经安装好了IMU和双目相机。

VINS-Fusion参数更改

在/VINS-Fusion/config/vi_car/vi_car.yaml（因为我们用的是小车，无人机去改config里面另外的文件），我们需要修改它的imu和image的topic,从这个

imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
output_path: "/home/tong/output/"

改成你自己的topic

imu_topic: "/imu/data"
image0_topic: "/stereocamera/right/image_raw"
image1_topic: "/stereocamera/left/image_raw"
output_path: "/home/tong/output/"

OK,现在启动VINS-Fusion(记得rosrun你刚才修改的vi_car文件)

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/你自己的workspace/src/VINS-Fusion/config/vi_car/vi_car.yaml

可以看到目前是已经跑通的状态，但是现在移动小车就会发现，轨迹很不准确，这是因为相机的内参和外参还没改。

相机的内参和外参更改

在我们刚才修改的vi_car.yaml文件中，我们会找到这个：

cam0_calib: "cam0_mei.yaml"
cam1_calib: "cam1_mei.yaml"

说明我们要修改这两个文件，这两个文件正好就在vi_car.yaml的目录下。

打开仿真环境，使用rostopic list

可以看到有这些

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

通过这个内参更改那两个文件，这是这个文件的含义：

#######################################################################
#                      Calibration Parameters                         #
#######################################################################
# These are fixed during camera calibration. Their values will be the #
# same in all messages until the camera is recalibrated. Note that    #
# self-calibrating systems may "recalibrate" frequently.              #
#                                                                     #
# The internal parameters can be used to warp a raw (distorted) image #
# to:                                                                 #
#   1. An undistorted image (requires D and K)                        #
#   2. A rectified image (requires D, K, R)                           #
# The projection matrix P projects 3D points into the rectified image.#
######################################################################## The image dimensions with which the camera was calibrated. Normally
# this will be the full camera resolution in pixels.# 高 ，单位：像素
uint32 height
# 宽 ，单位：像素
uint32 width# The distortion parameters, size depending on the distortion model.
# For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
# 畸变参数
float64[] D# Intrinsic camera matrix for the raw (distorted) images.
# 未做去畸变处理图像的内参
#     [fx  0 cx]
# K = [ 0 fy cy]
#     [ 0  0  1]
# Projects 3D points in the camera coordinate frame to 2D pixel
# coordinates using the focal lengths (fx, fy) and principal point
# (cx, cy).
float64[9]  K # 3x3 row-major matrix# Rectification matrix (stereo cameras only)
# 仅用于立体相机，通常是多目相机
# 用于极线对齐
# A rotation matrix aligning the camera coordinate system to the ideal
# stereo image plane so that epipolar lines in both stereo images are
# parallel.
float64[9]  R # 3x3 row-major matrix# Projection/camera matrix
# 投影矩阵：去畸变，修正后世界坐标系下的三维坐标点投影到像素坐标系下的二维点
#     [fx'  0  cx' Tx]
# P = [ 0  fy' cy' Ty]
#     [ 0   0   1   0]
# By convention, this matrix specifies the intrinsic (camera) matrix
#  of the processed (rectified) image. That is, the left 3x3 portion
#  is the normal camera intrinsic matrix for the rectified image.
# It projects 3D points in the camera coordinate frame to 2D pixel
#  coordinates using the focal lengths (fx', fy') and principal point
#  (cx', cy') - these may differ from the values in K.
# 单目相机，tx=ty=0
# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
#  also have R = the identity and P[1:3,1:3] = K.
# 双目相机
# For a stereo pair, the fourth column [Tx Ty 0]' is related to the
#  position of the optical center of the second camera in the first
#  camera's frame. We assume Tz = 0 so both cameras are in the same
#  stereo image plane. The first camera always has Tx = Ty = 0. For
#  the right (second) camera of a horizontal stereo pair, Ty = 0 and
#  Tx = -fx' * B, where B is the baseline between the cameras.
# Given a 3D point [X Y Z]', the projection (x, y) of the point onto
#  the rectified image is given by:
#  [u v w]' = P * [X Y Z 1]'
#         x = u / w
#         y = v / w
#  This holds for both images of a stereo pair.
float64[12] P # 3x4 row-major matrix

外参的通过ROS的TF坐标变换来获取，打开rviz，添加tf，如图所示

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

这里我参考gazebo仿真跑VINS-Fusion双目视觉惯性SLAM_gazebo双目相机xzcro-CSDN博客，之所以用camera,是因为我当时把imu放到原camera的位置了，我也没改名字，外参的话，Translation的偏移量，下面第一个Q是四元数，可以搜一下如何使用四元数得到相机的外参矩阵，在vi_car中修改，这样的话，我们的VINS-Fusion算是彻底跑通了。