欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

VINS-Mono运行小觅相机bag包

程序员文章站 2022-03-07 11:45:00
...

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) 测试运行结果

运行中有一些问题,没有检测到闭环(其实本应是有闭环的,但是这里却没有检测到):
VINS-Mono运行小觅相机bag包(2)测试运行结果
第二个功能包便可以检测到闭环,其中红色线是闭环校正优化后的轨迹,绿色线是无闭环的轨迹:
VINS-Mono运行小觅相机bag包

相关标签: 手写VIO slam