VINS-Mono运行小觅相机bag包
程序员文章站
2022-03-07 11:45:00
...
VINS-Mono运行小觅相机bag包
1. 小觅相机SDK安装
可参考官方给定使用说明书: 小觅相机技术说明书
SDK安装主要是为了使用ROS订阅相机图像,IMU数据的话题.
2. VINS-Mono安装
可参考github上的步骤下载安装即可:
cd ~/catkin_vio/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
cd ..
catkin_make
// 可以将下面命令直接添加到 .bashrc 中
source ~/catkin_vio/devel/setup.bash
运行测试Euroc数据集:
roslaunch vins_estimator euroc.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play ~/Datasets/Euroc/MH_02_easy.bag
3. 修改VINS-Mono启动文件
-
创建配置参数文件
在 ~/ws_vio/src/VINS-Mono/config文件下建立一个名为mynteye的文件夹,并新建mynteye_config.yaml文件, 修改如下:
%YAML:1.0
#common parameters
imu_topic: "/mynteye/imu/data_raw"
image_topic: "/mynteye/left/image_color"
output_path: "/home/guofeng/vio/output/"
#camera calibration
model_type: PINHOLE
camera_name: mynteye
image_width: 640
image_height: 480
distortion_parameters:
k1: -0.28706
k2: 0.069023
p1: 0.00036239
p2: 0.00070190
projection_parameters:
fx: 351.41
fy: 351.46
cx: 320.90
cy: 247.90
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 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
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0.999894, -0.0105982, 0.0100214,-0.0106528, -0.999929, 0.0054139, 0.00996336, -0.00552008, -0.999935]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [-0.04777362, -0.00223731, -0.00160071]
#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: 2.1657228523252730e-02 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 2.1923690143745844e-03 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 3.8153871149178200e-04 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.4221215955051228e-05 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.7915 # 长沙 gravity magnitude
#loop closure parameters
loop_closure: 1 # 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/guofeng/vio/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
-
建立启动文件
在 ~/ws_vio/src/VINS-Mono/vins_estimator/launch 下新建一个 mynteye.launch 文件,并修改:
<launch>
<arg name="config_path" default = "$(find feature_tracker)/../config/mynteye/mynteye_config.yaml" />
<arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
<node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
<param name="config_file" type="string" value="$(arg config_path)" />
<param name="vins_folder" type="string" value="$(arg vins_path)" />
</node>
<node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
<param name="config_file" type="string" value="$(arg config_path)" />
<param name="vins_folder" type="string" value="$(arg vins_path)" />
</node>
<node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">
<param name="config_file" type="string" value="$(arg config_path)" />
<param name="visualization_shift_x" type="int" value="0" />
<param name="visualization_shift_y" type="int" value="0" />
<param name="skip_cnt" type="int" value="0" />
<param name="skip_dis" type="double" value="0" />
</node>
<node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find vins_estimator)/../config/vins_rviz_config.rviz" />
</launch>
4. VINS-Mono运行小觅bag包
roslaunch vins_estimator mynteye.launch
rosbag play ~/Datasets/mynteye/mynteye_01.bag
(1) 测试运行结果
运行中有一些问题,没有检测到闭环(其实本应是有闭环的,但是这里却没有检测到):
(2)测试运行结果
第二个功能包便可以检测到闭环,其中红色线是闭环校正优化后的轨迹,绿色线是无闭环的轨迹: