vins-mono 复现
在 ros 中，我们可能有多个代码库，当然我们可以选择把它们放在一个工作空间下，我们也可以选择把它们放在不同的工作空间下，以防止冲突。比如，你有一个原始的代码库，你做了一些修改有了一个新的代码库，将这两个代码库放在一个 src 下，会导致冲突。所以下面我们重新创建了一个工作空间。
1. 创建和初始化工作空间
以下是创建和初始化一个新的ROS工作空间的步骤：
-
创建工作空间目录
在你的主目录或任何其他合适的地方创建一个目录作为你的ROS工作空间。通常，工作空间命名为vins_ws。mkdir -p ~/vins_ws/src
2. vins-mono
cd ~/vins_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
cd ../
catkin_make
source ~/vins_ws/devel/setup.bash
数据集：
开启 3 个终端：
roslaunch vins_estimator euroc.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play /root/LET-NET/datasets/MH_01_easy.bag
错误：
No protocol specified
qt.qpa.screen: QXcbConnection: Could not connect to display :1
Could not connect to any X display.
解决：在宿主机：
xhost +
3. 跑自己的相机+imu
修改两个文件：
- src/VINS-Mono/config/custom/custom_config_no_extrinsic.yaml
%YAML:1.0
#common parameters
imu_topic: "/imu"
image_topic: "/camera/color/image_raw"
output_path: "/home/shaozu/output"
#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.125197
k2: -0.196591
p1: 0.006816
p2: -0.006225
projection_parameters:
fx: 577.54679
fy: 578.63325
cx: 310.24326
cy: 253.65539
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 1.0909715156015328e-02 # accelerometer measurement noise standard deviation. #0.2 0.04 # Acc误差模型高斯白噪声
gyr_n: 8.0226069504443656e-04 # gyroscope measurement noise standard deviation. #0.05 0.004 # Gyro误差模型高斯白噪声
acc_w: 4.0347767978459928e-04 # accelerometer bias random work noise standard deviation. #0.02 # Acc误差模型随机游走噪声
gyr_w: 4.4153147263889589e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 # Gyro误差模型随机游走噪声
g_norm: 9.81007 # gravity magnitude
#loop closure parameters
loop_closure: 0 # start loop closure
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0 # useful in real-time and large project
pose_graph_save_path: "/home/shaozu/output/pose_graph/" # save and load path
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#rolling shutter parameters
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
#visualization parameters
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4 # size of camera marker in RVIZ
- src/VINS-Mono/vins_estimator/launch/custom_vi.launch
<!-- 这个文件是一个ROS（机器人操作系统）launch文件，用于启动两个ROS节点：vins_estimator和pose_graph -->
<launch>
<!-- 定义配置文件路径参数，默认值为$(find feature_tracker)/../config/uma-vi.yaml -->
<arg name="config_path" default = "$(find feature_tracker)/../config/custom/custom_config_no_extrinsic.yaml" />
<!-- 定义vins路径参数，默认值为$(find feature_tracker)/../config/../ -->
<arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
<!-- 启动 feature_tracker 节点 -->
<node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="screen">
<!-- 设置vins_estimator节点的配置文件参数 -->
<param name="config_file" type="string" value="$(arg config_path)" />
<!-- 设置vins_estimator节点的vins文件夹参数 -->
<param name="vins_folder" type="string" value="$(arg vins_path)" />
</node>
<!-- 启动vins_estimator节点 -->
<node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
<!-- 设置vins_estimator节点的配置文件参数 -->
<param name="config_file" type="string" value="$(arg config_path)" />
<!-- 设置vins_estimator节点的vins文件夹参数 -->
<param name="vins_folder" type="string" value="$(arg vins_path)" />
</node>
<!-- 启动pose_graph节点 -->
<node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">
<!-- 设置pose_graph节点的配置文件参数 -->
<param name="config_file" type="string" value="$(arg config_path)" />
<!-- 设置pose_graph节点的可视化偏移参数 -->
<param name="visualization_shift_x" type="int" value="0" />
<param name="visualization_shift_y" type="int" value="0" />
<!-- 设置pose_graph节点的跳过计数参数 -->
<param name="skip_cnt" type="int" value="0" />
<!-- 设置pose_graph节点的跳过距离参数 -->
<param name="skip_dis" type="double" value="0" />
</node>
</launch>
打开相机
docker exec -it d38 /bin/bash
source ./devel/setup.bash
roslaunch astra_camera astra_pro.launch
终端2： 打开 imu
docker exec -it d38 /bin/bash
ln -s /dev/ttyUSB0 /dev/fdilink_ahrs # 如果 /dev/fdilink_ahrs 找不到的话执行
roslaunch fdilink_ahrs ahrs_data.launch
终端3：
docker exec -it d38 /bin/bash
rqt
终端4：
docker exec -it d38 /bin/bash
cd vins_ws/
source ./devel/setup.bash
roslaunch vins_estimator custom_vi.launch
终端5：
docker exec -it d38 /bin/bash
cd vins_ws/
source ./devel/setup.bash
roslaunch vins_estimator vins_rviz.launch
实验结果：
不动时候，轨迹直接漂移。估计是 IMU 的问题。
我解决了该问题，把 该 N100 IMU 水平放置，使用 商家提供的 地面站软件，对 IMU 进行调平，并且对磁力计进行了 校准，然后再 把 IMU 的频率设置为 20hz， 使用的 相机参数是 相机自带的，IMU 参数是商家提供的默认值，然后 estimate_extrinsic: 2, loop_closure: 1 会在输出目录下得到估计的外参，然后把该外参加进来，再把 estimate_extrinsic: 1， 多弄几次，最后可以把 estimate_extrinsic: 0，现在我们神奇的发现不偏移了。
estimate_td: 1
td: -0.022135
注意：有时候需要观察 rqt 工具，来判断哪些节点没有启动。从而再启动下。
轨迹漂移相关的回答：
- https://www.zhihu.com/question/459171464/answer/1884120112
- https://zhuanlan.zhihu.com/p/544299506
- https://zhuanlan.zhihu.com/p/564283992
- https://zhuanlan.zhihu.com/p/585308509
4. 跑自己的录制的 bag 包：
roslaunch vins_estimator my.launch
roslaunch vins_estimator my_vins_rviz.launch
rosbag play /root/LET-NET/datasets/aligned_ZHZ_outdoor_jd_front.bag
mv VINS-Mono /root/VINS-Mono-orin
跑 LET-Net
roslaunch vins_estimator my_uma_vi.launch
roslaunch vins_estimator my_vins_rviz.launch
./devel/lib/feature_tracker/my_feature_tracker_illustration
rosbag play /root/LET-NET/datasets/MH_01_easy.bag
