VINS（二）Feature Detection and Tracking

系統入口是feature_tracker_node.cpp文件中的main函數

1. 首先建立feature_tracker節點，從配置文件中讀取信息（parameters.cpp），包括：

• ROS中發佈訂閱的話題名稱；
• 圖像尺寸；
• 特徵跟蹤參數；
• 是否須要加上魚眼mask來去除邊緣噪點；

%YAML:1.0

#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
   k1: -2.917e-01
   k2: 8.228e-02
   p1: 5.333e-05
   p2: -1.578e-04
projection_parameters:
   fx: 4.616e+02
   fy: 4.603e+02
   cx: 3.630e+02
   cy: 2.481e+02

# 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. 
ex_calib_result_path: "/config/euroc/ex_calib_result.yaml"  # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.                        
#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, -1, 0, 
           1, 0, 0, 
           0, 0, 1]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [-0.02,-0.06, 0.01]

#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: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.02         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.0002         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 2.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.81007     # gravity magnitude


#loop closure parameters
loop_closure: 1   #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;
                     #also give the camera calibration file same as feature_tracker node
pattern_file: "/support_files/brief_pattern.yml"
voc_file: "/support_files/brief_k10L6.bin"
min_loop_num: 25

該config.yaml文件中的其餘參數在vins_estimator_node中被讀取，屬於融合算法的參數。

• 優化參數（最大求解時間以保證明時性，不卡頓；最大迭代次數，避免冗餘計算；視差閾值，用於選取sliding window中的關鍵幀）；
• imu參數，包括加速度計陀螺儀的測量噪聲標準差、零偏隨機遊走噪聲標準差，重力值（imu放火星上須要改變）；
• imu和camera之間的外參R，t；可選（0）已知精確的外參，運行中無需改變,（1）已知外參初值，運行中優化，（2）什麼都不知道，在線初始化中標定
• 閉環參數，包括brief描述子的pattern文件（前端視覺使用光流跟蹤，不須要計算描述子），針對場景訓練好的DBow二進制字典文件；

2. 監聽IMAGE_TOPIC, 有圖像信息發佈到IMAGE_TOPIC上時，執行回調函數：

ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

 

 

3. img_callback()

前端視覺的算法基本在這個回調函數中，步驟爲：

　　1. 頻率控制，保證每秒鐘處理的image很少於FREQ；

　　2. 對於單目：

　　　　1）. readImage;

　　　　2）. showUndistortion(可選);

　　　　3）. 將特徵點矯正（相機模型camodocal）後歸一化平面的3D點（此時沒有尺度信息，3D點p.z=1），像素2D點，以及特徵的id，封裝成ros的sensor_msgs::PointCloud消息類型; 

　　3. 將處理完的圖像信息用PointCloud和Image的消息類型，發佈到"feature"和"feature_img"的topic：

pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);

 

4. 包含的視覺算法：

1. CLAHE(Contrast Limited Adaptive Histogram Equalization)

cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));

2. Optical Flow(光流追蹤)

cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

3. 根據匹配點計算Fundamental Matrix， 而後用Ransac剔除不符合Fundamental Matrix的外點

cv::findFundamentalMat(un_prev_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);

4. 特徵點檢測：goodFeaturesToTrack， 使用Shi-Tomasi的改進版Harris corner

cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);

 特徵點之間保證了最小距離30個像素，跟蹤成功的特徵點須要通過rotation-compensated旋轉補償的視差計算，視差在30個像素以上的特徵點纔會去參與三角化和後續的優化，保證了全部的特徵點質量都是比較高的，同時下降了計算量。