软件要求：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算是彻底跑通了。
![[2021最新]Java时间戳和日期时间互转换](https://img-blog.csdnimg.cn/20210420110509853.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ0NDkxNzA5,size_16,color_FFFFFF,t_70)
