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)测试运行结果
第二个功能包便可以检测到闭环,其中红色线是闭环校正优化后的轨迹,绿色线是无闭环的轨迹: